#!/usr/bin/env python3
"""Normalized PID experiment; Python 3.12 standard library only.
Run in an empty directory: python3 pid_lab.py
Writes pid-results.csv and pid-summary.json. All results are simulations.
"""
import csv
import json
from collections import deque


def simulate(name, kp=3., ki=2., kd=1., delay=0., limit=3., antiwindup=True, dt=.005):
    y = velocity = integral = previous = 0.
    delayed = deque([0.] * round(delay / dt))
    rows = []
    for n in range(round(12 / dt) + 1):
        t = n * dt
        target = 1. if t < 6 else .4
        measured = delayed.popleft() if delayed else y
        if delay:
            delayed.append(y)
        error = target - measured
        derivative = 0. if n == 0 else (measured - previous) / dt
        candidate = integral + error * dt
        raw = kp * error + ki * candidate - kd * derivative
        if not antiwindup or abs(raw) <= limit or raw * error < 0:
            integral = candidate
        u = max(-limit, min(limit, kp * error + ki * integral - kd * derivative))
        rows.append((name, t, target, y, u, integral))
        # Semi-implicit Euler of y'' + 2y' + y = u, normalized signals.
        velocity += dt * (u - 2 * velocity - y)
        y += dt * velocity
        previous = measured
    return rows


def main():
    scenarios = [simulate('nominal'), simulate('delay_0.4s', delay=.4),
                 simulate('limit_0.8_aw', limit=.8),
                 simulate('limit_0.8_no_aw', limit=.8, antiwindup=False)]
    with open('pid-results.csv', 'w', newline='') as handle:
        writer = csv.writer(handle)
        writer.writerow(['scenario', 'time_s', 'target', 'output', 'control', 'integral'])
        for rows in scenarios:
            writer.writerows(rows)
    summary = {rows[0][0]: {'output_at_6s': rows[1200][3], 'output_at_12s': rows[-1][3],
                           'IAE_0_12s': sum(abs(r[2] - r[3]) * .005 for r in rows[:-1])}
               for rows in scenarios}
    with open('pid-summary.json', 'w') as handle:
        json.dump(summary, handle, indent=2)
    print(json.dumps(summary, indent=2))


if __name__ == '__main__':
    main()