from matplotlib.pyplot import *
import numpy
import math
import cmath
import numpy.fft
import scipy.signal
import StringIO
            

s = open('CIMG5043.txt').read().replace(',','.')
data = numpy.loadtxt(StringIO.StringIO(s),skiprows=2,unpack=True)
debut = range(5)
t=numpy.delete(data[0],debut)
x=numpy.delete(data[1],debut)
y=numpy.delete(data[2],debut)
figure(figsize=(6,6))
plot(x,y)
xlabel("x")
ylabel("y")
grid()
                

angle = numpy.angle(-x+1j*y)

figure()
plot(t,angle*180.0/math.pi)
xlabel("t (ms)")
ylabel("angle (deg)")
grid()
                 

figure()
plot(t,angle*180.0/math.pi)
xlabel("t (ms)")
ylabel("angle (deg)")
grid()
axis([6e4,7e4,-10,10])
                 

fe=120.0
te=1.0/fe
N=y.size
T=1.0/fe*N
f = numpy.arange(angle.size)*1.0/T
spectre = numpy.abs(numpy.fft.fft(angle))

figure()
plot(f,spectre,'-o')
xlabel('f (Hz)')
ylabel('A')
grid()
axis([0,2,0,spectre.max()])
                

N1 = int(N/2)
spectre1 = numpy.abs(numpy.fft.fft(angle[0:N1]))
f1 = numpy.arange(spectre1.size)*1.0/(N1*te)
spectre2 = numpy.abs(numpy.fft.fft(angle[N1:N]))
f2 = numpy.arange(spectre2.size)*1.0/((N-N1)*te)

figure()
plot(f1,spectre1,'-o',label='[0,T/2]')
plot(f2,spectre2,'-o',label='[T/2,T]')
xlabel('f (Hz)')
ylabel('A')
grid()
legend(loc='upper right')
axis([0,2,0,spectre.max()])
                 

a=[te]
b=[1,-1]
omega = scipy.signal.lfilter(b,a,angle)
omega[0] = omega[1]

figure()
plot(t,omega)
xlabel("t (ms)")
ylabel("w (rad/s)")
grid()
                

figure()
plot(t,omega)
xlabel("t (ms)")
ylabel("w (rad/s)")
axis([6e4,6.2e4,-2,2])
grid()
                

P=30
fc=5.0/120
h = scipy.signal.firwin(numtaps=2*P+1,cutoff=[fc],nyq=0.5,window='hamming')
angle_filtre = scipy.signal.convolve(angle,h,mode='same')
figure()
plot(t,angle*180.0/math.pi,'b')
plot(t,angle_filtre*180.0/math.pi,'r')
xlabel("t (ms)")
ylabel("angle (deg)")
grid()
                      

figure()
plot(t,angle*180.0/math.pi,'b')
plot(t,angle_filtre*180.0/math.pi,'r')
xlabel("t (ms)")
ylabel("angle (deg)")
grid()
axis([6e4,7e4,-10,10])
                      

a=[te]
b=[1,-1]
omega_filtre = scipy.signal.lfilter(b,a,angle_filtre)

figure()
plot(t,omega_filtre)
xlabel("t (ms)")
ylabel("w (rad/s)")
axis([0,8e4,-8,8])
grid()
                      

figure()
debut=P
plot(angle_filtre[debut:N],omega_filtre[debut:N])
xlabel("angle (rad)")
ylabel("omega (rad/s)")
grid()