#!/usr/bin/env python
# coding: utf-8

# # IMU Data
# 
# blah ...

# In[1]:


import cv2
import numpy as np
from collections import namedtuple
import time
import pickle
from math import pi
from ins_nav import AHRS
from matplotlib import pyplot as plt
from squaternion import quat2euler, Quaternion


# ### Get Data

# In[53]:


Data_ts = namedtuple('Data_ts', 'data timestamp')
data = pickle.load(open("data/imu-still.pickle", "rb"))


# # Still

# In[54]:


accel = [x[0][0] for x in data['imu']]
mags = [x[0][1] for x in data['imu']]
gyros = [x[0][2] for x in data['imu']]
imutime = [x[1] for x in data['imu']]


# In[55]:


def std(data):
    x = [x[0] for x in data]
    y = [x[1] for x in data]
    z = [x[2] for x in data]
    plt.figure()
    plt.plot(data)
    plt.grid(True)
    print('Mean:')
    print(' x {}'.format(np.mean(x)))
    print(' y {}'.format(np.mean(y)))
    print(' z {}'.format(np.mean(z)))
    print('Standard Dev:')
    print(' x {}'.format(np.std(x)))
    print(' y {}'.format(np.std(y)))
    print(' z {}'.format(np.std(z)))


# In[56]:


print('Accel -----------------')
std(accel)
print('Mags ------------------')
std(mags)
print('Gyros -----------------')
std(gyros)


# In[45]:


hz=11.5740740e-6 # Hz, earth rotation
hz*2*pi/pi*180


# In[11]:


ahrs = AHRS()


# In[18]:


save = []
ot = 0
q = Quaternion(1,0,0,0)

for a,m,g,t in zip(accel, mags, gyros,imutime):
    #q = ahrs.updateAGM(a,m,g,1,t-ot)
    q = ahrs.updateAG(a,g,.1,t-ot)
    o = quat2euler(*q, degrees=True)
    save.append(o)
    ot = t


# In[19]:


plt.plot(imutime, [x[0] for x in save], label='x')
plt.plot(imutime, [x[1] for x in save], label='y')
plt.plot(imutime, [x[2] for x in save], label='z')
plt.legend()
plt.grid(True)
plt.show()


# # Move

# In[20]:


data = pickle.load(open("data/imu-move.pickle", "rb"))


# In[21]:


accel = [x[0][0] for x in data['imu']]
mags = [x[0][1] for x in data['imu']]
gyros = [x[0][2] for x in data['imu']]
imutime = [x[1] for x in data['imu']]


# In[36]:


save = []
ot = 0
q = Quaternion(1,0,0,0)

for a,m,g,t in zip(accel, mags, gyros,imutime):
    #q = ahrs.updateAGM(a,m,g,.01,t-ot)
    q = ahrs.updateAG(a,g,.1,t-ot)
    o = quat2euler(*q, degrees=True)
    save.append(o)
    ot = t


# In[37]:


plt.plot(imutime, [x[0] for x in save], label='x')
plt.plot(imutime, [x[1] for x in save], label='y')
plt.plot(imutime, [x[2] for x in save], label='z')
plt.legend()
plt.grid(True)
plt.show()


# In[ ]: