import numpy as np
import sys
import matplotlib.pyplot as plt

def map_trans(x, in_min, in_max, out_min, out_max):
  return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min


with open(sys.argv[1], "r") as data:
    while True:
        line = data.readline()
        if not line.startswith('#'): break

    header = [e for e in line.strip().split(',') if e]
    print(header)

    log = np.genfromtxt(data, names=header, dtype=None, delimiter=',')

print(log)
dt = np.ediff1d(log['t'])
dt = np.insert(dt, 0, 0)

t = log['t']/1000

fig, axs = plt.subplots(4, 1, layout='constrained', sharex=True)
axs[0].plot(t, dt, 'k.')
#axs[0].set_xlim(0, 2)
axs[0].set_xlabel('Time (s)')
axs[0].set_ylabel('Iteration Time (ms)')
axs[0].grid(True)
axs[0].set_xlim(min(t), max(t))
axs[0].set_ylim(0, max(dt)*1.1)

axs[1].plot(t, log['a_filtered_f']/10, 'k-')
axs[1].plot(t, log['sp_f']/10, 'b--')
axs[1].fill_between(t, -180, +180, where= log['o_f'] < 0, facecolor='#e22030', alpha=0.5)
axs[1].fill_between(t, -180, +180, where= log['o_f'] > 0, facecolor='#50d21d', alpha=0.5)
if 'status_f' in header:
	axs[1].fill_between(t, -180, +180, where= log['status_f'] == 'F', facecolor='#d2c81c', alpha=0.5)
axs[1].set_ylim(-180, +180)
axs[1].set_xlabel('Front Actuator (Blue Dash: Target, Black Solid: Filtered Sensor Reading)')
axs[1].grid(True)


axs[2].plot(t, log['a_filtered_r']/10, 'k-')
axs[2].plot(t, log['sp_r']/10, 'b--')
axs[2].fill_between(t, -180, +180, where= log['o_r'] < 0, facecolor='#e22030', alpha=0.5)
axs[2].fill_between(t, -180, +180, where= log['o_r'] > 0, facecolor='#50d21d', alpha=0.5)
if 'status_r' in header:
	axs[2].fill_between(t, -180, +180, where= log['status_r'] == 'F', facecolor='#d2c81c', alpha=0.5)
axs[2].set_ylim(-180, +180)
axs[2].set_xlabel('Rear Actuator (Blue Dash: Target, Black Solid: Filtered Sensor Reading)')
axs[2].grid(True)


axs[3].plot(t, map_trans(log['a_raw_r'], 410, 3686, -180, +180), 'g-')
axs[3].plot(t, map_trans(log['a_raw_f'], 410, 3686, -180, +180), 'b-')
axs[3].set_ylim(-270, 180)
axs[3].set_xlabel('Raw Sensor Readings (Green: Rear, Blue: Front)')
axs[3].grid(True)

plt.show()