squitter 0.1.1

//! Surface Position message — type codes 5-8. Carries no altitude (the
//! aircraft is on the ground); instead carries movement (ground speed) and
//! track angle.

use super::cpr::{self, CprFormat};
use crate::bits::{BitReader, BitWriter};
use crate::error::{BitError, MessageError};

/// Movement-field decoding bins: `(lower_bound, knots_at_lower_bound, step)`.
/// Per ICAO Annex 10 Vol IV Table 2-6, algorithm cross-checked against
/// pyModeS's `decoder/bds/bds06.py::_decode_movement`.
const MOV_LB: [u8; 7] = [2, 9, 13, 39, 94, 109, 124];
const KTS_LB: [f64; 7] = [0.125, 1.0, 2.0, 15.0, 70.0, 100.0, 175.0];
const STEP: [f64; 6] = [0.125, 0.25, 0.5, 1.0, 2.0, 5.0];

/// Decodes the 7-bit movement field to ground speed in knots, or `None` if
/// the field carries no speed information (raw `0` or `> 124`, values
/// `125..=127` being reserved).
#[must_use]
pub fn decode_movement(mov: u8) -> Option<f64> {
    if mov == 0 || mov > 124 {
        return None;
    }
    if mov == 1 {
        return Some(0.0);
    }
    if mov == 124 {
        return Some(175.0);
    }
    let i = MOV_LB
        .iter()
        .position(|&lb| lb > mov)
        .unwrap_or(MOV_LB.len());
    Some(KTS_LB[i - 1] + f64::from(mov - MOV_LB[i - 1]) * STEP[i - 1])
}

/// Encodes a ground speed in knots to the nearest representable 7-bit
/// movement code (the inverse of [`decode_movement`]). `None` encodes to
/// raw `0` (no information).
#[must_use]
pub fn encode_movement(knots: Option<f64>) -> u8 {
    let Some(knots) = knots else {
        return 0;
    };
    if knots <= 0.0 {
        return 1;
    }
    if knots >= 175.0 {
        return 124;
    }
    for i in 0..MOV_LB.len() {
        let bin_start = KTS_LB[i];
        let bin_end = KTS_LB.get(i + 1).copied().unwrap_or(f64::INFINITY);
        if knots >= bin_start && knots < bin_end {
            // core has no f64::round without libm; (x + 0.5) truncated by
            // the cast below is equivalent for x >= 0, which steps always
            // is here (knots >= bin_start is the loop condition above).
            let steps = (knots - bin_start) / STEP[i] + 0.5;
            #[allow(
                clippy::cast_possible_truncation,
                clippy::cast_sign_loss,
                reason = "steps is bounded by the bin width / STEP[i], always fits u8 here"
            )]
            return MOV_LB[i] + steps as u8;
        }
    }
    124
}

/// Surface Position message (type codes 5-8; 56 bits).
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
pub struct SurfacePosition {
    /// Type code (`5..=8`).
    pub type_code: u8,
    /// Raw 7-bit movement (packed ground speed) code.
    pub movement_raw: u8,
    /// Whether `track_raw` carries a valid track angle.
    pub track_status: bool,
    /// Raw 7-bit track angle code (`track_raw * 360/128` degrees), only
    /// meaningful when `track_status` is set.
    pub track_raw: u8,
    /// Whether the position was synchronized to a UTC time reference.
    pub utc_synced: bool,
    /// Which CPR encoding this frame's `lat_cpr`/`lon_cpr` use.
    pub cpr_format: CprFormat,
    /// Raw 17-bit CPR-encoded latitude.
    pub lat_cpr: u32,
    /// Raw 17-bit CPR-encoded longitude.
    pub lon_cpr: u32,
}

impl SurfacePosition {
    /// Ground speed in knots, or `None` if the movement field carries no
    /// speed information.
    #[must_use]
    pub fn ground_speed_knots(self) -> Option<f64> {
        decode_movement(self.movement_raw)
    }

    /// Track angle in decimal degrees, or `None` if `track_status` is unset.
    #[must_use]
    pub fn track_degrees(self) -> Option<f64> {
        self.track_status
            .then(|| f64::from(self.track_raw) * 360.0 / 128.0)
    }

    pub(crate) fn decode(type_code: u8, r: &mut BitReader<'_>) -> Result<Self, MessageError> {
        let movement_raw = r.read_u8(7)?;
        let track_status = r.read_bool()?;
        let track_raw = r.read_u8(7)?;
        let utc_synced = r.read_bool()?;
        let cpr_format = CprFormat::from_raw(r.read_u8(1)?);
        let lat_cpr = r.read_u32(17)?;
        let lon_cpr = r.read_u32(17)?;
        Ok(Self {
            type_code,
            movement_raw,
            track_status,
            track_raw,
            utc_synced,
            cpr_format,
            lat_cpr,
            lon_cpr,
        })
    }

    pub(crate) fn encode(&self, w: &mut BitWriter<'_>) -> Result<(), BitError> {
        w.write_bits(u64::from(self.type_code), 5)?;
        w.write_bits(u64::from(self.movement_raw), 7)?;
        w.write_bool(self.track_status)?;
        w.write_bits(u64::from(self.track_raw), 7)?;
        w.write_bool(self.utc_synced)?;
        w.write_bits(u64::from(self.cpr_format.to_raw()), 1)?;
        w.write_bits(u64::from(self.lat_cpr), 17)?;
        w.write_bits(u64::from(self.lon_cpr), 17)?;
        Ok(())
    }
}

/// Pairs one even-format and one odd-format [`SurfacePosition`] to resolve
/// an absolute latitude/longitude via CPR global decoding against a nearby
/// reference position (see [`cpr::surface_global_decode`] for why a
/// reference is required for surface, unlike airborne, CPR).
#[derive(Debug, Clone, Copy, Default)]
pub struct SurfacePositionPair {
    even: Option<SurfacePosition>,
    odd: Option<SurfacePosition>,
}

impl SurfacePositionPair {
    /// Builds an empty pair.
    #[must_use]
    pub const fn new() -> Self {
        Self {
            even: None,
            odd: None,
        }
    }

    /// Feeds one surface position message. Returns `Some((lat, lon))` once
    /// both parities are present and resolve against `lat_ref`/`lon_ref`
    /// (typically the receiving station's own position, within about 45
    /// nautical miles of the aircraft); `None` otherwise.
    pub fn push(&mut self, msg: SurfacePosition, lat_ref: f64, lon_ref: f64) -> Option<(f64, f64)> {
        let even_is_newer = matches!(msg.cpr_format, CprFormat::Even);
        match msg.cpr_format {
            CprFormat::Even => self.even = Some(msg),
            CprFormat::Odd => self.odd = Some(msg),
        }
        let even = self.even?;
        let odd = self.odd?;
        cpr::surface_global_decode(
            even.lat_cpr,
            even.lon_cpr,
            odd.lat_cpr,
            odd.lon_cpr,
            lat_ref,
            lon_ref,
            even_is_newer,
        )
    }
}

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

    fn round_trip(original: SurfacePosition) -> SurfacePosition {
        let mut me = [0u8; 7];
        let mut w = BitWriter::new(&mut me);
        original.encode(&mut w).unwrap();

        let mut r = BitReader::new(&me);
        let type_code = r.read_u8(5).unwrap();
        SurfacePosition::decode(type_code, &mut r).unwrap()
    }

    #[test]
    fn round_trips_a_surface_position() {
        let original = SurfacePosition {
            type_code: 7,
            movement_raw: 24,
            track_status: true,
            track_raw: 86,
            utc_synced: false,
            cpr_format: CprFormat::Even,
            lat_cpr: 11453,
            lon_cpr: 85668,
        };
        assert_eq!(round_trip(original), original);
    }

    // Real movement codes from pyModeS's own v2 corpus regression test
    // (test_movement_ranges_from_v2_corpus), each independently verified
    // against a real captured hex message.
    #[test]
    fn decode_movement_matches_real_reference_values() {
        assert_eq!(decode_movement(0), None);
        assert_eq!(decode_movement(1), Some(0.0));
        assert_eq!(decode_movement(9), Some(1.0));
        assert_eq!(decode_movement(24), Some(7.5));
        assert_eq!(decode_movement(25), Some(8.0));
        assert_eq!(decode_movement(39), Some(15.0));
        assert_eq!(decode_movement(94), Some(70.0));
        assert_eq!(decode_movement(109), Some(100.0));
        assert_eq!(decode_movement(124), Some(175.0));
        assert_eq!(decode_movement(125), None);
    }

    #[test]
    fn encode_movement_round_trips_bin_boundaries() {
        for mov in [1u8, 9, 24, 25, 39, 94, 109, 124] {
            let knots = decode_movement(mov).unwrap();
            assert_eq!(encode_movement(Some(knots)), mov);
        }
        assert_eq!(encode_movement(None), 0);
    }

    #[test]
    fn decodes_a_real_captured_taxiway_message() {
        // ME field of real hex frame 903a23ff426a38565950432ebf95 (ICAO
        // 3A23FF taxiing at LFBO): TC=8, mov=38, track_status=1,
        // track_raw=35, time=1, fmt=even, lat_cpr=11052, lon_cpr=86083.
        let me: [u8; 7] = [0x42, 0x6A, 0x38, 0x56, 0x59, 0x50, 0x43];
        let mut r = BitReader::new(&me);
        let type_code = r.read_u8(5).unwrap();
        assert_eq!(type_code, 8);
        let msg = SurfacePosition::decode(type_code, &mut r).unwrap();
        assert_eq!(msg.movement_raw, 38);
        assert!(msg.track_status);
        assert_eq!(msg.track_raw, 35);
        assert_eq!(msg.cpr_format, CprFormat::Even);
        assert_eq!(msg.lat_cpr, 11052);
        assert_eq!(msg.lon_cpr, 86083);
        assert!((msg.track_degrees().unwrap() - 35.0 * 360.0 / 128.0).abs() < 1e-9);
    }

    #[test]
    fn surface_position_pair_resolves_a_real_taxiway_pair() {
        let even = SurfacePosition {
            type_code: 8,
            movement_raw: 38,
            track_status: true,
            track_raw: 35,
            utc_synced: true,
            cpr_format: CprFormat::Even,
            lat_cpr: 11052,
            lon_cpr: 86083,
        };
        let odd = SurfacePosition {
            track_raw: 36,
            cpr_format: CprFormat::Odd,
            lat_cpr: 78587,
            lon_cpr: 84090,
            ..even
        };

        let mut pair = SurfacePositionPair::new();
        assert_eq!(pair.push(even, 43.63, 1.37), None);
        let (lat, lon) = pair.push(odd, 43.63, 1.37).unwrap();
        assert!((lat - 43.626_46).abs() < 1e-3);
        assert!((lon - 1.374_76).abs() < 1e-3);
    }
}