#!/usr/bin/env python3
"""Evaluates a run of run_fusion.sh against the RTK fixed epochs of the dataset, and compares it
with pure dead reckoning (wheel odometry alone, and wheel odometry + gyroscope).

Ground truth: the F9P fixes of gnss/f9p/weighted with RTK fixed carrier solution (3 cm sigma).
During the simulated outages these fixes never reached the estimator, so there they are
independent ground truth. Errors are horizontal, at the GNSS antenna.

Usage: evaluate.py RUN_DIR [--bag DATASET_BAG_DIR]
Writes RUN_DIR/metrics.txt, RUN_DIR/error_vs_time.png and RUN_DIR/trajectories.png.
Needs ROS 2 (rosbag2_py), numpy and matplotlib.
"""
import argparse
import math
from pathlib import Path

import matplotlib
matplotlib.use('Agg')
import matplotlib.pyplot as plt  # noqa: E402
import numpy as np  # noqa: E402
import rosbag2_py  # noqa: E402
from rclpy.serialization import deserialize_message  # noqa: E402
from rosidl_runtime_py.utilities import get_message  # noqa: E402

NS = '/a200_0812'
RTK_FIXED_VAR = 0.03 ** 2
WGS_A = 6378137.0
WGS_E2 = 6.69437999014e-3


def read(uri, topics):
    r = rosbag2_py.SequentialReader()
    r.open(rosbag2_py.StorageOptions(uri=str(uri), storage_id='mcap'),
           rosbag2_py.ConverterOptions('cdr', 'cdr'))
    types = {t.name: get_message(t.type) for t in r.get_all_topics_and_types()}
    r.set_filter(rosbag2_py.StorageFilter(topics=[t for t in topics if t in types]))
    out = {t: [] for t in topics}
    while r.has_next():
        t, raw, _ = r.read_next()
        out[t].append(deserialize_message(raw, types[t]))
    return out


def stamp(m):
    return m.header.stamp.sec + m.header.stamp.nanosec * 1e-9


def ecef(lat, lon, h):
    lat = np.radians(lat)
    lon = np.radians(lon)
    n = WGS_A / np.sqrt(1 - WGS_E2 * np.sin(lat) ** 2)
    return np.stack([(n + h) * np.cos(lat) * np.cos(lon), (n + h) * np.cos(lat) * np.sin(lon),
                     (n * (1 - WGS_E2) + h) * np.sin(lat)], -1)


def to_enu(lat, lon, h, ref):
    lat0 = math.radians(ref[0])
    lon0 = math.radians(ref[1])
    d = ecef(lat, lon, h) - ecef(np.array(ref[0]), np.array(ref[1]), np.array(ref[2]))
    rot = np.array([[-math.sin(lon0), math.cos(lon0), 0],
                    [-math.sin(lat0) * math.cos(lon0), -math.sin(lat0) * math.sin(lon0),
                     math.cos(lat0)],
                    [math.cos(lat0) * math.cos(lon0), math.cos(lat0) * math.sin(lon0),
                     math.sin(lat0)]])
    return d @ rot.T


def quat_rotate(q, v):
    """Rotates vector v by unit quaternions q (N x 4, x y z w)."""
    u = q[:, :3]
    w = q[:, 3:4]
    t = 2 * np.cross(u, v)
    return v + w * t + np.cross(u, t)


def dead_reckoning(odom, gyro_msgs, lever, use_gyro):
    t = np.array([stamp(m) for m in odom])
    v = np.array([m.twist.twist.linear.x for m in odom])
    if use_gyro:
        tg = np.array([stamp(m) for m in gyro_msgs])
        wz = np.array([m.angular_velocity.z for m in gyro_msgs])
        t_move = t[np.argmax(np.abs(v) > 0.05)]
        still = tg < t_move - 1.0
        bias = wz[still].mean() if still.sum() > 100 else 0.0
        yaw_g = np.concatenate([[0], np.cumsum(0.5 * (wz[1:] + wz[:-1] - 2 * bias) * np.diff(tg))])
        yaw = np.interp(t, tg, yaw_g)
    else:
        q = np.array([[m.pose.pose.orientation.z, m.pose.pose.orientation.w] for m in odom])
        yaw = np.unwrap(2 * np.arctan2(q[:, 0], q[:, 1]))
    dt = np.diff(t, prepend=t[0])
    x = np.cumsum(v * np.cos(yaw) * dt) + np.cos(yaw) * lever[0] - np.sin(yaw) * lever[1]
    y = np.cumsum(v * np.sin(yaw) * dt) + np.sin(yaw) * lever[0] + np.cos(yaw) * lever[1]
    return t, np.c_[x, y]


def align_start(t, xy, tg, g, seconds=60.0):
    """SE(2) alignment to the RTK track over the first seconds (known initial pose)."""
    m = tg <= tg[0] + seconds
    p = np.c_[np.interp(tg[m], t, xy[:, 0]), np.interp(tg[m], t, xy[:, 1])]
    pc = p - p.mean(0)
    qc = g[m] - g[m].mean(0)
    ang = math.atan2(np.sum(pc[:, 0] * qc[:, 1] - pc[:, 1] * qc[:, 0]), np.sum(pc * qc))
    rot = np.array([[math.cos(ang), -math.sin(ang)], [math.sin(ang), math.cos(ang)]])
    return xy @ rot.T + (g[m].mean(0) - rot @ p.mean(0))


def errors(t, xy, tg, g):
    m = (tg > t[0] + 1) & (tg < t[-1])
    p = np.c_[np.interp(tg[m], t, xy[:, 0]), np.interp(tg[m], t, xy[:, 1])]
    return m, np.hypot(*(p - g[m]).T)


def main():
    ap = argparse.ArgumentParser()
    ap.add_argument('run_dir')
    ap.add_argument('--bag', default=str(Path(__file__).resolve().parent.parent /
                                         'husky_2026-11-16_campus_wheels_imu_gnss'))
    a = ap.parse_args()
    run = Path(a.run_dir)

    ds = read(a.bag, [f'{NS}/gnss/f9p/weighted', f'{NS}/platform/odom', f'{NS}/taobotics/imu',
                      f'{NS}/tf_static'])
    lever = None
    for m in ds[f'{NS}/tf_static']:
        for x in m.transforms:
            if x.header.frame_id == 'base_link' and x.child_frame_id == 'gps_f9p':
                tr = x.transform.translation
                lever = np.array([tr.x, tr.y, tr.z])
    fixes = ds[f'{NS}/gnss/f9p/weighted']
    ref = (fixes[0].latitude, fixes[0].longitude, fixes[0].altitude)
    fixed = [m for m in fixes if abs(m.position_covariance[0] - RTK_FIXED_VAR) < 1e-9]
    tg = np.array([stamp(m) for m in fixed])
    g = to_enu(np.array([m.latitude for m in fixed]), np.array([m.longitude for m in fixed]),
               np.array([m.altitude for m in fixed]), ref)[:, :2]
    t_start = tg[0]

    # GNSS fixes the estimator actually received (to mark the outages):
    out = read(run / 'output', ['/state_estimation/geo_pose', '/gnss/input'])
    rx = np.array([stamp(m) for m in out['/gnss/input']])
    held = np.ones(len(tg), bool)
    if len(rx):
        k = np.searchsorted(rx, tg)
        near = np.minimum(np.abs(rx[np.clip(k, 0, len(rx) - 1)] - tg),
                          np.abs(rx[np.clip(k - 1, 0, len(rx) - 1)] - tg))
        held = near > 1e-3

    gp = out['/state_estimation/geo_pose']
    if not gp:
        raise SystemExit(f'No /state_estimation/geo_pose in {run}/output: check {run}/mola.log '
                         '(the estimator did not start, or the geo-reference never converged)')
    ts = np.array([stamp(m) for m in gp])
    o = np.argsort(ts)
    ts = ts[o]
    gp = [gp[i] for i in o]
    q = np.array([[m.pose.orientation.x, m.pose.orientation.y, m.pose.orientation.z,
                   m.pose.orientation.w] for m in gp])
    e = to_enu(np.array([m.pose.position.latitude for m in gp]),
               np.array([m.pose.position.longitude for m in gp]),
               np.array([m.pose.position.altitude for m in gp]), ref)
    e = e + quat_rotate(q, np.tile(lever, (len(gp), 1)))

    traj = {'Smoother fusion': (ts, e[:, :2])}
    for name, ug in (('Wheels only', False), ('Wheels + gyro', True)):
        t, xy = dead_reckoning(ds[f'{NS}/platform/odom'], ds[f'{NS}/taobotics/imu'], lever, ug)
        traj[name] = (t, align_start(t, xy, tg, g))

    lines = []
    fig, ax = plt.subplots(figsize=(11, 4.5))
    for name, (t, xy) in traj.items():
        m, err = errors(t, xy, tg, g)
        h = held[m]
        lines.append(f'{name:16} median {np.median(err):8.3f}  p95 {np.percentile(err, 95):8.3f}  '
                     f'max {err.max():8.3f}  | GNSS outages: median '
                     f'{np.median(err[h]) if h.any() else float("nan"):8.3f}  max '
                     f'{err[h].max() if h.any() else float("nan"):8.3f}  [m]')
        ax.plot((tg[m] - t_start) / 60, err, lw=1.4, label=name)
    ax.set_yscale('log')
    ax.set_xlabel('Time (min)')
    ax.set_ylabel('Horizontal error vs RTK (m)')
    ax.grid(True, alpha=0.3)
    ax.legend()
    fig.tight_layout()
    fig.savefig(run / 'error_vs_time.png', dpi=110)

    fig, ax = plt.subplots(figsize=(8, 8))
    ax.plot(g[:, 0], g[:, 1], color='0.5', lw=4, alpha=0.5, label='RTK fixed')
    for name, (t, xy) in traj.items():
        ax.plot(xy[:, 0], xy[:, 1], lw=1.5, label=name)
    ax.set_aspect('equal')
    ax.set_xlabel('East (m)')
    ax.set_ylabel('North (m)')
    ax.grid(True, alpha=0.3)
    ax.legend()
    fig.tight_layout()
    fig.savefig(run / 'trajectories.png', dpi=110)

    lines.append(f'RTK fixed epochs: {len(tg)}, hidden by the outages: {int(held.sum())}')
    (run / 'metrics.txt').write_text('\n'.join(lines) + '\n')
    print('\n'.join(lines))


if __name__ == '__main__':
    main()
