import math
import spidev
import time
import binascii
from numpy import matrix
from numpy import linalg
import numpy as np
import scipy.linalg
from numpy.linalg import inv
from scipy.signal import cont2discrete as c2d

def trapez(qr1, qr2, qpr, qsec, tm, textra):
    s=qr2-qr1;
    sign = np.sign(s);
    global t1, t2, tfin
    
    if (abs(s) > (qpr*qpr/qsec)):
        t1 = (qpr/qsec)
        t2 = (abs(s)/qpr)
        tfin = t2+t1
    else:
        t1 = math.sqrt(abs(s)/qsec)
        tfin = 2.0 * t1
        t2=0

    position = []
    velocity = []
    acceleration = []
    tiempo = []

    position.append(qr1)
    velocity.append(0)
    acceleration.append(0)

    #print t1
    #print t2
    #print tfin
    for time in range(0, int((tfin+textra)/tm)):
        #print time
        #print t1
        tiempo.append(time*tm)
        
        if (time < t1/tm):
            #print ('E1')
            acceleration.append(sign*qsec)
            velocity.append(velocity[time] + acceleration[time+1]*tm)
            position.append(position[time] + velocity[time+1]*tm + 0.5*acceleration[time+1]*tm*tm)    

        elif ((time >= t1/tm)&(time < t2/tm)):
            #print ('E2')
            acceleration.append(0)
            velocity.append(sign*qpr)
            position.append(position[time] + velocity[time+1]*tm)

        elif ((time >=t2/tm)&(time <=tfin/tm)):
            #print ('E3')
            acceleration.append(-sign*qsec)
            velocity.append(velocity[time] + acceleration[time+1]*tm)
            position.append(position[time] + velocity[time+1]*tm + 0.5*acceleration[time+1]*tm*tm)
        else:
            #print ('E4')
            acceleration.append(0)
            velocity.append(0)
            position.append(qr2)

    return (position, velocity, acceleration, tiempo)	

textra = 0.2
(qref1,vref1,aref1,t1)=trapez(0, 1.04, 1.8, 1.95, 0.01, textra);
(qref2,vref2,aref2,t2)=trapez(0, 0.35, 1, 1, 0.01, 0.48);

Ac = matrix([[0,0,0,0,1,0,0,0],[0,0,0,0,0,1,0,0],[0,0,0,0,0,0,1,0],[0,0,0,0,0,0,0,1],[-6051.39,15026.8,6075.95,-15038.0,0,0,0,0],[15157.9,-61537.6,-15189.9,61594.9,0,0,0,0],[7500.0,0,-7500.0,0,0,0,0,0],[151.285,23624.3,-151.899,-23624.1,0,0,0,0]])
Bc = matrix([[0,0],[0,0],[0,0],[0,0],[0,-0.0151899],[0,0.0379747],[0.75,0],[0,2.40038]])
Cc = np.diag([1,1,1,1,1,1,1,1])
Dc = np.diag([0,0,0,0,0,0,0,0])

dt_requested = 0.01

A, B, C, D, dt = c2d((Ac, Bc, Cc, Dc), dt_requested, method='zoh')

Q = np.diag([90000,90000,90000,90000,90000,90000,90000,90000])
P = np.diag([90000,90000,90000,90000,90000,90000,90000,90000])
R = matrix([[0.0001 ,0],[0 ,0.001]])

x0 = matrix([[qref1[0],qref2[0],qref1[0],qref2[0],vref1[0],vref2[0],vref1[0],vref2[0]]])

k=len(qref1)

S=C.T*P*C
v = np.zeros((8, k))
x = np.zeros((8, k))
u = np.zeros((k, 2, 2))
K = np.zeros((k-1,2,8))
Kv = np.zeros((k-1,2,8))

N=k-1
#r=[1; 0]
xd = matrix([[qref1[N],qref2[N],qref1[N],qref2[N],vref1[N],vref2[N],vref1[N],vref2[N]]]).T
v[:,[N]]=C.T*P*xd
x[:,[0]] = x0.T

for k in range(N-1, 0, -1):
    K[k,:,:] = np.dot(inv(R+np.dot(np.dot(B.T,S),B)),np.dot(np.dot(B.T,S),A))
    Kv[k,:,:] = np.dot(inv(R+np.dot(np.dot(B.T,S),B)),B.T)
    S = np.dot(np.dot(A.T,S),(A-np.dot(B,K[k,:,:])))+ np.dot(np.dot(C.T,Q),C)
    v[:,[k]] = np.dot((A-np.dot(B,K[k,:,:])).T,v[:,[k+1]])+np.dot(np.dot(C,Q),(matrix([[qref1[k],qref2[k],qref1[k],qref2[k],vref1[k],vref2[k],vref1[k],vref2[k]]])).T)

t = range(0, N)

spi = spidev.SpiDev() 
spi.open(0, 1) # open spi port 0, device (CS) 1
spi.max_speed_hz = 3500000
spi.bits_per_word = 8
spi.mode = 1

def convert(X):
    x = (int)(X)
    c = (x >> 8) & 0xff
    f = x & 0xff
    return c, f
#end def

def convertFloatToFour8bit(x):
    x = int(round(x*1000))      # Para considerar 3 decimales
    a = (x >> 24) & 0x000000ff
    b = (x >> 16) & 0x000000ff
    c = (x >> 8) & 0x000000ff
    d = (x & 0x000000ff)
    return a,b,c,d
#end def

def BytesToHex(Bytes):
    return ''.join(["0x%02X " % x for x in Bytes]).strip()
#end def

Data = []
print(N-1)
data_size1, data_size2 = convert(N-1)
spi.xfer([data_size1, data_size2])

for i in range(0, N-1):
    for j in range(0, 8):      
        data1, data2, data3, data4 =  convertFloatToFour8bit(K[i,0,j])
        data5, data6, data7, data8 =  convertFloatToFour8bit(K[i,1,j])
        data9, data10, data11, data12 =  convertFloatToFour8bit(Kv[i,0,j])
        data13, data14, data15, data16  =  convertFloatToFour8bit(Kv[i,1,j])
        data17, data18, data19, data20  =  convertFloatToFour8bit(v[j,i])
        Data.append(data1)
        Data.append(data2)
        Data.append(data3)
        Data.append(data4)
        Data.append(data5)
        Data.append(data6)
        Data.append(data7)
        Data.append(data8)
        Data.append(data9)
        Data.append(data10)
        Data.append(data11)
        Data.append(data12)
        Data.append(data13)
        Data.append(data14)
        Data.append(data15)
        Data.append(data16)
        Data.append(data17)
        Data.append(data18)
        Data.append(data19)
        Data.append(data20)
        

spi_tx = (int)(len(Data)/4000)

if ((len(Data)/4000) > spi_tx):
    spi_tx = spi_tx + 1

print (spi_tx)

for i in range(0, spi_tx):
    if (4000+4000*i) < len(Data):
        spi.xfer(Data[(4000*i):(4000+4000*i)])
    else:
        spi.xfer(Data[(4000*i):len(Data)])
    time.sleep(0.5)





