#! /usr/bin/env python

import serial
import numpy
import Gnuplot, Gnuplot.funcutils

def plotter():
    g = Gnuplot.Gnuplot(debug=0)
    g('set style data line')
    g('set yrange[-500:50000]')
    data = [];
    magic = 1/0.8044
    freq = numpy.linspace(0,200*magic,128)
    freq = numpy.array(freq)
    ser = serial.Serial('/dev/ttyACM0', 19200)

    while(1):
        temp = ser.readline()
        #print "read serial line"
        if temp.strip() == "START":
            #print "Found START"
            data = []
            rawInput = []
            
	    
            temp = ser.readline()
            while temp.strip() != "END":
                rawInput.append(temp)
                temp = ser.readline()

            for line in rawInput:
                temp = line.split()
                temp = map(int,temp)
                data.append(temp);
                

            #print "Data captured"
            b = numpy.array(data).transpose();
            t = b[0].tolist()
            x = b[1].tolist()
            y = b[2].tolist()
            z = b[3].tolist()
            #print "Doing FFT..."
            Fx = abs(numpy.fft.rfft(x))
	    Fy = abs(numpy.fft.rfft(y))
	    Fz = abs(numpy.fft.rfft(z))
            Fx=Fx[1:]            #remove DC component
	    Fy=Fy[1:]            #remove DC component
	    Fz=Fz[1:]            #remove DC component
	    ix = Fx.argmax()
	    iy = Fy.argmax()
	    iz = Fz.argmax()
            cx = freq[ix]
	    cy = freq[iy]
	    cz = freq[iz]
	    
	    mx = Fx.max()
	    my = Fy.max()
	    mz = Fz.max()
            vx = Gnuplot.Data([[cx,0],[cx,50000]],with='lines 6')
	    vy = Gnuplot.Data([[cy,0],[cy,50000]],with='lines 6')
	    vz = Gnuplot.Data([[cz,0],[cz,50000]],with='lines 6')
	    #print freq.size
	    #print Fx.size
	    d1 = Gnuplot.Data(freq,Fx,with='lines 1')
	    d2 = Gnuplot.Data(freq,Fy,with='lines 3')
	    d3 = Gnuplot.Data(freq,Fz,with='lines 4')
	    print cx
	    #print ix
	    #print iy
	    #print iz
            #print "Plotting..."
            g.plot(d1)
	    g.replot(d2)
	    g.replot(d3)
            g.replot(vx)
	    g.replot(vy)
	    g.replot(vz)
            
        
        
if __name__ == '__main__':
    plotter()