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

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

#print(map_trans(7000, 1000, 14000, -180, 180))

with open(sys.argv[1], "rb") as data:
    rawdata = data.read();
    entries = rawdata[:1024]
    header  = rawdata[1024:2048]

    varnames = []
    fmts = []
    fmt = '>'

    translator = {
        'L': np.int32,
        'l': np.uint32,
        'h': np.int16,
        'H': np.uint16,
        'c': np.byte,
        '?': np.bool_
    }

    dtype = []

    for i in range(64):
        if chr(entries[i*16]) != ' ':
            fmt += chr(entries[i*16])
            dtype.append((entries[i*16+1:i*16+16].decode('utf-8').strip(), translator[chr(entries[i*16])]))

    slen = struct.calcsize(fmt)
    blob    = rawdata[2048:int((len(rawdata)-2048)/slen)*slen+2048]
    print(fmt, len(blob), slen, len(blob)/slen)

    print(dtype)

    """log = np.dtype({"names": varnames,
                    "formats": fmts})

    slen = struct.calcsize(fmt)
    for i in range(int(len(blob)/slen)):
        line = struct.unpack_from(fmt, blob, offset=i*slen)

        log.append(line)"""

    log = np.frombuffer(blob, dtype=np.dtype(dtype))

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

t = log['TIME']

print('t:', t.dtype, t)

fig, axs = plt.subplots(5, 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)

for i, corner in enumerate(['LF', 'LA', 'RF', 'RA']):
    ax = axs[i+1]
    ax.plot(t, map_trans(log[corner+'_FILT_ANG'], 1638, 14746, -180, +180), 'b-')
    ax.plot(t, map_trans(log[corner+'_RAW_ANG'],  1638, 14746, -180, +180), 'c.')
    ax.plot(t, map_trans(log[corner+'_SETP'],     1638, 14746, -180, +180), 'k--')

    status_min = -270
    status_max = -200

    ax.fill_between(t, status_min, status_max, where= log[corner+'_STATUS'] == ord('+'), facecolor='cyan', alpha=1)
    ax.fill_between(t, status_min, status_max, where= log[corner+'_STATUS'] == ord('-'), facecolor='yellow', alpha=1)
    ax.fill_between(t, status_min, status_max, where= log[corner+'_STATUS'] == ord('L'), facecolor='green', alpha=1)
    ax.fill_between(t, status_min, status_max, where= log[corner+'_STATUS'] == ord('S'), facecolor='grey', alpha=1)
    ax.fill_between(t, status_min, status_max, where= log[corner+'_STATUS'] == ord('E'), facecolor='red', alpha=1)
    ax.set_ylim(status_min, 180)
    ax.set_yticks(np.arange(-270, 181, 90))
    ax.set_yticks(np.arange(-270, 181, 30), minor=True)
    ax.set_xlabel(corner+' Actuator (Black Dash: Target, Blue Solid: Filtered Rdg, Cyan Dot: Raw Rdg)')
    ax.grid(which='major', alpha=0.5)
    ax.grid(which='minor', alpha=0.2)

plt.show()