# -*- coding: utf-8 -*-
"""
Created on Tue Apr 12 10:53:30 2022

@author: Laurent
"""

import matplotlib
import matplotlib.pyplot as plt

import numpy as np

import csv


dt = np.dtype([ ('frame_id',    np.uint8 ),     \
                ('trim_com',    np.uint8),      \
                ('trim_fl',     np.uint8),      \
                ('trim_fr',     np.uint8),      \
                ('trim_rl',     np.uint8),      \
                ('trim_rr',     np.uint8),      \
                ('alt_p',       np.uint8),      \
                ('alt_i',       np.uint8),      \
                ('alt_d',       np.uint8),      \
                ('pitch_p',     np.uint8),      \
                ('pitch_i',     np.uint8),      \
                ('pitch_d',     np.uint8),      \
                ('roll_p',      np.uint8),      \
                ('roll_i',      np.uint8),      \
                ('roll_d',      np.uint8),      \
                ('alt_sp',      np.uint8),      \
                ('alt_bal',     np.uint8),      \
                ('packet_id',   np.uint16),     \
                ('rc1',         np.uint16),     \
                ('rc2',         np.uint16),     \
                ('pitch',       np.float32),    \
                ('roll',        np.float32),    \
                ('yaw',         np.float32),    \
                ('lat',         np.float32),    \
                ('long',        np.float32),    \
                ('vel',         np.float32),    \
                ('fl',          np.int32),      \
                ('fr',          np.int32),      \
                ('rl',          np.int32),      \
                ('rr',          np.int32),      \
                ('altl',        np.uint16),     \
                ('altr',        np.uint16),     \
                ('vbat',        np.uint16),     \
                ('status',      np.uint8),      \
                ('fr4',         np.uint8),      \
                ('fr5',         np.uint8),      \
                ('fr6',         np.uint8),      \
                ('fr7',         np.uint8),      \
                ('fr8',         np.uint8),      \
                ('fr9',         np.uint8),      \
                ('fr10',        np.uint8),      \
                ('crc',         np.uint16),     \
                ('eof',         np.uint8)  ])
               
data = np.fromfile('LOG_200723_120259.DAT', dtype=dt)


header = [      'frame_id', \
                'trim_com', 'trim_fl', 'trim_fr', 'trim_rl', 'trim_rr', \
                'alt_p', 'alt_i', 'alt_d', \
                'pitch_p', 'pitch_i', 'pitch_d', \
                'roll_p', 'roll_i', 'roll_d', \
                'alt_sp', 'alt_bal', \
                'packet_id', \
                'rc1', 'rc2', \
                'pitch', 'roll', 'yaw', \
                'lat', 'long', 'vel', \
                'fl', 'fr', 'rl', 'rr', \
                'altl', 'altr', \
                'vbat', \
                'status', \
                'fr4', 'fr5', 'fr6', 'fr7', 'fr8', 'fr9', 'fr10', \
                'crc', \
                'eof'       ]


with open('export.csv', 'w', encoding='UTF8', newline='') as f:
    writer = csv.writer(f)

    # write the header
    writer.writerow(header)

    # write multiple rows
    writer.writerows(data)


fig = plt.figure(1)
fig.clf

# RC channels (steering, throttle)
ax1 = plt.subplot(411)

c10 = plt.plot(data['rc1'])
c11 = plt.plot(data['rc2'])

plt.setp(c10, linewidth = 1.2, linestyle = '-', color = 'tab:orange')
plt.setp(c11, linewidth = 1.2, linestyle = '-', color = 'tab:purple')

plt.legend(['Steering', 'Throttle'])
plt.ylabel('RC')


# IMU (pitch/roll)
ax2 = plt.subplot(412, sharex = ax1)

c20 = plt.plot(data['pitch'])
c21 = plt.plot(data['roll'])

plt.setp(c20, linewidth = 1.2, linestyle = '-', color = 'tab:olive')
plt.setp(c21, linewidth = 1.2, linestyle = '-', color = 'tab:green')

plt.legend(['Pitch', 'Roll'])
plt.ylabel('IMU')


# Altitude (left/right)
ax3 = plt.subplot(413, sharex = ax1)

c30 = plt.plot(data['altl'])
c31 = plt.plot(data['altr'])

plt.setp(c30, linewidth = 1.2, linestyle = '-', color = 'tab:blue')
plt.setp(c31, linewidth = 1.2, linestyle = '-', color = 'tab:cyan')

plt.legend(['Left', 'Right'])
plt.ylabel('Altitude')


# Foil position (fl, fr, rl, rr)

ax4 = plt.subplot(414, sharex = ax1)

c40 = plt.plot(data['fl'])
c41 = plt.plot(data['fr'])
c42 = plt.plot(data['rl'])
c43 = plt.plot(data['rr'])

plt.setp(c40, linewidth = 1.2, linestyle = '-', color = 'tab:orange')
plt.setp(c41, linewidth = 1.2, linestyle = '-', color = 'tab:red')
plt.setp(c42, linewidth = 1.2, linestyle = '-', color = 'tab:green')
plt.setp(c43, linewidth = 1.2, linestyle = '-', color = 'tab:blue')

plt.legend(['fl', 'fr', 'rl', 'rr'])
plt.ylabel('Foils')


plt.subplots_adjust(hspace=0.2)
plt.grid(True)
plt.show()