#!/usr/bin/env python3
"""
noise_driver.py : what wrong noise values do. Runs next to wall_kf.py, which it imports unchanged.

    python3 noise_driver.py                      # sensor noise r: 100 times too small, as in wall_kf.py, 100 times too large
    python3 noise_driver.py --which q --factor 10

Three wall_kf.Filter objects share one wall_kf.Sim run: the same odometry steps and the same LiDAR scans
(LiDAR off from 12 to 17 s, as in wall_kf.py --headless). Only one noise value differs. For each filter it
reports the RMS error and how often the truth stays inside the filter's own 2-sigma band.
"""
import argparse, json, math
import wall_kf

NAMES = {"small": "too small", "matched": "as in wall_kf.py", "large": "too large"}

def run(which="r", factor=100.0, seconds=25.0, seed=11, q=0.01, r=0.02, outage=True):
    sim = wall_kf.Sim(seed)
    filters = {}
    for name, s in (("small", 1 / factor), ("matched", 1.0), ("large", factor)):
        f = wall_kf.Filter(q * s if which == "q" else q, r * s if which == "r" else r)
        f.d = sim.d + 0.04
        filters[name] = f
    rows = {name: [] for name in filters}; truth = []; times = []; next_scan = 0.2
    while sim.t < seconds - 1e-9:
        u = sim.odom()
        for f in filters.values(): f.predict(u)
        if sim.t >= next_scan - 1e-9:
            next_scan += 0.2; z = sim.scan(); lidar = not (outage and 12.0 <= sim.t < 17.0)
            for f in filters.values(): f.lidar = lidar; f.update(z)
        times.append(sim.t); truth.append(sim.d)
        for name, f in filters.items(): rows[name].append((f.d, math.sqrt(f.P)))
    out = {"t": times, "which": which, "factor": factor, "panels": {}}
    for name, f in filters.items():
        err = [d - x for (d, s), x in zip(rows[name], truth)]
        inside = sum(1 for e, (d, s) in zip(err, rows[name]) if abs(e) <= 2 * s) / len(err)
        out["panels"][name] = {"err": err, "sigma": [s for d, s in rows[name]], "q": f.q, "r": f.r,
                               "rms": math.sqrt(sum(e * e for e in err) / len(err)), "inside": inside}
    return json.dumps(out)

def report(which="r", factor=100.0, seconds=25.0, seed=11):
    o = json.loads(run(which, factor, seconds, seed))
    for name, p in o["panels"].items():
        print(f"  {which} {NAMES[name]:17s} (q {100 * p['q']:.3g} cm/sqrt(m), r {100 * p['r']:.3g} cm):  "
              f"RMS {100 * p['rms']:.2f} cm,  truth inside 2 sigma {100 * p['inside']:.0f}% of the time")

if __name__ == "__main__":
    ap = argparse.ArgumentParser(description=__doc__, formatter_class=argparse.RawDescriptionHelpFormatter)
    ap.add_argument("--which", choices=["r", "q"], default="r"); ap.add_argument("--factor", type=float, default=100.0)
    ap.add_argument("--seconds", type=float, default=25.0); ap.add_argument("--seed", type=int, default=11)
    a = ap.parse_args(); report(a.which, a.factor, a.seconds, a.seed)