# evaluating https://github.com/RafaelReyesCarmona/EMA

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(2, 1, layout='constrained', sharex=True)

a_f =  map_trans(log['a_raw_f'], 410, 3686, -180, +180)

#axs[0].plot(t, map_trans(log['a_raw_r'], 410, 3686, -180, +180), 'g-')
axs[0].plot(t, a_f, 'k.')
axs[1].plot(t, log['sp_f']/10, 'b--')
#axs[0].set_ylim(-270, 180)
axs[0].set_xlabel('Raw Sensor Readings (Green: Rear, Blue: Front)')
axs[0].grid(True)

# simulate a Exponential Moving Average Filter
amounts = [1,2,3,4,5,6]
for amt in amounts:
	avg = [0]
	for i in range(len(a_f)-1):
		avg.append(avg[-1] + (a_f[i] - avg[-1])/(2**amt))

	axs[0].plot(t, np.array(avg))

axs[0].legend(['Raw'] + ['K=%d'%amt for amt in amounts])


plt.show()

"""
My thoughts as of 01JUN2024:
K=1,2 too noisy (+/- 1.5 deg)
K=3 smooths out pretty well and has low latency (150ms ish, +/- 1 deg)
K=4 smooths out very well but has more latency (400ms ish, +/- 0.5 deg)
K=5,6 seems too laggy (>1 second)
"""