import csv
import numpy as np
from scipy import stats

csv_file = "autoaim_sysid_simulation.csv"

time = []
tx = []
omega = []
heading = []

with open(csv_file, 'r') as f:
    reader = csv.DictReader(f)
    for row in reader:
        time.append(float(row['Time']))
        tx.append(float(row['Tx']))
        omega.append(float(row['Omega']))
        heading.append(float(row['Heading']))

time = np.array(time)
tx = np.array(tx)
omega = np.array(omega)
heading = np.array(heading)

if len(np.unique(tx)) < 2:
    print("ERROR: All Tx values are identical. Test did not complete all steps.")
    exit(1)

dt = np.mean(np.diff(time))

P, _, _, _, _ = stats.linregress(tx, omega)

cumulative_error = np.cumsum(tx) * dt
I, _, _, _, _ = stats.linregress(cumulative_error, omega)

heading_derivative = np.gradient(heading, dt)
valid_indices = np.abs(heading_derivative) > 0.1

if np.sum(valid_indices) > 10:
    D, _, _, _, _ = stats.linregress(heading_derivative[valid_indices], omega[valid_indices])
else:
    D = 0.0

print(f"P: {P}")
print(f"I: {I}")
print(f"D: {D}")