"""Duskcoil synthetic experiments. Python 3.12 / NumPy 1.26 / Matplotlib 3.6.
Run in an empty output directory: python3 engineering_labs.py
No hardware data; all units and assumptions are documented in the articles.
"""
from pathlib import Path
import json
import numpy as np
import matplotlib
matplotlib.use('Agg')
import matplotlib.pyplot as plt
plt.rcParams.update({'svg.fonttype': 'none', 'font.size': 13})
OUT = Path('.')
RESULTS = {}
def csv(name, columns, names):
    np.savetxt(OUT / (name+'.csv'), np.column_stack(columns), delimiter=',', header=','.join(names), comments='', fmt='%.10g')
def save(name, title):
    plt.tight_layout()
    p=OUT/(name+'.svg');plt.savefig(p, metadata={'Date': None});plt.close()
    s=p.read_text();i=s.index('<svg');j=s.index('>',i)
    s=s[:j]+' role="img" aria-labelledby="title desc"'+s[j:j+1]+f'<title id="title">{title}</title><desc id="desc">Synthetic data, not hardware measurements. Axes include units; see article for conditions.</desc>'+s[j+1:]
    p.write_text(s)
def kalman():
    rng=np.random.default_rng(42);dt=.1;t=np.arange(300)*dt
    truth=t+np.maximum(t-12,0)*.4
    z=truth+rng.normal(0,.7,len(t));z[160]+=12;z[70:100]=np.nan
    F=np.array([[1,dt],[0,1]]);h=np.array([1.,0.]);I=np.eye(2)
    records={}
    for name,q,r,gate in [('baseline',.4,.49,False),('gated',.4,.49,True),('small_Q',.002,.49,True),('small_R',.4,.0049,True)]:
        x=np.array([0.,1.]);P=np.diag([1.,1.]);vals=[];sig=[];rejected=0
        # Discrete independent acceleration noise per step (not continuous spectral density).
        g=np.array([dt**2/2,dt]);Q=q*np.outer(g,g)
        for measurement in z:
            x=F@x;P=F@P@F.T+Q
            if np.isfinite(measurement):
                e=measurement-h@x;S=h@P@h+r
                if gate and e*e/S>9:
                    rejected+=1
                else:
                    K=P@h/S;x=x+K*e;A=I-np.outer(K,h);P=A@P@A.T+np.outer(K,K)*r
            assert np.linalg.eigvalsh(P).min()>-1e-12
            vals.append(x[0]);sig.append(np.sqrt(P[0,0]))
        vals=np.array(vals);records[name]=vals
        RESULTS['kalman_'+name]={'rmse_m':float(np.sqrt(np.mean((vals-truth)**2))),'rejected':rejected}
        csv('kalman_'+name,[t,truth,z,vals,sig],['time_s','truth_m','measurement_m','estimate_m','sigma_m'])
    plt.figure(figsize=(10,5));plt.plot(t,z-truth,'.',alpha=.35,label='measurement error')
    for k,v in records.items():plt.plot(t,v-truth,label=k)
    plt.axvspan(7,9.9,color='gray',alpha=.15,label='missing measurements');plt.ylim(-3,4)
    plt.xlabel('Time (s)');plt.ylabel('Position error (m)');plt.title('Synthetic KF: 300 samples, dt = 0.1 s; +12 m outlier at 16 s');plt.legend(ncol=2);save('kalman','Synthetic Kalman position error')
def rotation(a):
    return np.array([[np.cos(a),-np.sin(a)],[np.sin(a),np.cos(a)]])
def icp_fit(source,target,angle,translation,threshold):
    R=rotation(angle);u=np.array(translation,dtype=float)
    for _ in range(100):
        p=source@R.T+u;d=np.linalg.norm(p[:,None,:]-target[None,:,:],axis=2);idx=d.argmin(axis=1);dist=d[np.arange(len(p)),idx];mask=dist<threshold
        if mask.sum()<3:break
        a=p[mask];b=target[idx[mask]];ac=a.mean(axis=0);bc=b.mean(axis=0)
        U,_,Vt=np.linalg.svd((a-ac).T@(b-bc));D=np.diag([1.,np.linalg.det(Vt.T@U.T)]);delta=Vt.T@D@U.T
        shift=bc-delta@ac;R=delta@R;u=delta@u+shift
        if np.linalg.norm(shift)+np.linalg.norm(delta-np.eye(2))<1e-10:break
    p=source@R.T+u;dist=np.linalg.norm(p[:,None,:]-target[None,:,:],axis=2).min(axis=1);mask=dist<threshold
    return p,R,u,float(np.sqrt(np.mean(dist[mask]**2))) if mask.any() else None,float(mask.mean())
def icp():
    # Exact rigid data isolates correspondence and shape effects; no random sensor noise.
    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
    ring=np.column_stack([np.cos(np.arange(120)*2*np.pi/120),np.sin(np.arange(120)*2*np.pi/120)])
    outliers=np.column_stack([np.linspace(4,7,30),np.full(30,3.)]);dirty=np.vstack([source,outliers])
    cases=[('good_init',source,target,18,[.35,-.25],.5),('bad_init',source,target,110,[1.5,1],.5),('outliers_open',dirty,target,18,[.35,-.25],10),('outliers_gated',dirty,target,18,[.35,-.25],.5),('symmetric_ring',ring,ring,90,[0,0],.5)]
    fig,axes=plt.subplots(2,3,figsize=(12,8))
    for ax,(name,s,d,ang,u,threshold) in zip(axes.flat,cases):
        p,R,v,rmse,fitness=icp_fit(s,d,np.deg2rad(ang),u,threshold)
        gtR=np.eye(2) if name=='symmetric_ring' else true_R;gtu=np.zeros(2) if name=='symmetric_ring' else true_u
        error=np.rad2deg(np.arctan2((R@gtR.T)[1,0],(R@gtR.T)[0,0]))
        RESULTS['icp_'+name]={'inlier_rmse_m':rmse,'fitness':fitness,'rotation_error_deg':float(abs(error)),'translation_error_m':float(np.linalg.norm(v-gtu))}
        csv('icp_'+name,[s[:,0],s[:,1],p[:,0],p[:,1]],['source_x_m','source_y_m','aligned_x_m','aligned_y_m'])
        ax.scatter(d[:,0],d[:,1],s=18,label='target');ax.scatter(p[:,0],p[:,1],s=8,label='aligned');ax.set_aspect('equal');ax.set_title(name);ax.set_xlabel('x (m)');ax.set_ylabel('y (m)');ax.legend(fontsize=10)
    axes.flat[-1].axis('off');fig.suptitle('Synthetic 2D point-to-point ICP; thresholds differ by case');save('icp','Synthetic ICP alignments: good, bad, outlier and symmetric cases')
    csv('icp_target',[target[:,0],target[:,1]],['x_m','y_m'])
def frequency(dt=.01, export=True):
    t=np.arange(0,30+dt/2,dt);curves={}
    for H in (2.,5.):
        for control in (False,True):
            x=0.;p=0.;v=[]
            def f(y,deficit):
                z,g=y;return np.array([(g-deficit-z)/(2*H),((-z/.05 if control else 0)-g)/.5])
            for tm in t:
                v.append(50*(1+x));y=np.array([x,p]);deficit=.05 if tm>=2-1e-10 else 0
                k1=f(y,deficit);k2=f(y+dt*k1/2,deficit);k3=f(y+dt*k2/2,deficit);k4=f(y+dt*k3,deficit);x,p=y+dt*(k1+2*k2+2*k3+k4)/6
            key=f'H{H:g}_'+('droop' if control else 'no_governor');curves[key]=np.array(v)
            if export:RESULTS['frequency_'+key]={'min_Hz':float(min(v)),'end_Hz':float(v[-1]),'initial_rocof_Hz_s':-50*.05/(2*H)}
    if export:
        csv('frequency',[t,*curves.values()],['time_s',*curves.keys()]);plt.figure(figsize=(10,5))
        for k,v in curves.items():plt.plot(t,v,label=k)
        plt.xlabel('Time (s)');plt.ylabel('Frequency (Hz)');plt.title('Synthetic one-area grid: 5% deficit at 2 s, D=1, R=0.05, Tg=0.5 s');plt.legend();save('frequency','Synthetic frequency response to a five percent deficit')
    return curves
def aliasing():
    t=np.arange(40)/20;hi=np.arange(0,2,.001);a=np.cos(2*np.pi*12*t);b=np.cos(2*np.pi*8*t)
    RESULTS['aliasing_max_difference']=float(np.max(np.abs(a-b)));assert np.allclose(a,b,atol=1e-12)
    csv('aliasing',[t,a,b],['time_s','cos_12Hz','cos_8Hz']);plt.figure(figsize=(10,5));plt.plot(hi,np.cos(2*np.pi*12*hi),label='12 Hz continuous');plt.plot(hi,np.cos(2*np.pi*8*hi),'--',label='8 Hz continuous');plt.scatter(t,a,color='black',label='20 samples/s (identical)');plt.xlim(0,.5);plt.xlabel('Time (s)');plt.ylabel('Amplitude (unitless)');plt.title('Synthetic aliasing: 12 Hz and 8 Hz cosines at 20 samples/s');plt.legend();save('aliasing','Two continuous cosines produce the same sampled values')
def transforms():
    R=rotation(np.pi/2);p=np.array([2.,0.]);u=np.array([1.,2.]);q=R@p+u;back=R.T@(q-u)
    assert np.allclose(q,[1,4]) and np.allclose(back,p);RESULTS['transform']={'world_point_m':q.tolist(),'recovered_local_point_m':back.tolist()}
    plt.figure(figsize=(6,6));plt.arrow(0,0,1,0,width=.02,color='gray');plt.arrow(0,0,0,1,width=.02,color='gray');plt.arrow(1,2,0,1,width=.025,color='C0');plt.arrow(1,2,-1,0,width=.025,color='C1');plt.plot([1,1],[2,4],':');plt.scatter([1],[4]);plt.text(1.1,4,'point (1,4)');plt.text(1.1,3,'local x');plt.text(-.7,2.1,'local y');plt.text(1.1,2,'origin (1,2)');plt.xlim(-1,3);plt.ylim(-.5,5);plt.gca().set_aspect('equal');plt.xlabel('world x (m)');plt.ylabel('world y (m)');plt.title('Translation (1,2) m + 90 degree rotation');save('transforms','Coordinate transform with translation and ninety degree rotation')
if __name__=='__main__':
    kalman();icp();a=frequency();b=frequency(.005,False)
    RESULTS['frequency_dt_check_max_Hz']=max(float(np.max(np.abs(a[k]-b[k][::2]))) for k in a)
    assert RESULTS['frequency_dt_check_max_Hz']<1e-5
    aliasing();transforms();Path('engineering-results.json').write_text(json.dumps(RESULTS,indent=2)+'\n');print(json.dumps(RESULTS,indent=2))