fusion-nav 0.1.0

//! Magnetic declination from the World Magnetic Model, for the site the origin names.
//!
//! GOALS.md differentiator 7 lists declination as derived: "a magnetic model, given the GNSS
//! origin". The filter reads it from here when it places an origin and no caller has set one;
//! see [`Eskf::set_magnetic_declination`](crate::Eskf::set_magnetic_declination).
//!
//! [`TABLE`] is the model's declination every 10° of latitude and longitude at one epoch, and
//! [`declination_at`] interpolates it bilinearly. `tools/declination.py` generates the table from
//! NCEI's coefficient file, and its `--drift` says when the epoch has gone stale ([table]). The
//! WMM is US-government work in the public domain, so the table carries no license.
//!
//! It costs about 2.6 KB of flash ([measured]). Hence the feature, which GOALS.md's
//! derived-configuration table asks for, "optional, for its flash cost": off, none of it is linked.
//!
//! PX4 (`geo_magnetic_tables.hpp`) and ArduPilot (`AP_Declination`) ship the same shape, and
//! neither takes a date: a table is fixed at the epoch it was generated for, and secular
//! variation accumulates until it is regenerated. PX4's is the one `tools/ulog2replay.py` carries
//! to reproduce EKF2's own declination, so the replay harness's `declination_model=` key, this
//! table at each log's origin, sits beside the declination EKF2 applied there.
//!
//! [table]: https://github.com/wboayue/fusion-nav/blob/main/GOALS.md#magnetic-declination-from-a-table-read-where-the-origin-is-placed
//! [measured]: https://github.com/wboayue/fusion-nav/blob/main/DESIGN.md#measured-cost-by-function

use crate::geodetic::Geodetic;
use crate::math::wrap_pi;
use crate::units::Radians;

// BEGIN generated by tools/declination.py; edit the script, not these lines.

/// Degrees per table unit.
const SCALE: f32 = 0.01;

/// Grid spacing in degrees, on both axes.
const RESOLUTION: f32 = 10.0;

/// The declination from `WMM_2025` at epoch 2027.5, at sea level, in [`SCALE`] degrees.
/// Rows are latitude −90° to 90°, columns longitude −180° to 180°.
#[rustfmt::skip]
const TABLE: [[i16; 37]; 19] = [
    // -90°
    [
        14809, 13809, 12809, 11809, 10809, 9809, 8809, 7809, 6809, 5809, 4809, 3809, 2809, 1809,
        809, -191, -1191, -2191, -3191, -4191, -5191, -6191, -7191, -8191, -9191, -10191, -11191,
        -12191, -13191, -14191, -15191, -16191, -17191, 17809, 16809, 15809, 14809
    ],
    // -80°
    [
        12827, 11615, 10511, 9500, 8564, 7685, 6849, 6042, 5256, 4484, 3723, 2970, 2222, 1476, 727,
        -28, -798, -1586, -2397, -3232, -4093, -4978, -5886, -6818, -7777, -8766, -9795, -10874,
        -12017, -13238, -14548, -15948, -17421, 17070, 15577, 14151, 12827
    ],
    // -70°
    [
        8590, 7787, 7135, 6576, 6070, 5586, 5097, 4583, 4033, 3447, 2834, 2211, 1595, 997, 417,
        -159, -755, -1395, -2092, -2848, -3650, -4478, -5315, -6148, -6970, -7786, -8611, -9471,
        -10416, -11539, -13029, -15274, 17321, 13694, 11208, 9654, 8590
    ],
    // -60°
    [
        4898, 4743, 4569, 4398, 4240, 4083, 3895, 3635, 3272, 2793, 2215, 1580, 949, 377, -110,
        -533, -951, -1435, -2033, -2744, -3528, -4328, -5093, -5789, -6396, -6906, -7310, -7589,
        -7681, -7413, -6150, -1692, 3093, 4546, 4941, 4991, 4898
    ],
    // -50°
    [
        3197, 3214, 3174, 3114, 3064, 3034, 3005, 2920, 2708, 2313, 1721, 989, 234, -413, -875,
        -1167, -1383, -1653, -2091, -2725, -3478, -4229, -4883, -5387, -5708, -5819, -5678, -5212,
        -4313, -2930, -1257, 307, 1502, 2310, 2809, 3081, 3197
    ],
    // -40°
    [
        2300, 2352, 2353, 2322, 2283, 2259, 2257, 2241, 2125, 1797, 1189, 352, -531, -1246, -1691,
        -1904, -1978, -2017, -2175, -2596, -3223, -3855, -4342, -4614, -4632, -4370, -3821, -3005,
        -2023, -1064, -246, 441, 1034, 1531, 1911, 2164, 2300
    ],
    // -30°
    [
        1731, 1780, 1794, 1782, 1745, 1699, 1663, 1638, 1548, 1246, 621, -268, -1174, -1849, -2220,
        -2373, -2384, -2249, -2044, -2030, -2331, -2774, -3134, -3279, -3151, -2769, -2193, -1502,
        -829, -312, 57, 391, 748, 1098, 1397, 1612, 1731
    ],
    // -20°
    [
        1356, 1380, 1388, 1386, 1358, 1304, 1244, 1197, 1096, 778, 134, -737, -1565, -2125, -2374,
        -2397, -2251, -1915, -1446, -1068, -994, -1229, -1577, -1796, -1767, -1524, -1142, -682,
        -259, -3, 128, 289, 542, 826, 1080, 1264, 1356
    ],
    // -10°
    [
        1126, 1119, 1103, 1099, 1081, 1033, 972, 917, 793, 442, -204, -1006, -1710, -2133, -2225,
        -2049, -1701, -1256, -793, -402, -184, -238, -511, -776, -863, -782, -586, -303, -36, 80,
        86, 162, 376, 642, 883, 1056, 1126
    ],
    // 0°
    [
        1000, 976, 939, 932, 924, 886, 828, 760, 597, 209, -418, -1126, -1700, -1982, -1922, -1597,
        -1152, -721, -371, -86, 128, 166, -9, -239, -364, -375, -304, -152, 3, 35, -24, 8, 205, 477,
        735, 926, 1000
    ],
    // 10°
    [
        915, 914, 884, 891, 905, 878, 811, 703, 473, 37, -568, -1182, -1623, -1766, -1600, -1227,
        -785, -397, -124, 79, 253, 319, 211, 29, -92, -141, -144, -94, -40, -79, -182, -185, -10,
        267, 556, 796, 915
    ],
    // 20°
    [
        798, 876, 907, 958, 1007, 996, 912, 738, 417, -89, -693, -1225, -1534, -1556, -1335, -975,
        -570, -215, 28, 190, 329, 398, 332, 194, 91, 30, -18, -52, -100, -222, -377, -424, -288,
        -21, 297, 596, 798
    ],
    // 30°
    [
        609, 811, 956, 1085, 1178, 1187, 1085, 844, 422, -170, -800, -1280, -1488, -1424, -1176,
        -837, -463, -122, 124, 283, 405, 476, 453, 368, 290, 222, 133, 12, -152, -377, -602, -701,
        -607, -359, -28, 318, 609
    ],
    // 40°
    [
        386, 712, 990, 1216, 1363, 1394, 1278, 978, 460, -231, -916, -1384, -1543, -1438, -1172,
        -830, -458, -108, 168, 360, 501, 602, 646, 639, 598, 513, 355, 115, -199, -553, -856, -996,
        -927, -691, -356, 19, 386
    ],
    // 50°
    [
        194, 612, 997, 1317, 1532, 1602, 1482, 1122, 489, -336, -1113, -1607, -1757, -1637, -1353,
        -984, -583, -193, 146, 421, 646, 836, 987, 1078, 1084, 968, 701, 283, -235, -750, -1131,
        -1288, -1218, -976, -629, -228, 194
    ],
    // 60°
    [
        42, 518, 974, 1373, 1667, 1799, 1695, 1262, 450, -601, -1528, -2064, -2202, -2055, -1733,
        -1313, -849, -377, 74, 491, 875, 1225, 1524, 1739, 1815, 1688, 1296, 633, -185, -929, -1410,
        -1579, -1492, -1230, -859, -425, 42
    ],
    // 70°
    [
        -175, 346, 850, 1303, 1656, 1834, 1719, 1147, 25, -1357, -2406, -2885, -2918, -2667, -2248,
        -1730, -1158, -560, 44, 641, 1219, 1762, 2250, 2643, 2883, 2876, 2488, 1593, 295, -919,
        -1656, -1911, -1835, -1556, -1154, -683, -175
    ],
    // 80°
    [
        -864, -349, 123, 505, 721, 655, 138, -946, -2328, -3394, -3877, -3895, -3615, -3150, -2570,
        -1916, -1216, -486, 259, 1010, 1757, 2490, 3194, 3852, 4434, 4886, 5104, 4853, 3625, 1039,
        -1322, -2314, -2481, -2266, -1869, -1384, -864
    ],
    // 90°
    [
        -16076, -15076, -14076, -13076, -12076, -11076, -10076, -9076, -8076, -7076, -6076, -5076,
        -4076, -3076, -2076, -1076, -76, 924, 1924, 2924, 3924, 4924, 5924, 6924, 7924, 8924, 9924,
        10924, 11924, 12924, 13924, 14924, 15924, 16924, 17924, -17076, -16076
    ],
];

// END generated by tools/declination.py

/// The declination at `site`, east-positive, interpolated bilinearly in the table: `None` for
/// a latitude or longitude that is not a number.
///
/// Latitude is clamped to ±90° and longitude wrapped into ±180°. Height is ignored, as the
/// table is taken at sea level: the field's direction changes by well under the grid's own
/// interpolation error across any altitude a vehicle flies at.
pub(crate) fn declination_at(site: Geodetic) -> Option<Radians> {
    // In f32: in f64 the lookup linked four times the `.text` in software doubles when the two
    // were measured together (DESIGN.md, "Measured cost, by function"), and the answer is
    // returned in f32 anyway.
    let (latitude, longitude) = (site.latitude_deg() as f32, site.longitude_deg() as f32);
    if !latitude.is_finite() || !longitude.is_finite() {
        return None;
    }
    let latitude = latitude.clamp(-90.0, 90.0);
    // One turn either way: a longitude beyond ±540° is not one a receiver reports, and the clamp
    // puts it on the table's edge rather than in a cell it does not name.
    let mut longitude = longitude;
    if longitude > 180.0 {
        longitude -= 360.0;
    }
    if longitude < -180.0 {
        longitude += 360.0;
    }
    interpolate(&TABLE, latitude, longitude.clamp(-180.0, 180.0))
}

/// Bilinear interpolation in `table` at a latitude in ±90° and a longitude in ±180°, each
/// corner taken within half a turn of the south-west one, and the answer wrapped into ±180°.
/// A function of the table so the tests can hand it cells either side of the turn, which the
/// generated one holds only near the magnetic poles.
fn interpolate(table: &[[i16; 37]; 19], latitude: f32, longitude: f32) -> Option<Radians> {
    let (row, north) = cell(latitude + 90.0, table.len());
    let (column, east) = cell(longitude + 180.0, table[0].len());
    let (south_row, north_row) = (table.get(row)?, table.get(row + 1)?);
    let corner = |line: &[i16; 37], at: usize| line.get(at).map(|&v| f32::from(v));
    let sw = corner(south_row, column)?;
    // Each corner within half a turn of the south-west one. Near the magnetic poles neighboring
    // cells sit either side of ±180°, and averaging 179° with −179° gives 0° rather than 180°.
    let near_sw = |value: f32| {
        let (turn, step) = (360.0 / SCALE, value - sw);
        if step > turn / 2.0 {
            value - turn
        } else if step < -turn / 2.0 {
            value + turn
        } else {
            value
        }
    };
    let se = near_sw(corner(south_row, column + 1)?);
    let (nw, ne) = (
        near_sw(corner(north_row, column)?),
        near_sw(corner(north_row, column + 1)?),
    );

    let southern = sw + east * (se - sw);
    let northern = nw + east * (ne - nw);
    let degrees = (southern + north * (northern - southern)) * SCALE;
    Some(Radians::from_radians(wrap_pi(degrees.to_radians())))
}

/// The grid line at or below `offset` degrees from the table's first, and how far past it the
/// point lies as a fraction of the spacing. One short of the last line, so the next one exists;
/// truncation is the floor here, since `offset` is never negative.
fn cell(offset: f32, lines: usize) -> (usize, f32) {
    let index = ((offset / RESOLUTION) as usize).min(lines - 2);
    (index, (offset / RESOLUTION - index as f32).clamp(0.0, 1.0))
}

#[cfg(test)]
mod tests {
    use super::*;

    fn degrees_at(latitude: f64, longitude: f64) -> f32 {
        let radians = declination_at(Geodetic::from_degrees(latitude, longitude, 0.0));
        radians
            .map(|d| d.as_radians().to_degrees())
            .unwrap_or(f32::NAN)
    }

    /// PX4's own `test_geo_lookup.cpp` values at grid points, within its tolerance of
    /// 0.4 + 1.0 degrees: a table from another generator and another epoch of the same model.
    #[test]
    fn grid_points_agree_with_px4s_own_test() {
        assert!((degrees_at(-50.0, -180.0) - 31.7).abs() <= 1.4);
        assert!((degrees_at(-50.0, -100.0) - 27.2).abs() <= 1.4);
    }

    /// The model itself at the table's epoch, from `pygeomag` as `tools/declination.py` reads
    /// it, at three sites between grid points: the table differs from it by the grid's
    /// interpolation error and no more, 0.21° at the worst of these.
    #[test]
    fn between_grid_points_the_table_is_the_model_to_its_interpolation_error() {
        for (latitude, longitude, model) in [
            (56.41, 43.76, 13.8117),
            (47.397742, 8.545594, 3.7347),
            (-33.9, 151.2, 12.8519),
        ] {
            let got = degrees_at(latitude, longitude);
            assert!(
                (got - model).abs() < 0.25,
                "{latitude}, {longitude}: {got} against {model}"
            );
        }
    }

    /// At a grid point the lookup is that cell, in hundredths of a degree.
    #[test]
    fn at_a_grid_point_the_lookup_is_the_cell() {
        let cell = |row: usize, column: usize| f32::from(TABLE[row][column]) * SCALE;
        assert!((degrees_at(-50.0, -180.0) - cell(4, 0)).abs() < 1e-4);
        assert!((degrees_at(-80.0, -170.0) - cell(1, 1)).abs() < 1e-4);
        assert!((degrees_at(90.0, 180.0) - cell(18, 36)).abs() < 1e-4);
    }

    /// On the Antarctic coast, where neighboring cells sit either side of ±180°: the model
    /// reads −173.3° at (−65°, 135°), and averaging the corners without unwrapping them gives
    /// +8.6°; unwrapped it reads −171.4°. The grid is coarse there, so the bound is the
    /// interpolation error's.
    #[test]
    fn corners_either_side_of_a_half_turn_are_unwrapped_before_interpolating() {
        let got = degrees_at(-65.0, 135.0);
        let off = (got - -173.3 + 180.0).rem_euclid(360.0) - 180.0;
        assert!(off.abs() < 2.5, "{got} against -173.3");
        assert!(got.abs() <= 180.0);
    }

    /// Cells either side of the turn, on a table holding nothing else: halfway between 179°
    /// and −179° is 180°, whichever of the two is the south-west corner, and an answer past
    /// 180° comes back wrapped. A one-sided unwrap fails one direction; no final wrap, the last.
    #[test]
    fn interpolation_crosses_the_turn_in_either_direction_and_wraps() {
        let across = |sw: i16, rest: i16, latitude: f32, longitude: f32| {
            let mut table = [[0; 37]; 19];
            table[0][0] = sw;
            (table[0][1], table[1][0], table[1][1]) = (rest, rest, rest);
            interpolate(&table, latitude, longitude)
                .map(|d| d.as_radians().to_degrees())
                .unwrap_or(f32::NAN)
        };
        for (sw, rest) in [(17_900, -17_900), (-17_900, 17_900)] {
            let got = across(sw, rest, -90.0, -175.0);
            assert!((got.abs() - 180.0).abs() < 1e-3, "{sw} to {rest}: {got}");
        }
        // 179° to −178° at three quarters of the way east: 181.25°, which is −178.75°.
        let got = across(17_900, -17_800, -90.0, -172.5);
        assert!((got - -178.75).abs() < 1e-3, "{got}");
    }

    #[test]
    fn halfway_between_two_columns_is_their_mean() {
        let mean = (degrees_at(-50.0, -180.0) + degrees_at(-50.0, -170.0)) / 2.0;
        assert!((degrees_at(-50.0, -175.0) - mean).abs() < 1e-5);
    }

    #[test]
    fn longitude_wraps_and_latitude_clamps() {
        assert_eq!(degrees_at(0.0, 190.0), degrees_at(0.0, -170.0));
        assert_eq!(degrees_at(0.0, -190.0), degrees_at(0.0, 170.0));
        assert_eq!(degrees_at(95.0, 20.0), degrees_at(90.0, 20.0));
    }

    #[test]
    fn a_site_that_is_not_a_number_has_no_declination() {
        assert!(declination_at(Geodetic::from_degrees(f64::NAN, 0.0, 0.0)).is_none());
        assert!(declination_at(Geodetic::from_degrees(0.0, f64::INFINITY, 0.0)).is_none());
    }
}