#!/usr/bin/env python3
"""Offline GPS/IMU fusion for the stationary Genesys2 capture.

The two UART logs are captured sequentially, so their elapsed-time axes are
replayed together.  That is valid for the explicitly stationary experiment,
but is not a substitute for a synchronous moving-platform capture.

The IMU parser accepts both the original averaged ICM-4268x report and the
timestamped raw LSM6DSOX stream from ``steval_mki217v1_test``.
"""

from __future__ import annotations

import argparse
import csv
import json
import math
import re
from dataclasses import dataclass
from pathlib import Path

import matplotlib

matplotlib.use("Agg")
import matplotlib.pyplot as plt
import numpy as np


G = 9.80665

GPS_RE = re.compile(
    r"position lat=(?P<lat>[-+0-9.eE]+) lon=(?P<lon>[-+0-9.eE]+) "
    r"height=(?P<height>[-+0-9.eE]+)"
)
DOP_RE = re.compile(
    r"DOP GDOP=(?P<gdop>[-+0-9.eE]+) PDOP=(?P<pdop>[-+0-9.eE]+) "
    r"HDOP=(?P<hdop>[-+0-9.eE]+) VDOP=(?P<vdop>[-+0-9.eE]+)"
)
IMU_RE = re.compile(
    r"(?P<t>[0-9]+\.[0-9]+) s \| samples=\s*(?P<n>[0-9]+) total=\s*(?P<total>[0-9]+) \| "
    r"Temp=\s*(?P<temp>[-+0-9.]+) C \| Accel\[g\] "
    r"X=\s*(?P<ax>[-+0-9.]+) Y=\s*(?P<ay>[-+0-9.]+) Z=\s*(?P<az>[-+0-9.]+) \| "
    r"Gyro\[dps\] X=\s*(?P<gx>[-+0-9.]+) Y=\s*(?P<gy>[-+0-9.]+) Z=\s*(?P<gz>[-+0-9.]+)"
)


@dataclass
class GpsSample:
    t: float
    unix_s: float
    lat: float
    lon: float
    height: float
    hdop: float
    vdop: float


@dataclass
class ImuSample:
    t: float
    unix_s: float
    samples: int
    temperature_c: float
    accel_mps2: np.ndarray
    gyro_rps: np.ndarray


def read_uart_csv(path: Path):
    with path.open(newline="", encoding="utf-8") as stream:
        yield from csv.DictReader(stream)


def parse_gps(path: Path) -> list[GpsSample]:
    samples: list[GpsSample] = []
    last_dop = (math.nan, math.nan)
    for row in read_uart_csv(path):
        line = row["uart_line"]
        dop = DOP_RE.search(line)
        if dop:
            last_dop = (float(dop["hdop"]), float(dop["vdop"]))
        match = GPS_RE.search(line)
        if match:
            samples.append(
                GpsSample(
                    t=float(row["host_monotonic_s"]),
                    unix_s=float(row["host_unix_s"]),
                    lat=float(match["lat"]),
                    lon=float(match["lon"]),
                    height=float(match["height"]),
                    hdop=last_dop[0],
                    vdop=last_dop[1],
                )
            )
    if len(samples) < 4:
        raise ValueError(f"only {len(samples)} GPS positions found in {path}")
    t0 = samples[0].t
    for sample in samples:
        sample.t -= t0
    return samples


def parse_imu(path: Path) -> list[ImuSample]:
    samples: list[ImuSample] = []
    raw_meta: dict[str, float] | None = None
    first_raw_tick: int | None = None
    for row in read_uart_csv(path):
        line = row["uart_line"]

        if line.startswith("IMU_META,LSM6DSOX,"):
            fields = line.split(",")[2:]
            values: dict[str, float] = {}
            for field in fields:
                key, separator, value = field.partition("=")
                if not separator:
                    continue
                values[key] = float(value)
            required = {
                "tick_hz",
                "odr_hz",
                "accel_ug_lsb",
                "gyro_udps_lsb",
                "temp_lsb_c",
                "temp_offset_millic",
            }
            missing = required.difference(values)
            if missing:
                raise ValueError(
                    f"LSM6DSOX metadata missing {sorted(missing)} in {path}"
                )
            if values["tick_hz"] <= 0.0 or values["temp_lsb_c"] <= 0.0:
                raise ValueError(f"invalid LSM6DSOX metadata in {path}")
            raw_meta = values
            continue

        if line.startswith("IMU_RAW,LSM6DSOX,"):
            if raw_meta is None:
                raise ValueError(f"LSM6DSOX raw sample precedes metadata in {path}")
            fields = line.split(",")
            if len(fields) != 11:
                raise ValueError(f"malformed LSM6DSOX raw sample in {path}: {line}")
            tick = int(fields[3])
            if first_raw_tick is None:
                first_raw_tick = tick
            raw_accel = np.array([int(x) for x in fields[4:7]], dtype=float)
            raw_gyro = np.array([int(x) for x in fields[7:10]], dtype=float)
            raw_temperature = int(fields[10])
            samples.append(
                ImuSample(
                    t=(tick - first_raw_tick) / raw_meta["tick_hz"],
                    unix_s=float(row["host_unix_s"]),
                    samples=1,
                    temperature_c=(
                        raw_meta["temp_offset_millic"] / 1000.0
                        + raw_temperature / raw_meta["temp_lsb_c"]
                    ),
                    accel_mps2=(
                        G * raw_accel * raw_meta["accel_ug_lsb"] * 1e-6
                    ),
                    gyro_rps=np.deg2rad(
                        raw_gyro * raw_meta["gyro_udps_lsb"] * 1e-6
                    ),
                )
            )
            continue

        match = IMU_RE.search(line)
        if not match:
            continue
        samples.append(
            ImuSample(
                t=float(match["t"]),
                unix_s=float(row["host_unix_s"]),
                samples=int(match["n"]),
                temperature_c=float(match["temp"]),
                accel_mps2=G
                * np.array(
                    [float(match["ax"]), float(match["ay"]), float(match["az"])]
                ),
                gyro_rps=np.deg2rad(
                    [float(match["gx"]), float(match["gy"]), float(match["gz"])]
                ),
            )
        )
    if len(samples) < 4:
        raise ValueError(f"only {len(samples)} IMU windows found in {path}")
    if first_raw_tick is None:
        t0 = samples[0].t
        for sample in samples:
            sample.t -= t0
    return samples


def geodetic_to_ecef(lat_deg, lon_deg, height_m):
    lat = np.deg2rad(np.asarray(lat_deg, dtype=float))
    lon = np.deg2rad(np.asarray(lon_deg, dtype=float))
    height = np.asarray(height_m, dtype=float)
    a = 6378137.0
    e2 = 6.69437999014e-3
    n = a / np.sqrt(1.0 - e2 * np.sin(lat) ** 2)
    x = (n + height) * np.cos(lat) * np.cos(lon)
    y = (n + height) * np.cos(lat) * np.sin(lon)
    z = (n * (1.0 - e2) + height) * np.sin(lat)
    return np.column_stack((x, y, z))


def gps_to_enu(samples: list[GpsSample]):
    lat0 = float(np.median([sample.lat for sample in samples]))
    lon0 = float(np.median([sample.lon for sample in samples]))
    h0 = float(np.median([sample.height for sample in samples]))
    origin = geodetic_to_ecef([lat0], [lon0], [h0])[0]
    xyz = geodetic_to_ecef(
        [sample.lat for sample in samples],
        [sample.lon for sample in samples],
        [sample.height for sample in samples],
    )
    lat = math.radians(lat0)
    lon = math.radians(lon0)
    rotation = np.array(
        [
            [-math.sin(lon), math.cos(lon), 0.0],
            [
                -math.sin(lat) * math.cos(lon),
                -math.sin(lat) * math.sin(lon),
                math.cos(lat),
            ],
            [
                math.cos(lat) * math.cos(lon),
                math.cos(lat) * math.sin(lon),
                math.sin(lat),
            ],
        ]
    )
    return (xyz - origin) @ rotation.T, (lat0, lon0, h0)


def ecef_to_geodetic(xyz):
    """Convert ECEF XYZ to WGS-84 latitude, longitude and ellipsoid height."""
    xyz = np.atleast_2d(np.asarray(xyz, dtype=float))
    x, y, z = xyz.T
    a = 6378137.0
    e2 = 6.69437999014e-3
    horizontal = np.hypot(x, y)
    lon = np.arctan2(y, x)
    lat = np.arctan2(z, horizontal * (1.0 - e2))
    for _ in range(8):
        n = a / np.sqrt(1.0 - e2 * np.sin(lat) ** 2)
        height = horizontal / np.cos(lat) - n
        lat = np.arctan2(z, horizontal * (1.0 - e2 * n / (n + height)))
    n = a / np.sqrt(1.0 - e2 * np.sin(lat) ** 2)
    height = horizontal / np.cos(lat) - n
    return np.column_stack((np.rad2deg(lat), np.rad2deg(lon), height))


def enu_to_geodetic(enu, reference):
    """Convert local ENU positions back to WGS-84 coordinates."""
    lat0, lon0, h0 = reference
    lat = math.radians(lat0)
    lon = math.radians(lon0)
    rotation = np.array(
        [
            [-math.sin(lon), math.cos(lon), 0.0],
            [
                -math.sin(lat) * math.cos(lon),
                -math.sin(lat) * math.sin(lon),
                math.cos(lat),
            ],
            [
                math.cos(lat) * math.cos(lon),
                math.cos(lat) * math.sin(lon),
                math.sin(lat),
            ],
        ]
    )
    origin = geodetic_to_ecef([lat0], [lon0], [h0])[0]
    xyz = origin + np.atleast_2d(np.asarray(enu, dtype=float)) @ rotation
    return ecef_to_geodetic(xyz)


def skew(v):
    x, y, z = v
    return np.array([[0.0, -z, y], [z, 0.0, -x], [-y, x, 0.0]])


def quat_normalize(q):
    q = np.asarray(q, dtype=float)
    q /= np.linalg.norm(q)
    return q if q[0] >= 0.0 else -q


def quat_multiply(q, r):
    w, x, y, z = q
    a, b, c, d = r
    return np.array(
        [
            w * a - x * b - y * c - z * d,
            w * b + x * a + y * d - z * c,
            w * c - x * d + y * a + z * b,
            w * d + x * c - y * b + z * a,
        ]
    )


def rotvec_quat(v):
    angle = float(np.linalg.norm(v))
    if angle < 1e-10:
        return quat_normalize(np.r_[1.0, 0.5 * np.asarray(v)])
    return np.r_[math.cos(angle / 2.0), np.asarray(v) * math.sin(angle / 2.0) / angle]


def quat_rotation(q):
    w, x, y, z = q
    return np.array(
        [
            [1 - 2 * (y * y + z * z), 2 * (x * y - w * z), 2 * (x * z + w * y)],
            [2 * (x * y + w * z), 1 - 2 * (x * x + z * z), 2 * (y * z - w * x)],
            [2 * (x * z - w * y), 2 * (y * z + w * x), 1 - 2 * (x * x + y * y)],
        ]
    )


def align_quat(source, target):
    source = np.asarray(source, dtype=float) / np.linalg.norm(source)
    target = np.asarray(target, dtype=float) / np.linalg.norm(target)
    dot = float(np.dot(source, target))
    if dot < -0.999999:
        axis = np.cross(source, [1.0, 0.0, 0.0])
        if np.linalg.norm(axis) < 1e-6:
            axis = np.cross(source, [0.0, 1.0, 0.0])
        axis /= np.linalg.norm(axis)
        return np.r_[0.0, axis]
    return quat_normalize(np.r_[1.0 + dot, np.cross(source, target)])


def robust_sigma(values, floor):
    values = np.asarray(values, dtype=float)
    mad = np.median(np.abs(values - np.median(values)))
    return max(float(1.4826 * mad), floor)


class StationaryEskf:
    """15-state ESKF with right attitude error in a local ENU frame."""

    def __init__(self, position, attitude, accel_sigma, gyro_sigma, gps_sigma):
        self.p = np.asarray(position, dtype=float).copy()
        self.v = np.zeros(3)
        self.q = quat_normalize(attitude)
        self.ba = np.zeros(3)
        self.bg = np.zeros(3)
        self.p_cov = np.diag(
            np.r_[gps_sigma**2, np.full(3, 1.0), np.deg2rad([5, 5, 30]) ** 2,
                  np.full(3, 0.05**2), np.full(3, np.deg2rad(0.05) ** 2)]
        )
        self.accel_sigma = np.asarray(accel_sigma)
        self.gyro_sigma = np.asarray(gyro_sigma)
        self.gps_r = np.diag(np.asarray(gps_sigma) ** 2)
        self.rejected_gps = 0
        self.accepted_gps = 0

    def propagate(self, accel, gyro, dt):
        if dt <= 0.0:
            return
        dt = min(float(dt), 1.0)
        omega = np.asarray(gyro) - self.bg
        specific_force = np.asarray(accel) - self.ba
        rotation = quat_rotation(self.q)
        acceleration = rotation @ specific_force + np.array([0.0, 0.0, -G])
        self.p += self.v * dt + 0.5 * acceleration * dt * dt
        self.v += acceleration * dt
        self.q = quat_normalize(quat_multiply(self.q, rotvec_quat(omega * dt)))

        f = np.zeros((15, 15))
        f[0:3, 3:6] = np.eye(3)
        f[3:6, 6:9] = -rotation @ skew(specific_force)
        f[3:6, 9:12] = -rotation
        f[6:9, 6:9] = -skew(omega)
        f[6:9, 12:15] = -np.eye(3)
        phi = np.eye(15) + f * dt

        noise_map = np.zeros((15, 12))
        noise_map[3:6, 0:3] = rotation
        noise_map[6:9, 3:6] = np.eye(3)
        noise_map[9:12, 6:9] = np.eye(3)
        noise_map[12:15, 9:12] = np.eye(3)
        qc = np.diag(
            np.r_[
                self.accel_sigma**2,
                self.gyro_sigma**2,
                np.full(3, 0.002**2),
                np.full(3, np.deg2rad(0.002) ** 2),
            ]
        )
        self.p_cov = phi @ self.p_cov @ phi.T + noise_map @ qc @ noise_map.T * dt
        self.p_cov = 0.5 * (self.p_cov + self.p_cov.T)

    def _update(self, residual, h, r):
        innovation = h @ self.p_cov @ h.T + r
        gain = np.linalg.solve(innovation, h @ self.p_cov).T
        delta = gain @ residual
        identity = np.eye(15)
        ikh = identity - gain @ h
        self.p_cov = ikh @ self.p_cov @ ikh.T + gain @ r @ gain.T
        self.p += delta[0:3]
        self.v += delta[3:6]
        self.q = quat_normalize(quat_multiply(self.q, rotvec_quat(delta[6:9])))
        self.ba += delta[9:12]
        self.bg += delta[12:15]
        self.p_cov = 0.5 * (self.p_cov + self.p_cov.T)
        return innovation

    def gps_update(self, position, gate=16.27):
        h = np.zeros((3, 15))
        h[:, 0:3] = np.eye(3)
        residual = np.asarray(position) - self.p
        innovation = h @ self.p_cov @ h.T + self.gps_r
        mahalanobis = float(residual @ np.linalg.solve(innovation, residual))
        if mahalanobis > gate:
            self.rejected_gps += 1
            return False, mahalanobis
        self._update(residual, h, self.gps_r)
        self.accepted_gps += 1
        return True, mahalanobis

    def zero_velocity_update(self, sigma=0.025):
        h = np.zeros((3, 15))
        h[:, 3:6] = np.eye(3)
        self._update(-self.v, h, np.eye(3) * sigma**2)

    def zero_gyro_update(self, measured_gyro, sigma):
        h = np.zeros((3, 15))
        h[:, 12:15] = np.eye(3)
        # z = bg + noise when the true angular rate is zero.
        self._update(np.asarray(measured_gyro) - self.bg, h, np.diag(sigma**2))


def run_filter(gps: list[GpsSample], imu: list[ImuSample], enu):
    accel = np.vstack([sample.accel_mps2 for sample in imu])
    gyro = np.vstack([sample.gyro_rps for sample in imu])
    mean_accel = np.mean(accel, axis=0)
    attitude = align_quat(mean_accel, [0.0, 0.0, 1.0])
    accel_sigma = np.array([robust_sigma(accel[:, i], 0.003) for i in range(3)])
    gyro_sigma = np.array(
        [robust_sigma(gyro[:, i], math.radians(0.001)) for i in range(3)]
    )
    gps_sigma = np.array([robust_sigma(enu[:, i], 1.0 if i < 2 else 2.0) for i in range(3)])

    eskf = StationaryEskf(enu[0], attitude, accel_sigma, gyro_sigma, gps_sigma)
    events = [(sample.t, 0, i) for i, sample in enumerate(imu)]
    events += [(sample.t, 1, i) for i, sample in enumerate(gps)]
    events.sort()
    current_accel = accel[0]
    current_gyro = gyro[0]
    last_t = 0.0
    output = []

    for t, kind, index in events:
        eskf.propagate(current_accel, current_gyro, t - last_t)
        last_t = t
        if kind == 0:
            current_accel = accel[index]
            current_gyro = gyro[index]
            eskf.zero_velocity_update()
            eskf.zero_gyro_update(current_gyro, np.maximum(gyro_sigma, math.radians(0.002)))
        else:
            accepted, mahalanobis = eskf.gps_update(enu[index])
            output.append(
                {
                    "t": t,
                    "raw": enu[index].copy(),
                    "fused": eskf.p.copy(),
                    "velocity": eskf.v.copy(),
                    "position_std": np.sqrt(np.maximum(np.diag(eskf.p_cov)[0:3], 0.0)),
                    "accepted": accepted,
                    "mahalanobis": mahalanobis,
                }
            )
    return eskf, output, accel_sigma, gyro_sigma, gps_sigma, attitude


def radial_95(values):
    centered = values - np.median(values, axis=0)
    return float(np.percentile(np.linalg.norm(centered, axis=1), 95))


def write_csvs(output_dir, gps, imu, enu, filtered):
    with (output_dir / "gps_positions.csv").open("w", newline="") as stream:
        writer = csv.writer(stream)
        writer.writerow(["t_s", "unix_s", "lat_deg", "lon_deg", "height_m", "east_m", "north_m", "up_m", "hdop", "vdop"])
        for sample, local in zip(gps, enu):
            writer.writerow([sample.t, sample.unix_s, sample.lat, sample.lon, sample.height, *local, sample.hdop, sample.vdop])
    with (output_dir / "imu_windows.csv").open("w", newline="") as stream:
        writer = csv.writer(stream)
        writer.writerow(["t_s", "unix_s", "samples", "temperature_c", "ax_mps2", "ay_mps2", "az_mps2", "gx_rps", "gy_rps", "gz_rps"])
        for sample in imu:
            writer.writerow([sample.t, sample.unix_s, sample.samples, sample.temperature_c, *sample.accel_mps2, *sample.gyro_rps])
    with (output_dir / "eskf_positions.csv").open("w", newline="") as stream:
        writer = csv.writer(stream)
        writer.writerow(["t_s", "gps_e_m", "gps_n_m", "gps_u_m", "eskf_e_m", "eskf_n_m", "eskf_u_m", "ve_mps", "vn_mps", "vu_mps", "sigma_e_m", "sigma_n_m", "sigma_u_m", "gps_accepted", "mahalanobis2"])
        for row in filtered:
            writer.writerow([row["t"], *row["raw"], *row["fused"], *row["velocity"], *row["position_std"], int(row["accepted"]), row["mahalanobis"]])


def write_playback_map(output_dir, gps, filtered, reference):
    """Write a Leaflet map with a sample-by-sample GPS/ESKF player."""
    fused_lla = enu_to_geodetic(
        np.vstack([row["fused"] for row in filtered]), reference
    )
    records = []
    for index, (sample, row, corrected) in enumerate(zip(gps, filtered, fused_lla)):
        records.append(
            {
                "i": index,
                "t": round(float(row["t"]), 6),
                "raw": [sample.lat, sample.lon, sample.height],
                "fused": [float(corrected[0]), float(corrected[1]), float(corrected[2])],
                "accepted": bool(row["accepted"]),
                "mahalanobis": round(float(row["mahalanobis"]), 3),
                "separation": round(float(np.linalg.norm(row["raw"][:2] - row["fused"][:2])), 3),
            }
        )

    data_json = json.dumps(records, separators=(",", ":"), ensure_ascii=False)
    center_json = json.dumps([reference[0], reference[1]])
    html = """<!doctype html>
<html lang="tr">
<head>
  <meta charset="utf-8">
  <meta name="viewport" content="width=device-width,initial-scale=1">
  <title>GPS / ESKF sample oynatıcı</title>
  <link rel="stylesheet" href="https://unpkg.com/leaflet@1.9.4/dist/leaflet.css">
  <style>
    html,body,#map { height:100%; margin:0; font-family:system-ui,sans-serif; }
    .panel { position:absolute; z-index:1000; left:12px; bottom:18px; width:min(620px,calc(100% - 44px));
      background:rgba(255,255,255,.94); padding:12px 14px; border-radius:9px; box-shadow:0 2px 12px #0004; }
    .row { display:flex; align-items:center; gap:9px; flex-wrap:wrap; }
    button,select { padding:6px 10px; } input[type=range] { flex:1; min-width:180px; }
    .readout { margin-top:8px; display:grid; grid-template-columns:repeat(3,minmax(120px,1fr)); gap:4px 12px; font-size:13px; }
    .raw { color:#d62728; } .fused { color:#1769aa; } .gated { color:#e67e22; font-weight:700; }
    .legend { background:white; padding:7px 9px; line-height:1.5; box-shadow:0 1px 5px #0004; }
    .dot { display:inline-block; width:10px; height:10px; border-radius:50%; margin-right:5px; }
    @media(max-width:600px) { .readout { grid-template-columns:1fr 1fr; } }
  </style>
</head>
<body>
<div id="map"></div>
<div class="panel">
  <div class="row">
    <button id="play">▶ Oynat</button><button id="reset">↺ Başa al</button><button id="fit">İzleri sığdır</button>
    <label>Hız <select id="speed"><option>0.5</option><option selected>1</option><option>2</option><option>4</option></select>×</label>
  </div>
  <div class="row" style="margin-top:7px"><span id="tick">1 / 1</span><input id="slider" type="range" min="0" value="0"><b id="time">0.0 s</b></div>
  <div class="readout">
    <span class="raw" id="raw"></span><span class="fused" id="fused"></span><span id="height"></span>
    <span id="distance"></span><span id="gate"></span><span id="nis"></span>
  </div>
</div>
<script src="https://unpkg.com/leaflet@1.9.4/dist/leaflet.js"></script>
<script>
const samples=__DATA__, center=__CENTER__;
const map=L.map('map',{zoomControl:true}).setView(center,20);
L.tileLayer('https://tile.openstreetmap.org/{z}/{x}/{y}.png',{maxZoom:22,attribution:'&copy; OpenStreetMap contributors'}).addTo(map);
L.control.scale({imperial:false}).addTo(map);
const rawLine=L.polyline([],{color:'#d62728',weight:2,opacity:.55}).addTo(map);
const fusedLine=L.polyline([],{color:'#1769aa',weight:3,opacity:.9}).addTo(map);
const rawDots=L.layerGroup().addTo(map), fusedDots=L.layerGroup().addTo(map);
const rawNow=L.circleMarker(center,{radius:8,color:'#8b0000',weight:2,fillColor:'#d62728',fillOpacity:.9}).addTo(map);
const fusedNow=L.circleMarker(center,{radius:7,color:'#083b66',weight:2,fillColor:'#2196f3',fillOpacity:.95}).addTo(map);
const legend=L.control({position:'topright'}); legend.onAdd=()=>{const d=L.DomUtil.create('div','legend');d.innerHTML='<b>GPS / IMU ESKF</b><br><span class="dot" style="background:#d62728"></span>Ham GPS<br><span class="dot" style="background:#2196f3"></span>ESKF + ZUPT<br><span class="dot" style="background:#e67e22"></span>Reddedilen GPS';return d};legend.addTo(map);
const slider=document.getElementById('slider'); slider.max=samples.length-1;
let index=0,timer=null;
function fmt(v,n=7){return Number(v).toFixed(n)}
function render(i){
  index=Math.max(0,Math.min(samples.length-1,Number(i))); slider.value=index;
  const shown=samples.slice(0,index+1), s=samples[index];
  rawLine.setLatLngs(shown.map(x=>x.raw.slice(0,2))); fusedLine.setLatLngs(shown.map(x=>x.fused.slice(0,2)));
  rawDots.clearLayers(); fusedDots.clearLayers();
  shown.forEach(x=>{
    L.circleMarker(x.raw.slice(0,2),{radius:2,stroke:false,fillColor:x.accepted?'#d62728':'#e67e22',fillOpacity:.7}).addTo(rawDots);
    L.circleMarker(x.fused.slice(0,2),{radius:2,stroke:false,fillColor:'#1769aa',fillOpacity:.8}).addTo(fusedDots);
  });
  rawNow.setLatLng(s.raw.slice(0,2)).setStyle({fillColor:s.accepted?'#d62728':'#e67e22'});
  fusedNow.setLatLng(s.fused.slice(0,2));
  document.getElementById('tick').textContent=`${index+1} / ${samples.length}`;
  document.getElementById('time').textContent=`${s.t.toFixed(3)} s`;
  document.getElementById('raw').textContent=`GPS: ${fmt(s.raw[0])}, ${fmt(s.raw[1])}`;
  document.getElementById('fused').textContent=`ESKF: ${fmt(s.fused[0])}, ${fmt(s.fused[1])}`;
  document.getElementById('height').textContent=`h: ${s.raw[2].toFixed(1)} → ${s.fused[2].toFixed(1)} m`;
  document.getElementById('distance').textContent=`yatay fark: ${s.separation.toFixed(2)} m`;
  const gate=document.getElementById('gate'); gate.textContent=s.accepted?'GPS kabul':'GPS RED'; gate.className=s.accepted?'':'gated';
  document.getElementById('nis').textContent=`NIS: ${s.mahalanobis.toFixed(2)}`;
}
function stop(){if(timer){clearTimeout(timer);timer=null}document.getElementById('play').textContent='▶ Oynat'}
function step(){
  if(index>=samples.length-1){stop();return}
  const dt=Math.max(60,Math.min(2000,(samples[index+1].t-samples[index].t)*1000/Number(document.getElementById('speed').value)));
  timer=setTimeout(()=>{render(index+1);step()},dt);
}
document.getElementById('play').onclick=()=>{if(timer){stop()}else{if(index>=samples.length-1)render(0);document.getElementById('play').textContent='⏸ Duraklat';step()}};
document.getElementById('reset').onclick=()=>{stop();render(0)};
document.getElementById('fit').onclick=()=>map.fitBounds(L.latLngBounds(samples.flatMap(s=>[s.raw.slice(0,2),s.fused.slice(0,2)])).pad(.15));
slider.oninput=()=>{stop();render(slider.value)};
render(0); map.fitBounds(L.latLngBounds(samples.flatMap(s=>[s.raw.slice(0,2),s.fused.slice(0,2)])).pad(.15));
</script>
</body></html>
""".replace("__DATA__", data_json).replace("__CENTER__", center_json)
    (output_dir / "trajectory_map.html").write_text(html, encoding="utf-8")


def plot_results(output_dir, gps, imu, filtered):
    time = np.array([row["t"] for row in filtered])
    raw = np.vstack([row["raw"] for row in filtered])
    fused = np.vstack([row["fused"] for row in filtered])
    labels = ["East", "North", "Up"]
    fig, axes = plt.subplots(3, 1, figsize=(10, 8), sharex=True)
    for i, axis in enumerate(axes):
        axis.plot(time, raw[:, i], ".", color="0.65", ms=3, label="GPS WLS")
        axis.plot(time, fused[:, i], color="tab:blue", lw=1.8, label="ESKF + ZUPT")
        axis.set_ylabel(f"{labels[i]} [m]")
        axis.grid(alpha=0.25)
    axes[0].legend()
    axes[-1].set_xlabel("Normalized elapsed time [s]")
    fig.suptitle("Stationary GPS / IMU error-state Kalman filter")
    fig.tight_layout()
    fig.savefig(output_dir / "position_timeseries.png", dpi=160)
    plt.close(fig)

    accepted = np.array([row["accepted"] for row in filtered])
    fig, axis = plt.subplots(figsize=(7, 7))
    axis.scatter(raw[accepted, 0], raw[accepted, 1], s=12, alpha=0.5, color="0.5", label="GPS WLS accepted")
    axis.scatter(raw[~accepted, 0], raw[~accepted, 1], marker="x", s=55, color="tab:red", label="GPS gated")
    axis.plot(fused[:, 0], fused[:, 1], color="tab:blue", lw=1.5, label="ESKF + ZUPT")
    axis.scatter(fused[-1, 0], fused[-1, 1], marker="x", s=70, color="tab:red", label="ESKF final")
    axis.set_aspect("equal", adjustable="box")
    axis.set_xlabel("East [m]")
    axis.set_ylabel("North [m]")
    axis.grid(alpha=0.25)
    axis.legend()
    center = np.median(raw[:, :2], axis=0)
    central_radius = max(20.0, 1.2 * float(np.percentile(np.abs(raw[:, :2] - center), 99)))
    axis.set_xlim(center[0] - central_radius, center[0] + central_radius)
    axis.set_ylim(center[1] - central_radius, center[1] + central_radius)
    axis.set_title(f"Horizontal stationary scatter (central view; gated={np.sum(~accepted)})")
    fig.tight_layout()
    fig.savefig(output_dir / "horizontal_scatter.png", dpi=160)
    plt.close(fig)

    t_imu = np.array([sample.t for sample in imu])
    accel = np.vstack([sample.accel_mps2 for sample in imu]) / G
    gyro = np.rad2deg(np.vstack([sample.gyro_rps for sample in imu]))
    fig, axes = plt.subplots(2, 1, figsize=(10, 6), sharex=True)
    for i, name in enumerate("XYZ"):
        axes[0].plot(t_imu, accel[:, i], label=name)
        axes[1].plot(t_imu, gyro[:, i], label=name)
    axes[0].set_ylabel("Acceleration [g]")
    axes[1].set_ylabel("Angular rate [deg/s]")
    axes[1].set_xlabel("IMU elapsed time [s]")
    for axis in axes:
        axis.grid(alpha=0.25)
        axis.legend(ncol=3)
    fig.suptitle("ICM-4268x 500 ms window averages")
    fig.tight_layout()
    fig.savefig(output_dir / "imu_stationary.png", dpi=160)
    plt.close(fig)


def main():
    parser = argparse.ArgumentParser()
    parser.add_argument("--gps", type=Path, default=Path("logs/gps_60s.csv"))
    parser.add_argument("--imu", type=Path, default=Path("logs/imu_60s.csv"))
    parser.add_argument("--output", type=Path, default=Path("output"))
    args = parser.parse_args()
    args.output.mkdir(parents=True, exist_ok=True)

    gps = parse_gps(args.gps)
    imu = parse_imu(args.imu)
    enu, reference = gps_to_enu(gps)
    eskf, filtered, accel_sigma, gyro_sigma, gps_sigma, attitude = run_filter(gps, imu, enu)
    write_csvs(args.output, gps, imu, enu, filtered)
    plot_results(args.output, gps, imu, filtered)
    write_playback_map(args.output, gps, filtered, reference)

    raw = np.vstack([row["raw"] for row in filtered])
    fused = np.vstack([row["fused"] for row in filtered])
    raw_std = np.std(raw, axis=0, ddof=1)
    fused_std = np.std(fused, axis=0, ddof=1)
    raw_h95 = radial_95(raw[:, :2])
    fused_h95 = radial_95(fused[:, :2])
    imu_accel = np.vstack([sample.accel_mps2 for sample in imu])
    imu_gyro = np.vstack([sample.gyro_rps for sample in imu])
    mean_accel = np.mean(imu_accel, axis=0)
    mean_gyro = np.mean(imu_gyro, axis=0)
    lines = [
        "Stationary GPS/IMU ESKF result",
        "================================",
        f"GPS positions: {len(gps)} over {gps[-1].t - gps[0].t:.3f} s",
        f"IMU windows: {len(imu)} over {imu[-1].t - imu[0].t:.3f} s",
        f"Underlying SPI samples: {sum(sample.samples for sample in imu)}",
        "Capture mode: sequential logs replayed on a common elapsed-time axis",
        f"ENU reference (capture median): lat={reference[0]:.10f} deg, lon={reference[1]:.10f} deg, h={reference[2]:.3f} m",
        f"GPS robust sigma E/N/U [m]: {gps_sigma[0]:.3f}, {gps_sigma[1]:.3f}, {gps_sigma[2]:.3f}",
        f"GPS accepted/rejected: {eskf.accepted_gps}/{eskf.rejected_gps}",
        f"Raw GPS std E/N/U [m]: {raw_std[0]:.3f}, {raw_std[1]:.3f}, {raw_std[2]:.3f}",
        f"ESKF std E/N/U [m]: {fused_std[0]:.3f}, {fused_std[1]:.3f}, {fused_std[2]:.3f}",
        f"Raw/ESKF horizontal radial 95% [m]: {raw_h95:.3f} / {fused_h95:.3f}",
        f"IMU mean acceleration XYZ [m/s^2]: {mean_accel[0]:.6f}, {mean_accel[1]:.6f}, {mean_accel[2]:.6f}",
        f"IMU mean angular rate XYZ [deg/s]: {np.rad2deg(mean_gyro)[0]:.6f}, {np.rad2deg(mean_gyro)[1]:.6f}, {np.rad2deg(mean_gyro)[2]:.6f}",
        f"IMU robust accel noise XYZ [m/s^2]: {accel_sigma[0]:.6f}, {accel_sigma[1]:.6f}, {accel_sigma[2]:.6f}",
        f"IMU robust gyro noise XYZ [deg/s]: {np.rad2deg(gyro_sigma)[0]:.6f}, {np.rad2deg(gyro_sigma)[1]:.6f}, {np.rad2deg(gyro_sigma)[2]:.6f}",
        f"Initial body-to-ENU quaternion [w,x,y,z]: {attitude[0]:.8f}, {attitude[1]:.8f}, {attitude[2]:.8f}, {attitude[3]:.8f}",
        f"Final ESKF position E/N/U [m]: {eskf.p[0]:.3f}, {eskf.p[1]:.3f}, {eskf.p[2]:.3f}",
        f"Final ESKF velocity E/N/U [m/s]: {eskf.v[0]:.6f}, {eskf.v[1]:.6f}, {eskf.v[2]:.6f}",
        "Caution: no surveyed ground truth; reported gains are precision/stability, not absolute accuracy.",
        "Caution: ZUPT is enabled because the board was declared stationary.",
    ]
    summary = "\n".join(lines) + "\n"
    (args.output / "summary.txt").write_text(summary, encoding="utf-8")
    print(summary, end="")


if __name__ == "__main__":
    main()