rusty-telemetry/src/parser_ksudp.rs

//! Binary parser for KSUDP RTCarInfo packets (Assetto Corsa port 9996).
//!
//! After the 3-phase handshake, AC sends continuous RTCarInfo structs at ~20 Hz.
//! This parser converts the raw binary data into a `TelemetryFrame`.
//!
//! IMPORTANT: The AC RTCarInfo C struct uses **natural alignment** (not packed).
//! This means padding bytes exist:
//!   - 3 bytes after `char identifier` (offset 0) before `int size` (offset 4)
//!   - 2 bytes after the 6 bools (offsets 20–25) before the next float (offset 28)
//! Total struct size: 328 bytes (not 323).

use crate::binary_utils::{read_bool, read_f32, read_f32_4, read_i32};
use crate::telemetry_frame::{FeedSource, TelemetryFrame};

/// Minimum expected packet size for a naturally-aligned RTCarInfo struct.
/// The `size` field in the packet itself reports the true struct size (328).
const MIN_PACKET_SIZE: usize = 328;

/// Parse error type for KSUDP packets.
#[derive(Debug)]
pub enum KsudpParseError {
    /// Packet too short to contain a valid RTCarInfo.
    TooShort { actual: usize, expected: usize },
    /// Invalid identifier byte (expected 0x61 = 'a').
    InvalidIdentifier { got: u8 },
}

impl std::fmt::Display for KsudpParseError {
    fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
        match self {
            KsudpParseError::TooShort { actual, expected } => {
                write!(f, "packet too short: {} bytes (expected >= {})", actual, expected)
            }
            KsudpParseError::InvalidIdentifier { got } => {
                write!(f, "invalid identifier: 0x{:02x} (expected 0x61 = 'a')", got)
            }
        }
    }
}

impl std::error::Error for KsudpParseError {}

/// Parse a raw KSUDP RTCarInfo packet into a `TelemetryFrame`.
///
/// Field offsets follow the naturally-aligned C struct layout:
///   - offset 0:  char identifier
///   - offset 4:  int size  (3 bytes padding after char)
///   - offset 8:  float speedKmh
///   - offset 12: float speedMph
///   - offset 16: float speedMs
///   - offset 20: bool isAbsEnabled .. isEngineLimiterOn (6 bools at 20-25)
///   - offset 28: float accG_vertical  (2 bytes padding after bools)
///   - offset 32: float accG_horizontal
///   - offset 36: float accG_frontal
///   - offset 40: int lapTime
///   - offset 44: int lastLap
///   - offset 48: int bestLap
///   - offset 52: int lapCount
///   - offset 56: float gas
///   - offset 60: float brake
///   - offset 64: float fuel  (not clutch — KSUDP has no clutch input)
///   - offset 68: float engineRPM
///   - offset 72: float steer
///   - offset 76: int gear
///   - offset 80: float cgHeight
///   - offset 84:  float[4] wheelAngularSpeed
///   - offset 100: float[4] slipAngle
///   - offset 116: float[4] slipAngleContactPatch
///   - offset 132: float[4] slipRatio
///   - offset 148: float[4] tyreSlip
///   - offset 164: float[4] ndSlip
///   - offset 180: float[4] wheelLoad
///   - offset 196: float[4] wheelDy
///   - offset 212: float[4] wheelMz
///   - offset 228: float[4] tyreDirtyLevel
///   - offset 244: float[4] camberRAD
///   - offset 260: float[4] tyreRadius
///   - offset 276: float[4] tyreLoadedRadius
///   - offset 292: float[4] suspensionHeight
///   - offset 308: float carPositionNormalized
///   - offset 312: float carSlope
///   - offset 316: float worldX
///   - offset 320: float worldY
///   - offset 324: float worldZ
///
/// # Errors
/// Returns `KsudpParseError` if the packet is too short or has an invalid identifier.
pub fn parse_rt_car_info(data: &[u8]) -> Result<TelemetryFrame, KsudpParseError> {
    if data.len() < MIN_PACKET_SIZE {
        return Err(KsudpParseError::TooShort {
            actual: data.len(),
            expected: MIN_PACKET_SIZE,
        });
    }

    let identifier = data[0];
    if identifier != b'a' {
        return Err(KsudpParseError::InvalidIdentifier { got: identifier });
    }

    let reported_size = read_i32(data, 4) as usize;
    if data.len() < reported_size && reported_size > 0 {
        eprintln!(
            "[WARN] KSUDP: reported struct size {} but packet is {} bytes",
            reported_size,
            data.len()
        );
    }

    let frame = TelemetryFrame {
        source: FeedSource::Ksudp,
        raw_size: data.len(),

        speed_kmh: read_f32(data, 8),
        speed_mph: read_f32(data, 12),
        speed_ms: read_f32(data, 16),

        is_abs_enabled: read_bool(data, 20),
        is_abs_in_action: read_bool(data, 21),
        is_tc_in_action: read_bool(data, 22),
        is_tc_enabled: read_bool(data, 23),
        is_in_pit: read_bool(data, 24),
        is_engine_limiter_on: read_bool(data, 25),

        acc_g_vertical: read_f32(data, 28),
        acc_g_horizontal: read_f32(data, 32),
        acc_g_frontal: read_f32(data, 36),

        lap_time_ms: read_i32(data, 40),
        last_lap_ms: read_i32(data, 44),
        best_lap_ms: read_i32(data, 48),
        lap_count: read_i32(data, 52),

        throttle: read_f32(data, 56),
        brake: read_f32(data, 60),
        clutch: 0.0, // offset 64 is fuel level (0.0-1.0), not clutch; KSUDP has no clutch input
        engine_rpm: read_f32(data, 68),
        steer: read_f32(data, 72),

        gear: read_i32(data, 76),
        cg_height: read_f32(data, 80),

        wheel_angular_speed: read_f32_4(data, 84),
        slip_angle: read_f32_4(data, 100),
        slip_angle_contact_patch: read_f32_4(data, 116),
        slip_ratio: read_f32_4(data, 132),
        tyre_slip: read_f32_4(data, 148),
        nd_slip: read_f32_4(data, 164),
        wheel_load: read_f32_4(data, 180),
        wheel_dy: read_f32_4(data, 196),
        wheel_mz: read_f32_4(data, 212),
        tyre_dirty_level: read_f32_4(data, 228),
        camber_rad: read_f32_4(data, 244),
        tyre_radius: read_f32_4(data, 260),
        tyre_loaded_radius: read_f32_4(data, 276),
        suspension_height: read_f32_4(data, 292),

        car_position_normalized: read_f32(data, 308),
        car_slope: read_f32(data, 312),
        world_x: read_f32(data, 316),
        world_y: read_f32(data, 320),
        world_z: read_f32(data, 324),
        spline_position: read_f32(data, 308),

        ..Default::default()
    };

    Ok(frame)
}

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

    #[test]
    fn test_parse_minimal_valid_packet() {
        let mut packet = vec![0u8; MIN_PACKET_SIZE];
        packet[0] = b'a';
        packet[4..8].copy_from_slice(&(MIN_PACKET_SIZE as i32).to_le_bytes());
        packet[8..12].copy_from_slice(&123.4f32.to_le_bytes());
        packet[76..80].copy_from_slice(&3i32.to_le_bytes());
        packet[68..72].copy_from_slice(&7500.0f32.to_le_bytes());
        packet[40..44].copy_from_slice(&78123i32.to_le_bytes());

        let frame = parse_rt_car_info(&packet).unwrap();
        assert_eq!(frame.source, FeedSource::Ksudp);
        assert!((frame.speed_kmh - 123.4).abs() < 0.01);
        assert_eq!(frame.gear, 3);
        assert!((frame.engine_rpm - 7500.0).abs() < 0.01);
        assert_eq!(frame.lap_time_ms, 78123);
    }

    #[test]
    fn test_too_short() {
        let packet = vec![b'a', 0, 0, 0, 0];
        let result = parse_rt_car_info(&packet);
        assert!(result.is_err());
        if let Err(KsudpParseError::TooShort { actual, .. }) = result {
            assert_eq!(actual, 5);
        } else {
            panic!("Expected TooShort error");
        }
    }

    #[test]
    fn test_invalid_identifier() {
        let mut packet = vec![0u8; MIN_PACKET_SIZE];
        packet[0] = b'x';
        let result = parse_rt_car_info(&packet);
        assert!(result.is_err());
        if let Err(KsudpParseError::InvalidIdentifier { got }) = result {
            assert_eq!(got, b'x');
        } else {
            panic!("Expected InvalidIdentifier error");
        }
    }

    #[test]
    fn test_boolean_flags() {
        let mut packet = vec![0u8; MIN_PACKET_SIZE];
        packet[0] = b'a';
        packet[20] = 1;
        packet[21] = 0;
        packet[22] = 1;
        packet[23] = 1;
        packet[24] = 0;
        packet[25] = 1;

        let frame = parse_rt_car_info(&packet).unwrap();
        assert!(frame.is_abs_enabled);
        assert!(!frame.is_abs_in_action);
        assert!(frame.is_tc_in_action);
        assert!(frame.is_tc_enabled);
        assert!(!frame.is_in_pit);
        assert!(frame.is_engine_limiter_on);
    }

    #[test]
    fn test_per_wheel_arrays() {
        let mut packet = vec![0u8; MIN_PACKET_SIZE];
        packet[0] = b'a';
        for i in 0..4 {
            let val = (i as f32 + 1.0) * 0.1;
            packet[132 + i * 4..132 + i * 4 + 4].copy_from_slice(&val.to_le_bytes());
        }

        let frame = parse_rt_car_info(&packet).unwrap();
        assert!((frame.slip_ratio[0] - 0.1).abs() < 0.001);
        assert!((frame.slip_ratio[1] - 0.2).abs() < 0.001);
        assert!((frame.slip_ratio[2] - 0.3).abs() < 0.001);
        assert!((frame.slip_ratio[3] - 0.4).abs() < 0.001);
    }

    #[test]
    fn test_world_coordinates() {
        let mut packet = vec![0u8; MIN_PACKET_SIZE];
        packet[0] = b'a';
        packet[316..320].copy_from_slice(&100.5f32.to_le_bytes());
        packet[320..324].copy_from_slice(&2.3f32.to_le_bytes());
        packet[324..328].copy_from_slice(&(-50.7f32).to_le_bytes());

        let frame = parse_rt_car_info(&packet).unwrap();
        assert!((frame.world_x - 100.5).abs() < 0.01);
        assert!((frame.world_y - 2.3).abs() < 0.01);
        assert!((frame.world_z - (-50.7)).abs() < 0.01);
    }
}