import numpy
from matplotlib.pyplot import *
import matplotlib.animation as animation
from ArduinoMPU6050AccelGyro import *


ard = Arduino("COM3")
accel_range = ard.ACCEL_FS_2
g_max = 2
#g_max = 32000
gyro_range = ard.GYRO_FS_500
r_max = 500
dlpf_mode = ard.DLPF_BW_21
echelle_accel = ard.echelle_accel(accel_range)
#echelle_accel = 1
echelle_gyro = ard.echelle_gyro(gyro_range)
nblocs = 1000
N = nblocs*ard.TAILLE_BLOC_ACCEL_GYRO
rate_div = 9
fechant = ard.fechant(rate_div,dlpf_mode)
print("fe = %f"%fechant)
delai = 0.2
longueur_fenetre = 1000
temps = numpy.arange(longueur_fenetre)
a = numpy.zeros(longueur_fenetre)
fig,ax = subplots(2,1)
line0, = ax[0].plot(temps,a,"r",label="ax")
line1, = ax[0].plot(temps,a,"g",label="ay")
line2, = ax[0].plot(temps,a,"b",label="az")
line3, = ax[1].plot(temps,a,"r",label="rx")
line4, = ax[1].plot(temps,a,"g",label="ry")
line5, = ax[1].plot(temps,a,"b",label="rz")
ax[0].grid()
ax[0].set_xlabel("t (s)")
ax[0].set_ylabel("g")
ax[0].axis([0,longueur_fenetre,-g_max,g_max])
ax[0].legend(loc="lower left")
ax[1].grid()
ax[1].set_xlabel("t (s)")
ax[1].set_ylabel("deg/sec")
ax[1].axis([0,longueur_fenetre,-r_max,r_max])
ax[1].legend(loc="lower left")

acquisition = AcquisitionThread(ard,accel_range,gyro_range,rate_div,dlpf_mode,nblocs)
acquisition.start()


def animate(i):
    global line0,line1,line2, line3,line4,line5,acquisition,temps,ax
    (temps,data,tmax) = acquisition.paquet(longueur_fenetre)
    if isinstance(data,int)==False:
        line0.set_ydata(data[0]/echelle_accel)
        line1.set_ydata(data[1]/echelle_accel)
        line2.set_ydata((data[2]+2109.5)/echelle_accel)
        line0.set_xdata(temps)
        line1.set_xdata(temps)
        line2.set_xdata(temps)
        ax[0].axis([0,tmax,-g_max,g_max])
        line3.set_ydata(data[3]/echelle_gyro)
        line4.set_ydata(data[4]/echelle_gyro)
        line5.set_ydata(data[5]/echelle_gyro)
        line3.set_xdata(temps)
        line4.set_xdata(temps)
        line5.set_xdata(temps)
        ax[1].axis([0,tmax,-r_max,r_max])
        

ani = animation.FuncAnimation(fig,animate,100,interval=delai*1000)
show()
acquisition.stop()
ard.close()

data = acquisition.data
data[0] = data[0] / echelle_accel
data[1] = data[1] / echelle_accel
data[2] = (data[2]+2109.5)/ echelle_accel
data[3] = data[3] / echelle_gyro
data[4] = data[4] / echelle_gyro
data[5] = data[5] / echelle_gyro
numpy.savetxt("accel-gyro-1.txt",data.T,delimiter="\t")