"""Controlled 2D bearing-localization teaching study. No observed data."""
import json,csv
from pathlib import Path
import numpy as np
import matplotlib
matplotlib.use('Agg')
import matplotlib.pyplot as plt
OUT=Path(__file__).resolve().parent/'localization-study';OUT.mkdir(exist_ok=True)
SEED=27092026;N=1000
noise=np.random.default_rng(SEED).standard_normal((N,4))
def run(spread,sigma=.1,distance=100):
 angles=np.deg2rad(np.linspace(-spread/2,spread/2,4));landmarks=distance*np.stack([np.cos(angles),np.sin(angles)],axis=1)
 theta=angles+np.deg2rad(sigma)*noise
 normals=np.stack([-np.sin(theta),np.cos(theta)],axis=2)
 A=np.einsum('nki,nkj->nij',normals,normals)
 b=np.einsum('nki,nkj,kj->ni',normals,normals,landmarks)
 valid=np.linalg.det(A)>1e-12
 positions=np.linalg.solve(A[valid],b[valid,:,None])[:,:,0];errors=np.linalg.norm(positions,axis=1)
 return {'spread_deg':spread,'bearing_noise_sd_deg':sigma,'range_m':distance,'trials':N,'failed':int(N-valid.sum()),'median_error_m':float(np.median(errors)) if len(errors) else None,'p95_error_m':float(np.percentile(errors,95)) if len(errors) else None},positions
spreads=[5,10,20,30,45,60,90,120,160];rows=[run(s)[0] for s in spreads]
assert run(90,0)[0]['p95_error_m']<1e-10
assert run(0,0)[0]['failed']==N
assert rows[0]['median_error_m']>rows[-1]['median_error_m']
for s in spreads:
 a=run(s,distance=100)[0]['median_error_m'];b=run(s,distance=200)[0]['median_error_m'];assert abs(b-2*a)<1e-9
meta={'seed':SEED,'noise':'Independent Gaussian angular errors, known global heading; same seeded draws across settings.','estimator':'Unweighted least squares of perpendicular distances to infinite bearing lines; not angular maximum likelihood.','landmarks':'Four exact points equally spaced in bearing, all 100 m from truth at origin.','limitations':['Synthetic only','Exact landmark map and heading','No outliers or common bias','P95 is an error percentile, not a confidence bound'],'cases':rows}
(OUT/'study.json').write_text(json.dumps(meta,indent=2))
with (OUT/'study.csv').open('w',newline='') as f:w=csv.DictWriter(f,fieldnames=rows[0]);w.writeheader();w.writerows(rows)
plt.rcParams.update({'font.size':10,'axes.spines.top':False,'axes.spines.right':False,'svg.fonttype':'none','pdf.fonttype':42})
fig,axes=plt.subplots(1,2,figsize=(11,4.3),layout='constrained')
for key,marker,label in [('median_error_m','o','Median'),('p95_error_m','s','95th percentile')]:axes[0].plot(spreads,[r[key] for r in rows],marker=marker,label=label)
axes[0].set(xlabel='Landmark angular spread (degrees)',ylabel='Position error (m)',title='(a) Same range and sensor noise');axes[0].legend(frameon=False);axes[0].grid(alpha=.2)
for s,col,marker in [(5,'#8843a2','.'),(120,'#146b8c','+')]:
 _,p=run(s);axes[1].scatter(p[:,0],p[:,1],s=8,alpha=.3,c=col,marker=marker,label=f'{s} degrees / 1,000 trials')
axes[1].scatter([0],[0],c='black',marker='x',s=60,label='True position');axes[1].set(xlabel='Estimated x (m)',ylabel='Estimated y (m)',title='(b) All simulated positions');axes[1].set_aspect('equal',adjustable='datalim');axes[1].legend(fontsize=8,frameon=False);axes[1].grid(alpha=.2)
for ext in ['svg','png','pdf']:fig.savefig(OUT/f'geometry-study.{ext}',dpi=200,bbox_inches='tight')
print(json.dumps({'cases':len(rows),'trials':len(rows)*N,'first':rows[0],'last':rows[-1],'checks':'zero-noise recovery; exact aligned singularity; range scaling; narrow-vs-wide difference'}))