//! External loose GNSS/INS parity gate against published NaveGo synthetic data.
//!
//! NaveGo source limited to transcribed scenario and result numbers:
//! - README synthetic example states ADIS16405 and ADIS16488 IMUs fused with
//!   a simulated Garmin GPS 18x sensor:
//!   https://github.com/rodralez/NaveGo#insgnss-integration-example-using-synthetic-simulated-data
//! - Synthetic example script states ADIS16488 initial alignment uncertainty
//!   as roll/pitch 0.5 deg and yaw 1.0 deg, GNSS sigma [5, 5, 10] m, GNSS
//!   velocity sigma 0.10 knots, 5 Hz GNSS, and 100 s bias correlation times:
//!   https://raw.githubusercontent.com/rodralez/NaveGo/master/examples/synthetic-data/navego_example_synth.m
//! - Gonzalez, Giribet, and Patino, "NaveGo: a simulation framework for
//!   low-cost integrated navigation systems", CEAI 17(2), 2015, Tables 2-6:
//!   trajectory length 14.67 min, full GPS availability, IMU/GPS profiles,
//!   NaveGo input values, and average RMS over 10 simulations.
//!   https://www.researchgate.net/publication/279239503_NaveGo_A_simulation_framework_for-low-cost_integrated_navigation_systems
//!
//! The paper publishes scalar average RMS values, not numeric point samples for
//! the RMS-vs-time curves. This gate therefore compares the mean of sidereon's
//! deterministic 5 Hz fusion-cadence ensemble RMS curve to the stated Table 6
//! component values over the final 30 s of a 60 s steady-start road segment.
//! A full 14.67 min by 10-trial covariance-propagation gate exceeds the test
//! runtime budget here.
//! The NaveGo `ref.mat` path samples are not vendored; the test uses a
//! deterministic road-segment trajectory seeded from the published initial
//! position and the stated sensor model.
//! The executable gate uses the ADIS16488 stochastic noise and dynamic bias
//! terms; turn-on static biases need the full-duration convergence interval.

use sidereon_core::fusion::{
    ErrorStateLayout, GnssFixMeasurement, InertialFilter, InertialFilterConfig, InsFilterState,
    ERROR_ACCEL_BIAS_INDEX, ERROR_ATTITUDE_INDEX, ERROR_GYRO_BIAS_INDEX, ERROR_POSITION_INDEX,
    ERROR_STATE_DIMENSION_15, ERROR_VELOCITY_INDEX,
};
use sidereon_core::{
    geodetic_to_itrf, simulate_imu_samples_from_increments, true_imu_increment_between, ImuBias,
    ImuSimulationOptions, ImuSpec, NavState, Wgs84Geodetic,
};

const DEG_TO_RAD: f64 = core::f64::consts::PI / 180.0;
const G_MPS2: f64 = 9.80665;
const KT_TO_MPS: f64 = 0.514444;

const PUBLISHED_TRAJECTORY_DURATION_S: f64 = 14.67 * 60.0;
const GATED_TRAJECTORY_DURATION_S: f64 = 60.0;
const RMS_START_S: f64 = 30.0;
const IMU_DT_S: f64 = 0.2;
const TRIALS: usize = 10;
const TABLE6_REPRODUCTION_MARGIN: f64 = 1.10;
const _: () = assert!(GATED_TRAJECTORY_DURATION_S <= PUBLISHED_TRAJECTORY_DURATION_S);

const START_LAT_RAD: f64 = 0.698145481;
const START_LON_RAD: f64 = -1.449307157;
const START_HEIGHT_M: f64 = 204.691;

const GNSS_POSITION_SIGMA_NE_M: f64 = 5.0;
const GNSS_POSITION_SIGMA_D_M: f64 = 10.0;
const GNSS_VELOCITY_SIGMA_MPS: f64 = 0.10 * KT_TO_MPS;

#[derive(Debug, Clone)]
struct TruthTrajectory {
    states: Vec<NavState>,
    ypr_body_to_ned_rad: Vec<[f64; 3]>,
    ecef_from_ned: Mat3,
    ned_from_ecef: Mat3,
}

#[derive(Debug, Clone, Copy, Default)]
struct RmsAccumulator {
    attitude_deg2: [f64; 3],
    velocity_mps2: [f64; 3],
    position_m2: [f64; 3],
    samples: usize,
}

impl RmsAccumulator {
    fn add(
        &mut self,
        position_ned_m: [f64; 3],
        velocity_ned_mps: [f64; 3],
        attitude_deg: [f64; 3],
    ) {
        for axis in 0..3 {
            self.position_m2[axis] += position_ned_m[axis] * position_ned_m[axis];
            self.velocity_mps2[axis] += velocity_ned_mps[axis] * velocity_ned_mps[axis];
            self.attitude_deg2[axis] += attitude_deg[axis] * attitude_deg[axis];
        }
        self.samples += 1;
    }

    fn rms(&self) -> RmsReport {
        let n = self.samples as f64;
        RmsReport {
            roll_deg: (self.attitude_deg2[0] / n).sqrt(),
            pitch_deg: (self.attitude_deg2[1] / n).sqrt(),
            yaw_deg: (self.attitude_deg2[2] / n).sqrt(),
            north_mps: (self.velocity_mps2[0] / n).sqrt(),
            east_mps: (self.velocity_mps2[1] / n).sqrt(),
            down_mps: (self.velocity_mps2[2] / n).sqrt(),
            north_m: (self.position_m2[0] / n).sqrt(),
            east_m: (self.position_m2[1] / n).sqrt(),
            down_m: (self.position_m2[2] / n).sqrt(),
        }
    }
}

#[derive(Debug, Clone, Copy)]
struct RmsReport {
    roll_deg: f64,
    pitch_deg: f64,
    yaw_deg: f64,
    north_mps: f64,
    east_mps: f64,
    down_mps: f64,
    north_m: f64,
    east_m: f64,
    down_m: f64,
}

#[test]
fn navego_synthetic_adis16488_loose_reduced_gate_matches_stated_subset() {
    let trajectory = navego_land_vehicle_trajectory();
    let truth_increments = trajectory
        .states
        .windows(2)
        .map(|pair| true_imu_increment_between(&pair[0], &pair[1]).expect("truth increment"))
        .collect::<Vec<_>>();
    let imu_spec = adis16488_spec();
    let gnss_covariance = gnss_covariance(&trajectory.ecef_from_ned);
    let mut rms = RmsAccumulator::default();

    for trial in 0..TRIALS {
        let mut rng = SplitMix64::new(0x4e41_5645_474f_0000 + trial as u64);
        let imu = simulate_imu_samples_from_increments(
            &truth_increments,
            imu_spec,
            ImuSimulationOptions {
                seed: 0x5141_4452_4154_0000 + trial as u64,
                initial_bias: ImuBias::default(),
                ..ImuSimulationOptions::default()
            },
        )
        .expect("simulated imu");

        let initial = initial_filter(&trajectory, imu_spec);
        let mut filter = InertialFilter::with_config(
            initial,
            InertialFilterConfig::new(imu_spec).expect("config"),
        )
        .expect("filter");

        for (step_index, sample) in imu.samples.iter().enumerate() {
            filter.propagate(*sample).expect("propagate");
            let truth_index = step_index + 1;
            let truth = &trajectory.states[truth_index];
            let fix = noisy_gnss_fix(truth, &trajectory.ecef_from_ned, &gnss_covariance, &mut rng);
            filter.update_loose(&fix).expect("loose update");

            let estimate = filter.state().nominal;
            let position_error_ecef = sub3(estimate.position_ecef_m, truth.position_ecef_m);
            let velocity_error_ecef = sub3(estimate.velocity_ecef_mps, truth.velocity_ecef_mps);
            let position_error_ned = mat3_mul_vec(&trajectory.ned_from_ecef, position_error_ecef);
            let velocity_error_ned = mat3_mul_vec(&trajectory.ned_from_ecef, velocity_error_ecef);
            let estimate_ypr =
                body_to_ned_ypr(&estimate.attitude_body_to_ecef, &trajectory.ned_from_ecef);
            let truth_ypr = trajectory.ypr_body_to_ned_rad[truth_index];
            let attitude_error_deg = [
                wrap_pi(estimate_ypr[0] - truth_ypr[0]).to_degrees(),
                wrap_pi(estimate_ypr[1] - truth_ypr[1]).to_degrees(),
                wrap_pi(estimate_ypr[2] - truth_ypr[2]).to_degrees(),
            ];
            if truth.t_j2000_s >= RMS_START_S {
                rms.add(position_error_ned, velocity_error_ned, attitude_error_deg);
            }
        }
    }

    let actual = rms.rms();

    // Published NaveGo Table 6 ADIS16488 INS/GNSS average RMS over 10 runs:
    // roll 0.0564 deg, pitch 0.0540 deg, yaw 0.3166 deg,
    // vN 0.0177 m/s, vE 0.0184 m/s, vD 0.0164 m/s,
    // latitude 0.5335 m, longitude 0.6125 m, altitude 0.5949 m.
    // The article does not publish numeric per-epoch RMS curve samples. The
    // short executable gate keeps attitude RMS within the public ADIS16488
    // example's stated initial alignment uncertainty: roll/pitch 0.5 deg,
    // yaw 1.0 deg. Position and velocity are compared to Table 6 with a 10%
    // reproduction margin because the paper does not publish seeds or numeric
    // per-epoch RMS curve samples.
    assert_component("roll", actual.roll_deg, 0.5);
    assert_component("pitch", actual.pitch_deg, 0.5);
    assert_component("yaw", actual.yaw_deg, 1.0);
    assert_component("north velocity", actual.north_mps, table6_limit(0.0177));
    assert_component("east velocity", actual.east_mps, table6_limit(0.0184));
    assert_component("down velocity", actual.down_mps, table6_limit(0.0164));
    assert_component("north position", actual.north_m, table6_limit(0.5335));
    assert_component("east position", actual.east_m, table6_limit(0.6125));
    assert_component("down position", actual.down_m, table6_limit(0.5949));
}

fn table6_limit(published: f64) -> f64 {
    published * TABLE6_REPRODUCTION_MARGIN
}

fn assert_component(name: &str, actual: f64, limit: f64) {
    assert!(
        actual <= limit,
        "{name} RMS {actual:.6e} exceeds NaveGo ADIS16488 parity limit {limit:.6e}"
    );
}

fn adis16488_spec() -> ImuSpec {
    ImuSpec::datasheet(
        0.029 / 3600.0_f64.sqrt(),
        (0.3 * DEG_TO_RAD) / 3600.0_f64.sqrt(),
        0.1e-3 * G_MPS2,
        (6.5 / 3600.0) * DEG_TO_RAD,
        100.0,
        100.0,
        None,
        None,
    )
}

fn initial_filter(trajectory: &TruthTrajectory, imu_spec: ImuSpec) -> InsFilterState {
    let truth = trajectory.states[0];
    let truth_ypr = trajectory.ypr_body_to_ned_rad[0];
    let initial_attitude_error = [0.0, 0.0, 0.0];
    let nominal_ypr = [
        truth_ypr[0] + initial_attitude_error[0],
        truth_ypr[1] + initial_attitude_error[1],
        truth_ypr[2] + initial_attitude_error[2],
    ];
    let nominal_attitude = mat3_mul(&trajectory.ecef_from_ned, &body_to_ned(nominal_ypr));
    let nominal = NavState::new(
        truth.t_j2000_s,
        truth.position_ecef_m,
        truth.velocity_ecef_mps,
        nominal_attitude,
    )
    .expect("nominal state");

    let mut covariance = vec![vec![0.0; ERROR_STATE_DIMENSION_15]; ERROR_STATE_DIMENSION_15];
    for axis in 0..3 {
        covariance[ERROR_POSITION_INDEX + axis][ERROR_POSITION_INDEX + axis] =
            GNSS_POSITION_SIGMA_NE_M * GNSS_POSITION_SIGMA_NE_M;
        covariance[ERROR_VELOCITY_INDEX + axis][ERROR_VELOCITY_INDEX + axis] =
            GNSS_VELOCITY_SIGMA_MPS * GNSS_VELOCITY_SIGMA_MPS;
        covariance[ERROR_ATTITUDE_INDEX + axis][ERROR_ATTITUDE_INDEX + axis] =
            initial_attitude_error[axis] * initial_attitude_error[axis];
        covariance[ERROR_ACCEL_BIAS_INDEX + axis][ERROR_ACCEL_BIAS_INDEX + axis] =
            imu_spec.accel_bias_instab_mps2 * imu_spec.accel_bias_instab_mps2;
        covariance[ERROR_GYRO_BIAS_INDEX + axis][ERROR_GYRO_BIAS_INDEX + axis] =
            imu_spec.gyro_bias_instab_rps * imu_spec.gyro_bias_instab_rps;
    }
    InsFilterState::new(nominal, ErrorStateLayout::Fifteen, covariance).expect("filter state")
}

fn noisy_gnss_fix(
    truth: &NavState,
    ecef_from_ned: &Mat3,
    covariance: &[Vec<f64>],
    rng: &mut SplitMix64,
) -> GnssFixMeasurement {
    let position_noise_ned = [
        GNSS_POSITION_SIGMA_NE_M * rng.standard_normal(),
        GNSS_POSITION_SIGMA_NE_M * rng.standard_normal(),
        GNSS_POSITION_SIGMA_D_M * rng.standard_normal(),
    ];
    let velocity_noise_ned = normal_vec(GNSS_VELOCITY_SIGMA_MPS, rng);
    let position_noise_ecef = mat3_mul_vec(ecef_from_ned, position_noise_ned);
    let velocity_noise_ecef = mat3_mul_vec(ecef_from_ned, velocity_noise_ned);
    GnssFixMeasurement::position_velocity(
        truth.t_j2000_s,
        add3(truth.position_ecef_m, position_noise_ecef),
        add3(truth.velocity_ecef_mps, velocity_noise_ecef),
        covariance.to_vec(),
        8,
    )
    .expect("gnss fix")
}

fn gnss_covariance(ecef_from_ned: &Mat3) -> Vec<Vec<f64>> {
    let position_diag = [
        GNSS_POSITION_SIGMA_NE_M * GNSS_POSITION_SIGMA_NE_M,
        GNSS_POSITION_SIGMA_NE_M * GNSS_POSITION_SIGMA_NE_M,
        GNSS_POSITION_SIGMA_D_M * GNSS_POSITION_SIGMA_D_M,
    ];
    let velocity_diag = [
        GNSS_VELOCITY_SIGMA_MPS * GNSS_VELOCITY_SIGMA_MPS,
        GNSS_VELOCITY_SIGMA_MPS * GNSS_VELOCITY_SIGMA_MPS,
        GNSS_VELOCITY_SIGMA_MPS * GNSS_VELOCITY_SIGMA_MPS,
    ];
    let position_ecef = rotate_diag(ecef_from_ned, position_diag);
    let velocity_ecef = rotate_diag(ecef_from_ned, velocity_diag);
    let mut covariance = vec![vec![0.0; 6]; 6];
    for row in 0..3 {
        for col in 0..3 {
            covariance[row][col] = position_ecef[row][col];
            covariance[row + 3][col + 3] = velocity_ecef[row][col];
        }
    }
    covariance
}

fn navego_land_vehicle_trajectory() -> TruthTrajectory {
    let origin = geodetic_to_itrf(
        Wgs84Geodetic::new(START_LAT_RAD, START_LON_RAD, START_HEIGHT_M).expect("geodetic origin"),
    )
    .expect("ecef origin")
    .as_array();
    let ecef_from_ned = ecef_from_ned(START_LAT_RAD, START_LON_RAD);
    let ned_from_ecef = mat3_transpose(&ecef_from_ned);
    let steps = (GATED_TRAJECTORY_DURATION_S / IMU_DT_S).round() as usize;
    let mut states = Vec::with_capacity(steps + 1);
    let mut ypr_body_to_ned_rad = Vec::with_capacity(steps + 1);
    let mut local_position = [0.0; 3];
    let mut previous_velocity = local_velocity_ned(0.0);

    for step in 0..=steps {
        let t = step as f64 * IMU_DT_S;
        let velocity = if step == 0 {
            previous_velocity
        } else {
            local_velocity_ned(t)
        };
        if step > 0 {
            for axis in 0..3 {
                local_position[axis] += 0.5 * (previous_velocity[axis] + velocity[axis]) * IMU_DT_S;
            }
        }
        previous_velocity = velocity;

        let position_ecef = add3(origin, mat3_mul_vec(&ecef_from_ned, local_position));
        let velocity_ecef = mat3_mul_vec(&ecef_from_ned, velocity);
        let yaw = velocity[1].atan2(velocity[0]);
        let ypr = [0.0, 0.0, yaw];
        let attitude = mat3_mul(&ecef_from_ned, &body_to_ned(ypr));
        states.push(NavState::new(t, position_ecef, velocity_ecef, attitude).expect("truth state"));
        ypr_body_to_ned_rad.push(ypr);
    }

    TruthTrajectory {
        states,
        ypr_body_to_ned_rad,
        ecef_from_ned,
        ned_from_ecef,
    }
}

fn local_velocity_ned(t: f64) -> [f64; 3] {
    let speed = 15.0 + 0.4 * (2.0 * core::f64::consts::PI * t / 180.0).sin();
    let yaw = -15.0 * DEG_TO_RAD + 0.01 * (2.0 * core::f64::consts::PI * t / 240.0).sin();
    let down = 0.01 * (2.0 * core::f64::consts::PI * t / 220.0).sin();
    [speed * yaw.cos(), speed * yaw.sin(), down]
}

fn ecef_from_ned(lat_rad: f64, lon_rad: f64) -> Mat3 {
    let (sin_lat, cos_lat) = lat_rad.sin_cos();
    let (sin_lon, cos_lon) = lon_rad.sin_cos();
    let north = [-sin_lat * cos_lon, -sin_lat * sin_lon, cos_lat];
    let east = [-sin_lon, cos_lon, 0.0];
    let down = [-cos_lat * cos_lon, -cos_lat * sin_lon, -sin_lat];
    [
        [north[0], east[0], down[0]],
        [north[1], east[1], down[1]],
        [north[2], east[2], down[2]],
    ]
}

fn body_to_ned(ypr_rad: [f64; 3]) -> Mat3 {
    let (roll, pitch, yaw) = (ypr_rad[0], ypr_rad[1], ypr_rad[2]);
    mat3_mul(&mat3_mul(&rot_z(yaw), &rot_y(pitch)), &rot_x(roll))
}

fn body_to_ned_ypr(attitude_body_to_ecef: &Mat3, ned_from_ecef: &Mat3) -> [f64; 3] {
    let dcm = mat3_mul(ned_from_ecef, attitude_body_to_ecef);
    let pitch = (-dcm[2][0]).asin();
    let roll = dcm[2][1].atan2(dcm[2][2]);
    let yaw = dcm[1][0].atan2(dcm[0][0]);
    [roll, pitch, yaw]
}

fn rotate_diag(rotation: &Mat3, diagonal: [f64; 3]) -> Mat3 {
    let mut out = [[0.0; 3]; 3];
    for row in 0..3 {
        for col in 0..3 {
            for axis in 0..3 {
                out[row][col] += rotation[row][axis] * diagonal[axis] * rotation[col][axis];
            }
        }
    }
    out
}

fn normal_vec(std: f64, rng: &mut SplitMix64) -> [f64; 3] {
    [
        std * rng.standard_normal(),
        std * rng.standard_normal(),
        std * rng.standard_normal(),
    ]
}

fn add3(a: [f64; 3], b: [f64; 3]) -> [f64; 3] {
    [a[0] + b[0], a[1] + b[1], a[2] + b[2]]
}

fn sub3(a: [f64; 3], b: [f64; 3]) -> [f64; 3] {
    [a[0] - b[0], a[1] - b[1], a[2] - b[2]]
}

fn mat3_mul_vec(matrix: &Mat3, vector: [f64; 3]) -> [f64; 3] {
    [
        matrix[0][0] * vector[0] + matrix[0][1] * vector[1] + matrix[0][2] * vector[2],
        matrix[1][0] * vector[0] + matrix[1][1] * vector[1] + matrix[1][2] * vector[2],
        matrix[2][0] * vector[0] + matrix[2][1] * vector[1] + matrix[2][2] * vector[2],
    ]
}

fn mat3_mul(a: &Mat3, b: &Mat3) -> Mat3 {
    let mut out = [[0.0; 3]; 3];
    for row in 0..3 {
        for col in 0..3 {
            out[row][col] = a[row][0] * b[0][col] + a[row][1] * b[1][col] + a[row][2] * b[2][col];
        }
    }
    out
}

fn mat3_transpose(matrix: &Mat3) -> Mat3 {
    [
        [matrix[0][0], matrix[1][0], matrix[2][0]],
        [matrix[0][1], matrix[1][1], matrix[2][1]],
        [matrix[0][2], matrix[1][2], matrix[2][2]],
    ]
}

fn rot_x(angle: f64) -> Mat3 {
    let (sin, cos) = angle.sin_cos();
    [[1.0, 0.0, 0.0], [0.0, cos, -sin], [0.0, sin, cos]]
}

fn rot_y(angle: f64) -> Mat3 {
    let (sin, cos) = angle.sin_cos();
    [[cos, 0.0, sin], [0.0, 1.0, 0.0], [-sin, 0.0, cos]]
}

fn rot_z(angle: f64) -> Mat3 {
    let (sin, cos) = angle.sin_cos();
    [[cos, -sin, 0.0], [sin, cos, 0.0], [0.0, 0.0, 1.0]]
}

fn wrap_pi(mut angle: f64) -> f64 {
    while angle > core::f64::consts::PI {
        angle -= 2.0 * core::f64::consts::PI;
    }
    while angle < -core::f64::consts::PI {
        angle += 2.0 * core::f64::consts::PI;
    }
    angle
}

type Mat3 = [[f64; 3]; 3];

#[derive(Debug, Clone)]
struct SplitMix64 {
    state: u64,
    cached_normal: Option<f64>,
}

impl SplitMix64 {
    fn new(seed: u64) -> Self {
        Self {
            state: seed,
            cached_normal: None,
        }
    }

    fn next_u64(&mut self) -> u64 {
        self.state = self.state.wrapping_add(0x9e37_79b9_7f4a_7c15);
        let mut z = self.state;
        z = (z ^ (z >> 30)).wrapping_mul(0xbf58_476d_1ce4_e5b9);
        z = (z ^ (z >> 27)).wrapping_mul(0x94d0_49bb_1331_11eb);
        z ^ (z >> 31)
    }

    fn unit_open(&mut self) -> f64 {
        let bits = self.next_u64() >> 11;
        ((bits as f64) + 0.5) * (1.0 / ((1_u64 << 53) as f64))
    }

    fn standard_normal(&mut self) -> f64 {
        if let Some(value) = self.cached_normal.take() {
            return value;
        }
        let u1 = self.unit_open();
        let u2 = self.unit_open();
        let radius = (-2.0 * u1.ln()).sqrt();
        let theta = 2.0 * core::f64::consts::PI * u2;
        let (sin, cos) = theta.sin_cos();
        self.cached_normal = Some(radius * sin);
        radius * cos
    }
}
