##TP3- traitement num   #corrigé 
from pylab import *
import random as rd
import numpy as np
import os
from matplotlib import *

os.chdir('P:/TP3 - Traitement num et tennis 2018')

fic=open("mesure_accelero.txt","r")
tab=[]
for ligne in fic :
    L=ligne.strip().split(',')
    tab.append(L)
fic.close()

tab2=np.zeros([len(tab)-1,len(tab[0])])
for i in range(1,len(tab)):
    for j in range(len(tab[i])):
        tab2[i-1][j]=float(tab[i][j])
tab=tab2 
T=tab[:,0]
X=tab[:,1]
Y=tab[:,2]
Z=tab[:,3]

plot(T,X,'b')
plot(T,Y,'r')
plot(T,Z,'g')
title('accceleration en g')

# on enlève la première ligne car le premier pas de temps a un soucis
##Q1
Ax=tab[:,1]/25.5;   # 1g=255 inc
Ay=tab[:,2]/25.5;
Az=tab[:,3]/25.5;

##Q2  calcul du vecteur temps
t=tab[:,0]*0;
for i in range (1,len(tab)):
    t[i]=t[i-1]+tab[i,0]

t=t/1e6; # retour en secondes

clf()
plot(t,tab[:,1]) # visualiser Ax
clf()
plot(t,tab[:,0]) # visualiser les pas de temps

clf()
plot(t,Ax,"b");
plot(t,Ay,"r");
plot(t,Az,"g");


#Etalonnage sur les 3 premières secondes
ni=1500
nf=9000
Ax2=np.zeros(ni)
Ay2=np.zeros(ni)
Az2=np.zeros(ni)
for i in range (len(Ax)):
    if t[i]<t[ni]:
        Ax2[i]=Ax[i]
        Ay2[i]=Ay[i]
        Az2[i]=Az[i]

clf()
plot(t,Ax2,"k")
        
EtalX=np.sum(Ax2)/1500 #1.140185
EtalY=np.sum(Ay2)/1500 #0.3724575
EtalZ=np.sum(Az2)/1500 #8.6746197

Axe=Ax-EtalX;
Aye=Ay-EtalY;
Aze=Az-EtalZ;


## partie filtrage
def filtreB(Ue,Te,tau):
    H0=1
    Uf=[Ue[0]]      #initialisation à [0]
    for i in range(1,len(Ue)):
        Uf.append(Uf[i-1] + Te/tau*(H0*Ue[i-1]-Uf[i-1]))
    return Uf
Te=0.0005
Df0=filtreB(X2,Te,0.05)

plot(T,Df0,'r')
show()

##part integration
def integration(t,f):
    y=0*f;
    for i in range (1, len(t)):
        y[i]=y[i-1]+f[i-1]*(t[i]-t[i-1]);
    return y

Vx=integration(t,Ax);
Vxe=integration(t,Axe);
clf()
plot(t,Vx)
plot(t,Vxe)

Vye=integration(t,Aye);
Vze=integration(t,Aze);

##part3
Pxe=integration(t,Vxe);
Pye=integration(t,Vye);
Pze=integration(t,Vze);

clf()
plot(t,Pxe,"b")
plot(t,Pye,"r")
plot(t,Pze,"g")
show()

clf()
plot(Pxe[ni:nf],Pye[ni:nf])

clf()
plot(Pxe[1300:2300],Pye[1300:2300])
show()