"""Bounded synthetic PID/Kalman/ICP experiments, shared by browser and local CLI.
Download experiment.py, kernels.py and pid_lab.py into one directory.
python3 experiment.py pid --parameters '{"kp": 3, "ki": 2, "kd": 1, "delay": 0, "limit": 3, "antiwindup": true}'
"""
import json
import math
import time

def metric(key, value, unit, reason='No correspondences'):
    result = dict(id=key, value=value, unit=unit)
    if value is None: result['reason'] = reason
    return result

def series(key, x, y, xu, yu):
    return dict(id=key, x=[float(v) for v in x], y=[float(v) if math.isfinite(v) else None for v in y], x_unit=xu, y_unit=yu)

def run(name, p):
    start = time.perf_counter()
    if name == 'pid':
        from pid_lab import simulate
        rows = simulate('interactive', **p)
        metrics = [metric('IAE_0_12s',sum(abs(r[2]-r[3])*.005 for r in rows[:-1]),'normalized·s'),metric('output_at_6s',rows[1200][3],'normalized'),metric('output_at_12s',rows[-1][3],'normalized')]
        curves = [series(key,[r[1] for r in rows],[r[col] for r in rows],'s','normalized') for key,col in [('target',2),('output',3),('control',4)]]
    elif name == 'kalman':
        import numpy as np
        from kernels import kalman_values
        t,truth,z,vals,sigma,rejected = kalman_values(**p)
        metrics = [metric('rmse_m',float(np.sqrt(np.mean((vals-truth)**2))),'m'),metric('rejected',int(rejected),'count')]
        curves = [series(key,t,value,'s','m') for key,value in [('truth',truth),('measurement',z),('estimate',vals)]]
    elif name == 'icp':
        import numpy as np
        from kernels import rotation,icp_fit
        case=p['case']; threshold=p['threshold']
        a=np.column_stack([np.linspace(0,3,61),np.zeros(61)])
        b=np.column_stack([np.zeros(41),np.linspace(0,2,41)])
        target=np.vstack([a,b]); true_R=rotation(np.deg2rad(20)); true_u=np.array([.4,-.3]); source=(target-true_u)@true_R
        angle=18; translation=[.35,-.25]
        if case=='bad_init':angle=110;translation=[1.5,1]
        if case=='outliers':source=np.vstack([source,np.column_stack([np.linspace(4,7,30),np.full(30,3.)])])
        if case=='symmetric_ring':
            target=np.column_stack([np.cos(np.arange(120)*2*np.pi/120),np.sin(np.arange(120)*2*np.pi/120)])
            source=target.copy();true_R=np.eye(2);true_u=np.zeros(2);angle=90;translation=[0,0]
        aligned,R,u,rmse,fitness=icp_fit(source,target,np.deg2rad(angle),translation,threshold)
        error=np.rad2deg(np.arctan2((R@true_R.T)[1,0],(R@true_R.T)[0,0]))
        metrics=[metric('inlier_rmse_m',rmse,'m'),metric('fitness',fitness,'fraction'),metric('rotation_error_deg',float(abs(error)),'deg'),metric('translation_error_m',float(np.linalg.norm(u-true_u)),'m')]
        curves=[series(key,points[:,0],points[:,1],'m','m') for key,points in [('target',target),('aligned',aligned)]]
    else:raise ValueError('Unknown experiment')
    return dict(metrics=metrics,series=curves,stdout='',provenance=dict(data_type='synthetic',compute_ms=(time.perf_counter()-start)*1000))

if __name__=='__main__':
    import argparse
    parser=argparse.ArgumentParser(description=__doc__)
    parser.add_argument('experiment',choices=['pid','kalman','icp'])
    parser.add_argument('--parameters',required=True)
    args=parser.parse_args()
    print(json.dumps(run(args.experiment,json.loads(args.parameters)),allow_nan=False))