squitter 0.1.1

//! Airborne Velocity message — type code 19.
//!
//! Field layout and decode formulas cross-checked against pyModeS's
//! `decoder/bds/bds09.py`.

use crate::bits::{BitReader, BitWriter};
use crate::error::{BitError, MessageError};

/// `core`-only square root for f64 (no `libm` dependency). Newton-Raphson;
/// a fixed 20 iterations converges to full `f64` precision for the input
/// range this module ever computes over (velocity magnitudes up to a few
/// thousand knots), verified against known values in this module's tests.
fn sqrt(x: f64) -> f64 {
    if x <= 0.0 {
        return 0.0;
    }
    let mut guess = x;
    for _ in 0..20 {
        guess = f64::midpoint(guess, x / guess);
    }
    guess
}

/// Subtype-specific velocity data: either ground speed (subtypes 1-2) or
/// airspeed (subtypes 3-4).
///
/// Ground speed's signed east-west/north-south components are exact (no
/// `libm` needed to decode them); combining them into a single
/// speed-and-track reading needs `sqrt`/`atan2` --
/// [`AirborneVelocity::ground_speed_knots`] hand-rolls a private
/// Newton-Raphson `sqrt` for the former, but a track-angle accessor needing
/// `atan2` is left as a TODO rather than approximated.
#[derive(Debug, Clone, Copy, PartialEq)]
pub enum VelocityData {
    /// Ground speed (subtypes 1 = subsonic, 2 = supersonic).
    GroundSpeed {
        /// Signed east-west velocity in knots (positive = east), or `None`
        /// if not available.
        east_west_knots: Option<i32>,
        /// Signed north-south velocity in knots (positive = north), or
        /// `None` if not available.
        north_south_knots: Option<i32>,
    },
    /// Airspeed (subtypes 3 = subsonic, 4 = supersonic).
    AirSpeed {
        /// Heading in decimal degrees, or `None` if not available.
        heading_degrees: Option<f64>,
        /// Whether `airspeed_knots` is true airspeed (`true`) or indicated
        /// airspeed (`false`).
        is_true_airspeed: bool,
        /// Airspeed in knots, or `None` if not available.
        airspeed_knots: Option<i32>,
    },
}

/// Airborne Velocity message (type code 19; 56 bits).
#[derive(Debug, Clone, Copy, PartialEq)]
pub struct AirborneVelocity {
    /// Subtype (`1` = subsonic ground speed, `2` = supersonic ground speed,
    /// `3` = subsonic airspeed, `4` = supersonic airspeed).
    pub subtype: u8,
    /// Whether the aircraft is about to change heading/speed/altitude.
    pub intent_change: bool,
    /// IFR capability flag.
    pub ifr_capability: bool,
    /// Navigation accuracy category for velocity (raw 3-bit value).
    pub nac_v: u8,
    /// Subtype-specific velocity data.
    pub velocity: VelocityData,
    /// Whether the vertical rate source is barometric (`true`) or GNSS
    /// (`false`).
    pub vr_source_barometric: bool,
    /// Vertical rate in feet/minute (positive = climb), or `None` if not
    /// available.
    pub vertical_rate_fpm: Option<i32>,
    /// Difference between GNSS height and barometric altitude, in feet
    /// (positive = GNSS above barometric), or `None` if not available.
    pub geo_minus_baro_feet: Option<i32>,
}

impl AirborneVelocity {
    /// Ground speed magnitude in knots, combining the signed
    /// east-west/north-south components via the Pythagorean theorem. Only
    /// meaningful for ground-speed subtypes (1-2); `None` for airspeed
    /// subtypes or if either component is unavailable.
    #[must_use]
    pub fn ground_speed_knots(self) -> Option<f64> {
        let VelocityData::GroundSpeed {
            east_west_knots: Some(ew),
            north_south_knots: Some(ns),
        } = self.velocity
        else {
            return None;
        };
        Some(sqrt(f64::from(ew * ew + ns * ns)))
    }

    pub(crate) fn decode(r: &mut BitReader<'_>) -> Result<Self, MessageError> {
        let subtype = r.read_u8(3)?;
        let intent_change = r.read_bool()?;
        let ifr_capability = r.read_bool()?;
        let nac_v = r.read_u8(3)?;

        let velocity = match subtype {
            1 | 2 => {
                let scale = i32::from(subtype == 2) * 3 + 1; // 1 or 4
                let ew_sign = r.read_bool()?;
                let ew_mag = r.read_u16(10)?;
                let ns_sign = r.read_bool()?;
                let ns_mag = r.read_u16(10)?;
                let east_west_knots = (ew_mag != 0).then(|| {
                    let v = i32::from(ew_mag - 1) * scale;
                    if ew_sign { -v } else { v }
                });
                let north_south_knots = (ns_mag != 0).then(|| {
                    let v = i32::from(ns_mag - 1) * scale;
                    if ns_sign { -v } else { v }
                });
                VelocityData::GroundSpeed {
                    east_west_knots,
                    north_south_knots,
                }
            }
            3 | 4 => {
                let scale = i32::from(subtype == 4) * 3 + 1; // 1 or 4
                let heading_status = r.read_bool()?;
                let heading_raw = r.read_u16(10)?;
                let is_true_airspeed = r.read_bool()?;
                let airspeed_mag = r.read_u16(10)?;
                let heading_degrees =
                    heading_status.then(|| f64::from(heading_raw) / 1024.0 * 360.0);
                let airspeed_knots =
                    (airspeed_mag != 0).then(|| i32::from(airspeed_mag - 1) * scale);
                VelocityData::AirSpeed {
                    heading_degrees,
                    is_true_airspeed,
                    airspeed_knots,
                }
            }
            other => return Err(MessageError::UnknownVelocitySubtype(other)),
        };

        let vr_source_barometric = r.read_bool()?;
        let vr_sign = r.read_bool()?;
        let vr_mag = r.read_u16(9)?;
        let vertical_rate_fpm = (vr_mag != 0).then(|| {
            let v = i32::from(vr_mag - 1) * 64;
            if vr_sign { -v } else { v }
        });

        r.skip(2)?; // reserved

        let diff_sign = r.read_bool()?;
        let diff_mag = r.read_u8(7)?;
        let geo_minus_baro_feet = (diff_mag != 0 && diff_mag != 127).then(|| {
            let v = i32::from(diff_mag - 1) * 25;
            if diff_sign { -v } else { v }
        });

        Ok(Self {
            subtype,
            intent_change,
            ifr_capability,
            nac_v,
            velocity,
            vr_source_barometric,
            vertical_rate_fpm,
            geo_minus_baro_feet,
        })
    }

    pub(crate) fn encode(&self, w: &mut BitWriter<'_>) -> Result<(), BitError> {
        w.write_bits(19, 5)?;
        w.write_bits(u64::from(self.subtype), 3)?;
        w.write_bool(self.intent_change)?;
        w.write_bool(self.ifr_capability)?;
        w.write_bits(u64::from(self.nac_v), 3)?;

        match self.velocity {
            VelocityData::GroundSpeed {
                east_west_knots,
                north_south_knots,
            } => {
                let scale = i32::from(self.subtype == 2) * 3 + 1;
                write_signed_component(w, east_west_knots, scale)?;
                write_signed_component(w, north_south_knots, scale)?;
            }
            VelocityData::AirSpeed {
                heading_degrees,
                is_true_airspeed,
                airspeed_knots,
            } => {
                let scale = i32::from(self.subtype == 4) * 3 + 1;
                if let Some(deg) = heading_degrees {
                    w.write_bool(true)?;
                    #[allow(
                        clippy::cast_sign_loss,
                        clippy::cast_possible_truncation,
                        reason = "deg is in 0.0..360.0 by construction, so deg/360*1024 fits u16"
                    )]
                    w.write_bits(u64::from((deg / 360.0 * 1024.0) as u16), 10)?;
                } else {
                    w.write_bool(false)?;
                    w.write_bits(0, 10)?;
                }
                w.write_bool(is_true_airspeed)?;
                let mag = airspeed_knots.map_or(0, |v| v / scale + 1);
                #[allow(
                    clippy::cast_sign_loss,
                    clippy::cast_possible_truncation,
                    reason = "airspeed_knots is always non-negative and small by construction"
                )]
                w.write_bits(u64::from(mag as u16), 10)?;
            }
        }

        w.write_bool(self.vr_source_barometric)?;
        let (vr_sign, vr_mag) = signed_to_raw(self.vertical_rate_fpm, 64);
        w.write_bool(vr_sign)?;
        w.write_bits(u64::from(vr_mag), 9)?;

        w.write_bits(0, 2)?; // reserved

        let (diff_sign, diff_mag) = signed_to_raw(self.geo_minus_baro_feet, 25);
        w.write_bool(diff_sign)?;
        w.write_bits(u64::from(diff_mag), 7)?;

        Ok(())
    }
}

fn write_signed_component(
    w: &mut BitWriter<'_>,
    knots: Option<i32>,
    scale: i32,
) -> Result<(), BitError> {
    let sign = knots.is_some_and(|v| v < 0);
    let mag = knots.map_or(0, |v| v.abs() / scale + 1);
    w.write_bool(sign)?;
    #[allow(
        clippy::cast_sign_loss,
        clippy::cast_possible_truncation,
        reason = "mag is always non-negative and small by construction"
    )]
    w.write_bits(u64::from(mag as u16), 10)
}

/// Converts a signed decoded value back to `(sign_bit, raw_magnitude)`,
/// inverting `(raw - 1) * unit` (and its sign flip). `0` maps to
/// `(false, 0)` ("not available"), matching the decode side.
fn signed_to_raw(value: Option<i32>, unit: i32) -> (bool, u16) {
    let Some(value) = value else {
        return (false, 0);
    };
    let sign = value < 0;
    #[allow(
        clippy::cast_sign_loss,
        clippy::cast_possible_truncation,
        reason = "value.abs() is always non-negative and small by construction"
    )]
    let mag = (value.abs() / unit + 1) as u16;
    (sign, mag)
}

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

    fn round_trip(original: AirborneVelocity) -> AirborneVelocity {
        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();
        assert_eq!(type_code, 19);
        AirborneVelocity::decode(&mut r).unwrap()
    }

    #[test]
    fn round_trips_ground_speed() {
        let original = AirborneVelocity {
            subtype: 1,
            intent_change: false,
            ifr_capability: true,
            nac_v: 0,
            velocity: VelocityData::GroundSpeed {
                east_west_knots: Some(-8),
                north_south_knots: Some(-159),
            },
            vr_source_barometric: false,
            vertical_rate_fpm: Some(-832),
            geo_minus_baro_feet: Some(550),
        };
        assert_eq!(round_trip(original), original);
    }

    #[test]
    fn round_trips_airspeed_with_zero_heading() {
        let original = AirborneVelocity {
            subtype: 3,
            intent_change: false,
            ifr_capability: false,
            nac_v: 0,
            velocity: VelocityData::AirSpeed {
                heading_degrees: Some(0.0),
                is_true_airspeed: false,
                airspeed_knots: Some(100),
            },
            vr_source_barometric: false,
            vertical_rate_fpm: Some(1024),
            geo_minus_baro_feet: None,
        };
        assert_eq!(round_trip(original), original);
    }

    #[test]
    fn round_trips_all_fields_unavailable() {
        let original = AirborneVelocity {
            subtype: 1,
            intent_change: false,
            ifr_capability: false,
            nac_v: 0,
            velocity: VelocityData::GroundSpeed {
                east_west_knots: None,
                north_south_knots: None,
            },
            vr_source_barometric: true,
            vertical_rate_fpm: None,
            geo_minus_baro_feet: None,
        };
        assert_eq!(round_trip(original), original);
    }

    #[test]
    fn rejects_reserved_subtypes() {
        let mut me = [0u8; 7];
        let mut w = BitWriter::new(&mut me);
        w.write_bits(19, 5).unwrap();
        w.write_bits(0, 3).unwrap(); // subtype 0, reserved
        let mut r = BitReader::new(&me);
        r.read_u8(5).unwrap();
        assert_eq!(
            AirborneVelocity::decode(&mut r).unwrap_err(),
            MessageError::UnknownVelocitySubtype(0)
        );
    }

    #[test]
    fn sqrt_matches_known_values() {
        assert!((sqrt(4.0) - 2.0).abs() < 1e-9);
        assert!((sqrt(2.0) - core::f64::consts::SQRT_2).abs() < 1e-9);
        assert!((sqrt(0.0) - 0.0).abs() < 1e-9);
        assert!((sqrt(25_281.0) - 159.0).abs() < 1e-6); // 159^2 = 25281
    }

    // Real captured hex frame 8D485020994409940838175B284F, ME field
    // extracted and independently verified against pyModeS's golden v2
    // corpus: subtype=1, v_we=-8, v_sn=-159, groundspeed=159,
    // track~=182.88, vertical_rate=-832 (GNSS source), geo_minus_baro=550.
    #[test]
    fn decodes_a_real_captured_ground_speed_message() {
        let me: [u8; 7] = [0x99, 0x44, 0x09, 0x94, 0x08, 0x38, 0x17];
        let mut r = BitReader::new(&me);
        let type_code = r.read_u8(5).unwrap();
        assert_eq!(type_code, 19);
        let msg = AirborneVelocity::decode(&mut r).unwrap();
        assert_eq!(msg.subtype, 1);
        let VelocityData::GroundSpeed {
            east_west_knots,
            north_south_knots,
        } = msg.velocity
        else {
            panic!("expected GroundSpeed");
        };
        assert_eq!(east_west_knots, Some(-8));
        assert_eq!(north_south_knots, Some(-159));
        assert!((msg.ground_speed_knots().unwrap() - 159.2).abs() < 0.1);
        assert!(!msg.vr_source_barometric);
        assert_eq!(msg.vertical_rate_fpm, Some(-832));
        assert_eq!(msg.geo_minus_baro_feet, Some(550));
    }
}