"""Deterministic, synthetic GNSS least-squares teaching experiment (stdlib only)."""
import csv, json, math
from pathlib import Path

SATELLITES = {
    "good": [(15600000, 0, 20180000), (-15600000, 0, 20183000), (0, 15600000, 20186000), (0, -15600000, 20189000)],
    "poor": [(15600000, 0, 20180000), (15000000, 2500000, 20182000), (14400000, 5000000, 20185000), (13800000, 7500000, 20190000)],
}

def _solve(a, b):
    n=len(a); m=len(a[0]); aug=[list(map(float,row))+[float(y)] for row,y in zip(a,b)]
    for col in range(m):
        pivot=max(range(col,n), key=lambda r:abs(aug[r][col]))
        if abs(aug[pivot][col])<1e-14: raise ValueError("singular geometry")
        aug[col],aug[pivot]=aug[pivot],aug[col]; q=aug[col][col]
        aug[col]=[v/q for v in aug[col]]
        for r in range(n):
            if r==col: continue
            q=aug[r][col]
            aug[r]=[x-q*y for x,y in zip(aug[r],aug[col])]
    return [aug[i][-1] for i in range(m)]

def _normal(a,b):
    m=len(a[0]); ata=[[sum(row[i]*row[j] for row in a) for j in range(m)] for i in range(m)]
    atb=[sum(row[i]*y for row,y in zip(a,b)) for i in range(m)]
    return _solve(ata,atb), ata

def solve(rows, geometry="good"):
    if len(rows)<4: raise ValueError("four satellites required")
    pos=[0.,0.,0.,0.]
    for _ in range(12):
        a=[]; b=[]
        for r in rows:
            sx,sy,sz=SATELLITES[geometry][int(r["satellite_id"])]
            dx,dy,dz=sx-pos[0],sy-pos[1],sz-pos[2]; rho=math.sqrt(dx*dx+dy*dy+dz*dz)
            a.append([-(dx/rho),-(dy/rho),-(dz/rho),1.])
            b.append(float(r["pseudorange_m"])-(rho+pos[3]))
        d,_=_normal(a,b); pos=[x+y for x,y in zip(pos,d)]
        if max(abs(x) for x in d)<1e-8: break
    residual=[]
    for r in rows:
        sx,sy,sz=SATELLITES[geometry][int(r["satellite_id"])]
        rho=math.dist((sx,sy,sz),pos[:3]); residual.append(float(r["pseudorange_m"])-(rho+pos[3]))
    _,normal=_normal(a,b); inv=[]
    for i in range(4):
        e=[1. if j==i else 0. for j in range(4)]; inv.append(_solve(normal,e))
    dop=math.sqrt(sum(inv[i][i] for i in (0,1,2)))
    return {"x_m":pos[0],"y_m":pos[1],"z_m":pos[2],"clock_bias_m":pos[3],"residual_rms_m":math.sqrt(sum(x*x for x in residual)/len(residual)),"hdop":math.sqrt(inv[0][0]+inv[1][1]),"pdop":dop}

def analyze(csv_path):
    with open(csv_path,newline='',encoding='utf-8') as f: rows=list(csv.DictReader(f))
    if not rows: raise ValueError("empty data")
    if set(rows[0]) != {"case","geometry","satellite_id","frequency_hz","pseudorange_m","iono_delay_m"}: raise ValueError("unexpected columns")
    grouped={}
    for r in rows:
        try:
            int(r["satellite_id"]); float(r["frequency_hz"]); float(r["pseudorange_m"]); float(r["iono_delay_m"])
        except (ValueError,TypeError): raise ValueError("invalid observation") from None
        if not all(math.isfinite(float(r[k])) for k in r if k not in ("case","geometry")): raise ValueError("nonfinite observation")
        grouped.setdefault((r["case"],r["geometry"]),[]).append(r)
    results={}
    for (case,geometry),values in grouped.items(): results[case]=solve(values,geometry)
    # Dual-frequency ionosphere-free first-order combination (coefficients only).
    f1,f2=1575.42e6,1227.60e6; tec=12.
    i1=40.3*tec*1e16/f1**2; i2=40.3*tec*1e16/f2**2
    results["dual_frequency"]={"iono_l1_m":i1,"iono_l2_m":i2,"iono_free_m":(f1*f1*i1-f2*f2*i2)/(f1*f1-f2*f2)}
    results["rtk_baseline"]={"common_error_removed_m":1.25,"single_error_m":1.25+0.08,"differenced_error_m":0.08}
    return results

if __name__=='__main__':
    print(json.dumps(analyze(Path(__file__).with_name('synthetic.csv')),indent=2,sort_keys=True))