%matplotlib inline
import numpy as np
import utils
from matplotlib import rcParams
rcParams.update({'font.size': 14})


columns = np.loadtxt("data/ADIS.csv", delimiter=',', unpack=True)

seqn = columns[0]
timestamp = columns[1]
gyr_x = columns[3]
gyr_y = columns[4]
gyr_z = columns[5]
acc_x = columns[6]
acc_y = columns[7]
acc_z = columns[8]
mag_x = columns[9]
mag_y = columns[10]
mag_z = columns[11]

# normalize time to liftoff, put in seconds
t_0 = 202945201730391 # nanoseconds in FC time frame
timestamp = np.subtract(timestamp, t_0)
timestamp = np.divide(timestamp, 1e9)

# limit to just boost and coast data (don't care after the chutes open
timestamp = timestamp[1700:30000]
gyr_x = gyr_x[1700:30000]
gyr_y = gyr_y[1700:30000]
gyr_z = gyr_z[1700:30000]
acc_x = acc_x[1700:30000]
acc_y = acc_y[1700:30000]
acc_z = acc_z[1700:30000]
mag_x = mag_x[1700:30000]
mag_y = mag_y[1700:30000]
mag_z = mag_z[1700:30000]

fig = utils.Plot(r"Launch 11 Roll Rate", "Time [$s$]", "Rate [${}^0/s$]")
fig.plot(timestamp, gyr_x)
fig.ylim([-40, 425])
fig.show()

fig = utils.Plot(r"Launch 11 Magnetometer", "Time [$s$]", "Magnetometer Values [$T$]")
fig.plot(timestamp, mag_y, label=r"$y$")
fig.plot(timestamp, mag_z, color=utils.blue, label=r"$z$")
fig.legend(loc=3)
fig.show()

# Trick! Get the orgin to be zero by setting the mean to be zero
mag_y = mag_y-mag_y.mean()
mag_z = mag_z-mag_z.mean()

fig = utils.Plot(r"Launch 11 Magnetometer: $[y,z]$ vector", "Time [$s$]", "Magnetometer [$T$]")
fig.plot(mag_y, mag_z)
fig.plot(mag_y[:100], mag_z[:100], color='#00ff00') # mark the begining, look for a small green dot
fig.show()

# use numpy to build [y,z] vector from the two arrays
mag = np.dstack((mag_y, mag_z))[0]

# reference angle, should be a mean, but good enough.
begin = mag[0]

# storage
angles = []     # roll angle
rollup = 0      # every time we make one revolution, add 360 to this
last_angle = 0  # for checking if we've wrapped back to -pi

for v1 in mag:
    
    # compute angle between now and the reference angle
    s = np.cross(v1, begin)
    c = np.dot(v1, begin)
    angle = np.arctan2(s, c)
    
    # if we just wrapped around, add a revolution
    if last_angle > 2 and angle < 0:
        rollup += 360
        
    # store
    angles.append(np.degrees(angle)+rollup)
    last_angle = angle
    
# how many times did we spin?
print "The rocket made", rollup/360, "revolutions during ascent"

fig = utils.Plot(r"Launch 11 Magetometer Derived Roll Angle", "Time [$s$]", "Total Angle [${}^0$]")
fig.plot(timestamp, angles)
fig.ylim([-200,14000])
fig.show()

# differentiate
sample_rate = 819.2
angle_rate = np.diff(angles)
angle_rate = angle_rate*sample_rate  # correct for time units using the sample rate of the sensor

# low pass the hell out of the result
from scipy.signal import firwin, lfilter
freq_cuttoff = 0.5
filter_taps = 4096
window = firwin(numtaps=filter_taps, cutoff=freq_cuttoff, nyq=sample_rate/2)
angle_rate_padded = np.concatenate((angle_rate, np.zeros(filter_taps / 2)))
angle_rate_filtered = lfilter(window, 1.0, angle_rate_padded)[filter_taps / 2:]


fig = utils.Plot(r"Launch 11 Magetometer Derived Roll Rate", "Time [$s$]", "Angle [${}^0/s$]")
fig.plot(timestamp[1:], angle_rate, color='black', alpha=0.1, label="Roll Rate")
fig.plot(timestamp[1:], angle_rate_filtered, alpha=1, lw=1.6, label="Filtered Roll Rate")
fig.plot(timestamp, gyr_x, color=utils.blue, label="Roll Gryo")
fig.ylim([-80, 1000])
fig.legend()
fig.show()