'''
Final Design Data Parser
Cornell ECE 4760
Aidan Angus (apa45)
Matthew Magaldi (mjm484)
'''
from math import sqrt, cos, sin, pi
from time import clock

#User defined functions
def integrate( vect, time ):
    '''This function returns the time integral of vect'''
    integ = [0]
    integral = 0
    for i in range(1,len(vect)):
        integral += 0.5*(vect[i] + vect[i-1])*(time[i] - time[i-1])   #trapezoidal integral
        integ.append(integral)
    return integ

def fullVel( vx, vy, vz ):
    '''This function combines all three axial velocities into one absolute velocity'''
    vel = []
    for i in range(0, len(vx)):
        vel.append(sqrt(vx[i]**2 + vy[i]**2 + vz[i]**2))
    return vel

#Parse CSV to get data
filename = input('Filename: ')
start = clock() #Used simply for the curious to see this script's run time
ax = []
ay = []
az = []
gx = []
gy = []
gz = []
temp = []
press = []
alt = []
time = []
f = open(filename+'.csv', mode = 'r')
for line in f:
    line = line.replace('\x00', '') #Our PIC code creates a char array of fiex length, but out data doesn't always
    #fill up this length, so extraneous characters of \x00 need to be removed
    values = line.split(',')        #Split the string into an array based on the commas
    if len(values) == 10:
        #Sometimes the PIC writes a blank line of only '\n' into the CSV and we dunno why, so we only consider
        #properly sized lines (10)
        ax.append(float(values[0]))
        ay.append(float(values[1]))
        az.append(float(values[2]))
        gx.append(float(values[3]))
        gy.append(float(values[4]))
        gz.append(float(values[5]))
        temp.append(float(values[6]))
        press.append(float(values[7]))
        alt.append(float(values[8]))
        time.append(int(values[9]))
    
f.close()
rows = len(ax)

#Unit fixing / Sensor Zeroing

base_time = time[0]
time2 = time[:]
for i in range(0,rows):
    ax[i] = (ax[i] - 480) * 9.8 / 95
    ay[i] = (ay[i] - 480) * 9.8 / 95
    az[i] = (az[i] - 495) * 9.8 / 93
    gx[i] = (gx[i] + 0.7985) * pi / 180
    gy[i] = (gy[i] + .07784) * pi / 180
    gz[i] = (gz[i] + 1.3404) * pi / 180
    time[i] = (time[i] - base_time) / 1000

#Calculate gravity-compensated acceleration vectors

gxt = integrate(gx, time)
gyt = integrate(gy, time)
gzt = integrate(gz, time)
g = [ax[0], ay[0], az[0]]
for i in range(0, rows):
    #Subtract out the gravity vector
    ax[i] -= g[0]
    ay[i] -= g[1]
    az[i] -= g[2]
    #Calculate next gravity vector
    try:
        t = time[i+1] - time[i] #time difference
        w = gxt[i+1]    #angle rotated about x axis
        p = gyt[i+1]    #angle rotated about y axis
        k = gzt[i+1]    #angle rotated about z axis
        #Implementation of R3 vector rotation
        g0 = g[0]*cos(p)*cos(k) + g[1]*(cos(w)*sin(k)+sin(w)*sin(p)*cos(k)) + g[2]*\
             (sin(w)*sin(k)-cos(w)*sin(p)*cos(k))
        g1 = g[0]*(-cos(p)*sin(k)) + g[1]*(cos(w)*cos(k)-sin(w)*sin(p)*sin(k)) + g[2]*\
             (sin(w)*cos(k)+cos(w)*sin(p)*sin(k))
        g2 = g[0]*sin(p) + g[1]*(-sin(w)*cos(p)) + g[2]*cos(w)*cos(p)
        g = [g0, g1, g2]    #next gravity vector
    except:
        break

#Calculate drift-corrected total velocity vector

vx = integrate(ax, time)    #x velocity
vy = integrate(ay, time)    #y velocity
vz = integrate(az, time)    #z velocity
vt = fullVel(vx, vy, vz)    #toatl velocity

vt2 = vt[:]                 #keep a copy of the original velocity for later comparison
h = -vt[-1]/time[-1]        #linear correction term

for i in range(0,rows):
    vt[i] += h * time[i]    #linear error correction

#Write new CSV, to be plotted by Excel or MATLAB

f = open(filename+'_linear.csv', mode = 'w')
f.write('Time, Uncorrected Velocity, Velocity, Temperature, Pressure, Altitude\n')
for i in range(0, rows):
    f.write(str(time[i])+', '+str(vt2[i])+', '+str(vt[i])+', '+str(temp[i])+', '+str(press[i])+', '+str(alt[i])+'\n')
    #f.write(str(ax[i])+', '+str(ay[i])+', '+str(az[i])+', '+str(gx[i])+', '+str(gy[i])+', '+str(gz[i])+', '+str(time[i])+'\n')
f.close()

print(clock() - start) #Shows script's run time