// rusty-telemetry — Sim racing telemetry analysis platform
// Copyright (C) 2026  Jan-Peter v. Hunnius
//
// This program is free software: you can redistribute it and/or modify
// it under the terms of the GNU Affero General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// (at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU Affero General Public License for more details.
//
// You should have received a copy of the GNU Affero General Public License
// along with this program.  If not, see <https://www.gnu.org/licenses/>.
//! Binary parser for Project CARS 1 & 2 UDP telemetry packets (port 5606).
//!
//! Both PCARS1 and PCARS2 broadcast on port 5606. The parser auto-detects the
//! format based on packet structure:
//! - PCARS 1 (Format 1): ~1367 bytes, 3-byte header (u16 build + packed u8)
//! - PCARS 2 (Format 2): 12-byte header with packet_type at offset 10

use crate::binary_utils::*;
use crate::telemetry_frame::{FeedSource, TelemetryFrame, NUM_WHEELS};

// ── PCARS 1 constants ──────────────────────────────────────────────────
const V1_TELEMETRY_SIZE: usize = 1367;
const V1_PACKET_TYPE_TELEMETRY: u8 = 0;

// ── PCARS 2 constants ──────────────────────────────────────────────────
const V2_HEADER_SIZE: usize = 12;
const V2_CAR_PHYSICS_SIZE: usize = 559;
const V2_PACKET_TYPE_CAR_PHYSICS: u8 = 0;

/// Parse error type for PCARS packets.
#[derive(Debug)]
pub enum PcarsParseError {
    /// Packet too short.
    TooShort { actual: usize, needed: usize },
}

impl std::fmt::Display for PcarsParseError {
    fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
        match self {
            PcarsParseError::TooShort { actual, needed } => {
                write!(f, "packet too short: {} bytes (needed {})", actual, needed)
            }
        }
    }
}

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

/// Parsed result of a PCARS packet.
pub enum PcarsPacket {
    /// PCARS 1 telemetry data (Format 1, type 0).
    V1Telemetry(TelemetryFrame),
    /// PCARS 2 car physics data (Format 2, type 0).
    V2CarPhysics(TelemetryFrame),
    /// PCARS 2 non-telemetry packet (timings, game state, etc.) — ignored.
    V2Other,
    /// Unrecognised packet.
    Unknown,
}

/// Read a single byte as u8 at the given offset.
fn read_u8_at(data: &[u8], offset: usize) -> u8 {
    data.get(offset).copied().unwrap_or(0)
}

/// Read a single byte as i8 at the given offset.
fn read_i8_at(data: &[u8], offset: usize) -> i8 {
    data.get(offset).copied().unwrap_or(0) as i8
}

/// Read 4 consecutive u8 values starting at offset.
fn read_u8_4(data: &[u8], offset: usize) -> [u8; NUM_WHEELS] {
    let mut arr = [0u8; NUM_WHEELS];
    for i in 0..NUM_WHEELS {
        arr[i] = read_u8_at(data, offset + i);
    }
    arr
}

/// Read 4 consecutive i16 values starting at offset.
fn read_i16_4(data: &[u8], offset: usize) -> [i16; NUM_WHEELS] {
    let mut arr = [0i16; NUM_WHEELS];
    for i in 0..NUM_WHEELS {
        arr[i] = read_i16(data, offset + i * 2);
    }
    arr
}

/// Normalize u8 (0–255) to f32 (0.0–1.0).
fn normalize_u8(v: u8) -> f32 {
    v as f32 / 255.0
}

/// Normalize int8 (-127..127) to f32 (-1.0..1.0).
fn normalize_i8(v: i8) -> f32 {
    v as f32 / 127.0
}

/// Auto-detect and parse a PCARS UDP packet.
pub fn parse_pcars_packet(data: &[u8]) -> Result<PcarsPacket, PcarsParseError> {
    if data.len() < 3 {
        return Err(PcarsParseError::TooShort { actual: data.len(), needed: 3 });
    }

    // Detection strategy:
    // PCARS 1 telemetry packets are ~1367 bytes — much larger than any V2 packet.
    // PCARS 2 packets have a 12-byte header with packet_type at offset 10.
    //
    // Check V1 first for large packets (V2 never produces packets > ~1100 bytes).

    // Try V1 format: check packet type from packed byte at offset 2
    let seq_type = read_u8_at(data, 2);
    let v1_packet_type = seq_type & 0x03;

    if v1_packet_type == V1_PACKET_TYPE_TELEMETRY && data.len() >= V1_TELEMETRY_SIZE {
        return Ok(PcarsPacket::V1Telemetry(parse_v1_telemetry(data)?));
    }

    // Try V2 format: 12-byte header with packet_type at offset 10
    if data.len() >= V2_HEADER_SIZE {
        let packet_type = read_u8_at(data, 10);

        if packet_type <= 8 {
            match packet_type {
                V2_PACKET_TYPE_CAR_PHYSICS if data.len() >= V2_CAR_PHYSICS_SIZE => {
                    return Ok(PcarsPacket::V2CarPhysics(parse_v2_car_physics(data)?));
                }
                _ => {
                    return Ok(PcarsPacket::V2Other);
                }
            }
        }
    }

    Ok(PcarsPacket::Unknown)
}

/// Parse a PCARS 1 (Format 1) telemetry packet (1367 bytes).
fn parse_v1_telemetry(data: &[u8]) -> Result<TelemetryFrame, PcarsParseError> {
    if data.len() < V1_TELEMETRY_SIZE {
        return Err(PcarsParseError::TooShort { actual: data.len(), needed: V1_TELEMETRY_SIZE });
    }

    let speed_ms = read_f32(data, 120);
    let speed_kmh = speed_ms * 3.6;

    // Timing (f32 seconds → i32 ms)
    let best_lap_s = read_f32(data, 12);
    let last_lap_s = read_f32(data, 16);
    let current_lap_s = read_f32(data, 20);

    // Gear: packed byte at 128, gear in LSB 4 bits
    let gear_packed = read_u8_at(data, 128);
    let gear = (gear_packed & 0x0F) as i32;

    // Inputs (u8 0-255, normalize to 0-1)
    let throttle = normalize_u8(read_u8_at(data, 113));
    let brake = normalize_u8(read_u8_at(data, 112));
    let clutch = normalize_u8(read_u8_at(data, 114));
    let steer = normalize_i8(read_i8_at(data, 115));

    let rpm = read_u16(data, 124) as f32;

    // Accelerations (local_acceleration at offset 184)
    let acc_vert = read_f32(data, 184);
    let acc_horiz = read_f32(data, 188);
    let acc_front = read_f32(data, 192);

    // Engine & damage
    let aero_damage = read_u8_at(data, 456);
    let engine_damage = read_u8_at(data, 457);
    let ambient_temp = read_i8_at(data, 458);
    let track_temp = read_i8_at(data, 459);
    let rain_density = read_u8_at(data, 460);

    // Track length
    let track_length = read_f32(data, 1360);

    // V1 position data from viewed participant
    let viewed_idx = read_i8_at(data, 4) as usize;
    let participant_offset = 464 + viewed_idx * 16;
    let v1_world_x = read_i16(data, participant_offset) as f32;
    let v1_world_y = read_i16(data, participant_offset + 2) as f32;
    let v1_world_z = read_i16(data, participant_offset + 4) as f32;
    let current_lap_distance = read_u16(data, participant_offset + 6);
    let spline_pos = if track_length > 0.0 {
        (current_lap_distance as f32 / track_length).clamp(0.0, 1.0)
    } else {
        0.0
    };
    let laps_completed = (read_u8_at(data, participant_offset + 9) & 0x7F) as i32;
    let _current_lap = read_u8_at(data, participant_offset + 10) as i32;

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

        speed_kmh,
        speed_mph: speed_ms * 2.236936,
        speed_ms,

        engine_rpm: rpm,
        gear,

        throttle,
        brake,
        clutch,
        steer,

        lap_time_ms: (current_lap_s * 1000.0) as i32,
        last_lap_ms: (last_lap_s * 1000.0) as i32,
        best_lap_ms: (best_lap_s * 1000.0) as i32,
        lap_count: laps_completed,

        world_x: v1_world_x,
        world_y: v1_world_y,
        world_z: v1_world_z,
        spline_position: spline_pos,

        acc_g_vertical: acc_vert,
        acc_g_horizontal: acc_horiz,
        acc_g_frontal: acc_front,

        is_abs_enabled: false,
        is_abs_in_action: false,
        is_tc_enabled: false,
        is_tc_in_action: false,
        is_in_pit: false,
        is_engine_limiter_on: false,

        // Per-wheel
        wheel_angular_speed: read_f32_4(data, 244), // tyre_rps
        slip_angle: [0.0; NUM_WHEELS],
        slip_ratio: [0.0; NUM_WHEELS],
        tyre_slip: [0.0; NUM_WHEELS],
        nd_slip: [0.0; NUM_WHEELS],
        suspension_height: read_f32_4(data, 408),   // suspension_travel
        tyre_dirty_level: [0.0; NUM_WHEELS],

        // KSUDP-specific (defaults)
        slip_angle_contact_patch: [0.0; NUM_WHEELS],
        wheel_load: [0.0; NUM_WHEELS],
        wheel_dy: read_f32_4(data, 228),             // tyre_y
        wheel_mz: [0.0; NUM_WHEELS],
        camber_rad: [0.0; NUM_WHEELS],
        tyre_radius: [0.0; NUM_WHEELS],
        tyre_loaded_radius: [0.0; NUM_WHEELS],
        cg_height: 0.0,
        car_slope: 0.0,
        car_position_normalized: 0.0,

        // PCARS-specific
        oil_temp_celsius: Some(read_i16(data, 100)),
        water_temp_celsius: Some(read_i16(data, 104)),
        fuel_capacity: Some(read_u8_at(data, 111)),
        boost_amount: Some(read_u8_at(data, 129)),
        crash_state: Some(read_u8_at(data, 131) & 0x07),
        odometer_km: Some(read_f32(data, 132)),
        aero_damage: Some(aero_damage),
        engine_damage: Some(engine_damage),
        ambient_temperature: Some(ambient_temp),
        track_temperature: Some(track_temp),
        rain_density: Some(rain_density),
        track_length: Some(track_length),
        full_position: Some([v1_world_x, v1_world_y, v1_world_z]),
        current_lap_distance: Some(current_lap_distance),
        tyre_wear: Some(read_u8_4(data, 316)),
        brake_temp_celsius: Some(read_i16_4(data, 328)),
        suspension_damage: Some(read_u8_4(data, 324)),

        ..Default::default()
    };

    Ok(frame)
}

/// Parse a PCARS 2 (Format 2) car physics packet (559 bytes).
fn parse_v2_car_physics(data: &[u8]) -> Result<TelemetryFrame, PcarsParseError> {
    if data.len() < V2_CAR_PHYSICS_SIZE {
        return Err(PcarsParseError::TooShort { actual: data.len(), needed: V2_CAR_PHYSICS_SIZE });
    }

    let speed_ms = read_f32(data, 36);
    let speed_kmh = speed_ms * 3.6;

    // Gear: packed byte at 45, gear in LSB 4 bits
    let gear_packed = read_u8_at(data, 45);
    let gear = (gear_packed & 0x0F) as i32;

    // Inputs
    let throttle = normalize_u8(read_u8_at(data, 30));
    let brake = normalize_u8(read_u8_at(data, 29));
    let clutch = normalize_u8(read_u8_at(data, 31));
    let steer = normalize_i8(read_i8_at(data, 44));

    let rpm = read_u16(data, 40) as f32;

    // Accelerations (local_acceleration at offset 100)
    let acc_vert = read_f32(data, 100);
    let acc_horiz = read_f32(data, 104);
    let acc_front = read_f32(data, 108);

    // Full 3D position (PCARS2 only, offset 542)
    let full_pos = read_f32_3(data, 542);

    // Tyre compounds (4 × 40 chars at offset 378)
    let tyre_compound_str = read_string(data, 378, 40);

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

        speed_kmh,
        speed_mph: speed_ms * 2.236936,
        speed_ms,

        engine_rpm: rpm,
        gear,

        throttle,
        brake,
        clutch,
        steer,

        lap_time_ms: 0, // V2 timing is in a separate packet (type 3)
        last_lap_ms: 0,
        best_lap_ms: 0,
        lap_count: 0,

        world_x: full_pos[0],
        world_y: full_pos[1],
        world_z: full_pos[2],
        spline_position: 0.0,

        acc_g_vertical: acc_vert,
        acc_g_horizontal: acc_horiz,
        acc_g_frontal: acc_front,

        is_abs_enabled: false,
        is_abs_in_action: false,
        is_tc_enabled: false,
        is_tc_in_action: false,
        is_in_pit: false,
        is_engine_limiter_on: false,

        // Per-wheel
        wheel_angular_speed: read_f32_4(data, 160),  // tyre_rps
        slip_angle: [0.0; NUM_WHEELS],
        slip_ratio: [0.0; NUM_WHEELS],
        tyre_slip: [0.0; NUM_WHEELS],
        nd_slip: [0.0; NUM_WHEELS],
        suspension_height: read_f32_4(data, 312),    // suspension_travel
        tyre_dirty_level: [0.0; NUM_WHEELS],

        // KSUDP-specific (defaults)
        slip_angle_contact_patch: [0.0; NUM_WHEELS],
        wheel_load: [0.0; NUM_WHEELS],
        wheel_dy: read_f32_4(data, 144),              // tyre_y
        wheel_mz: [0.0; NUM_WHEELS],
        camber_rad: [0.0; NUM_WHEELS],
        tyre_radius: [0.0; NUM_WHEELS],
        tyre_loaded_radius: [0.0; NUM_WHEELS],
        cg_height: 0.0,
        car_slope: 0.0,
        car_position_normalized: 0.0,

        // Telemetry Tool-specific (none applicable)
        tyre_compound: if tyre_compound_str.is_empty() { None } else { Some(tyre_compound_str) },
        turbo_boost: Some(read_f32(data, 538)),

        // PCARS-specific
        oil_temp_celsius: Some(read_i16(data, 18)),
        water_temp_celsius: Some(read_i16(data, 22)),
        fuel_capacity: Some(read_u8_at(data, 28)),
        boost_amount: Some(read_u8_at(data, 46)),
        crash_state: Some(read_u8_at(data, 47)),
        odometer_km: Some(read_f32(data, 48)),
        aero_damage: Some(read_u8_at(data, 371)),
        engine_damage: Some(read_u8_at(data, 372)),
        ambient_temperature: None,  // In Game State packet (type 4), not here
        track_temperature: None,
        rain_density: None,
        track_length: None,
        full_position: Some(full_pos),
        tyre_wear: Some(read_u8_4(data, 196)),
        brake_temp_celsius: Some(read_i16_4(data, 208)),
        suspension_damage: Some(read_u8_4(data, 204)),
        current_lap_distance: None,

        ..Default::default()
    };

    Ok(frame)
}

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

    #[test]
    fn test_v1_telemetry_packet() {
        let mut packet = vec![0u8; V1_TELEMETRY_SIZE];
        // Header: build_version = 1104, seq_type = 0 (type 0 = telemetry)
        packet[0..2].copy_from_slice(&1104u16.to_le_bytes());
        packet[2] = 0x00; // seq=0, type=0

        // Speed = 50.0 m/s at offset 120
        packet[120..124].copy_from_slice(&50.0f32.to_le_bytes());
        // RPM = 7200 at offset 124
        packet[124..126].copy_from_slice(&7200u16.to_le_bytes());
        // Gear: packed at offset 128, gear=3 (meaning gear 2)
        packet[128] = 0x30 | 0x03; // 3 gears-high, gear=3
        // Throttle at 113 = 200/255
        packet[113] = 200;
        // Brake at 112 = 0
        packet[112] = 0;
        // Best lap at 12 = 78.123 seconds
        packet[12..16].copy_from_slice(&78.123f32.to_le_bytes());
        // Last lap at 16 = 80.456 seconds
        packet[16..20].copy_from_slice(&80.456f32.to_le_bytes());
        // Current time at 20 = 42.5 seconds
        packet[20..24].copy_from_slice(&42.5f32.to_le_bytes());

        // World position for participant 0 at offset 464: x=100, y=5, z=-200
        packet[464..466].copy_from_slice(&100i16.to_le_bytes());
        packet[466..468].copy_from_slice(&5i16.to_le_bytes());
        packet[468..470].copy_from_slice(&(-200i16).to_le_bytes());
        // Current lap distance at offset 464+6 = 470: 2500 meters
        packet[470..472].copy_from_slice(&2500u16.to_le_bytes());
        // Track length at offset 1360: 5000.0 meters
        packet[1360..1364].copy_from_slice(&5000.0f32.to_le_bytes());

        let result = parse_pcars_packet(&packet).unwrap();
        match result {
            PcarsPacket::V1Telemetry(frame) => {
                assert_eq!(frame.source, FeedSource::Pcars);
                assert!((frame.speed_kmh - 180.0).abs() < 0.1);
                assert!((frame.speed_ms - 50.0).abs() < 0.01);
                assert!((frame.engine_rpm - 7200.0).abs() < 1.0);
                assert_eq!(frame.gear, 3);
                assert!((frame.throttle - (200.0 / 255.0)).abs() < 0.01);
                assert!((frame.brake - 0.0).abs() < 0.01);
                assert_eq!(frame.best_lap_ms, 78123);
                assert_eq!(frame.last_lap_ms, 80456);
                assert_eq!(frame.lap_time_ms, 42500);
                assert!((frame.world_x - 100.0).abs() < 0.1);
                assert!((frame.world_y - 5.0).abs() < 0.1);
                assert!((frame.world_z - (-200.0)).abs() < 0.1);
                assert!((frame.spline_position - 0.5).abs() < 0.01);
                assert!(frame.full_position.is_some());
                assert_eq!(frame.current_lap_distance, Some(2500));
                assert_eq!(frame.oil_temp_celsius, Some(0));
                assert!(frame.aero_damage.is_some());
            }
            _ => panic!("Expected V1Telemetry"),
        }
    }

    #[test]
    fn test_v2_car_physics_packet() {
        let mut packet = vec![0u8; V2_CAR_PHYSICS_SIZE];
        // Header
        packet[0..4].copy_from_slice(&1u32.to_le_bytes()); // packet_number
        packet[4..8].copy_from_slice(&1u32.to_le_bytes()); // category_packet_number
        packet[10] = V2_PACKET_TYPE_CAR_PHYSICS;            // packet_type = 0
        packet[11] = 1;                                     // packet_version

        // Speed = 60.0 m/s at offset 36
        packet[36..40].copy_from_slice(&60.0f32.to_le_bytes());
        // RPM = 8500 at offset 40
        packet[40..42].copy_from_slice(&8500u16.to_le_bytes());
        // Gear at offset 45: gear=4
        packet[45] = 0x40 | 0x04;
        // Throttle at offset 30 = 255
        packet[30] = 255;
        // Brake at offset 29 = 128
        packet[29] = 128;
        // Full position at offset 542
        packet[542..546].copy_from_slice(&100.0f32.to_le_bytes());
        packet[546..550].copy_from_slice(&5.0f32.to_le_bytes());
        packet[550..554].copy_from_slice(&(-200.0f32).to_le_bytes());

        let result = parse_pcars_packet(&packet).unwrap();
        match result {
            PcarsPacket::V2CarPhysics(frame) => {
                assert_eq!(frame.source, FeedSource::Pcars);
                assert!((frame.speed_kmh - 216.0).abs() < 0.1);
                assert!((frame.engine_rpm - 8500.0).abs() < 1.0);
                assert_eq!(frame.gear, 4);
                assert!((frame.throttle - 1.0).abs() < 0.01);
                assert!((frame.brake - (128.0 / 255.0)).abs() < 0.01);
                assert!((frame.world_x - 100.0).abs() < 0.01);
                assert!((frame.world_y - 5.0).abs() < 0.01);
                assert!((frame.world_z - (-200.0)).abs() < 0.01);
                assert!(frame.full_position.is_some());
                assert!(frame.turbo_boost.is_some());
            }
            _ => panic!("Expected V2CarPhysics"),
        }
    }

    #[test]
    fn test_too_short() {
        let result = parse_pcars_packet(&[0u8; 2]);
        assert!(result.is_err());
    }

    #[test]
    fn test_unknown_format() {
        // A packet that's too small for V1 (< 1367) and has byte[10] > 8 (not a valid V2 type)
        let mut packet = vec![0u8; 100];
        packet[2] = 0xFF; // V1 type bits = 3 (not telemetry)
        packet[10] = 9;   // V2 type = 9 (invalid)
        let result = parse_pcars_packet(&packet);
        assert!(matches!(result, Ok(PcarsPacket::Unknown)));

        // Very small packet with no recognizable format
        let mut p2 = vec![0u8; 50];
        p2[2] = 0xFF; // V1 type bits = 3 (not telemetry)
        p2[10] = 9;   // V2 type = 9 (invalid)
        let result2 = parse_pcars_packet(&p2);
        assert!(matches!(result2, Ok(PcarsPacket::Unknown)));
    }

    #[test]
    fn test_v2_other_packet_types() {
        let mut packet = vec![0u8; V2_HEADER_SIZE + 20];
        packet[0..4].copy_from_slice(&1u32.to_le_bytes());
        packet[10] = 3; // Timings packet
        let result = parse_pcars_packet(&packet);
        assert!(matches!(result, Ok(PcarsPacket::V2Other)));

        packet[10] = 4; // Game State
        let result = parse_pcars_packet(&packet);
        assert!(matches!(result, Ok(PcarsPacket::V2Other)));
    }

    #[test]
    fn test_v1_position_nonzero_viewed_participant() {
        let mut packet = vec![0u8; V1_TELEMETRY_SIZE];
        packet[0..2].copy_from_slice(&1104u16.to_le_bytes());
        packet[2] = 0x00;

        packet[4] = 3i8.to_le_bytes()[0];

        packet[120..124].copy_from_slice(&30.0f32.to_le_bytes());

        packet[1360..1364].copy_from_slice(&6000.0f32.to_le_bytes());

        let p_offset = 464 + 3 * 16;
        packet[p_offset..p_offset+2].copy_from_slice(&300i16.to_le_bytes());
        packet[p_offset+2..p_offset+4].copy_from_slice(&10i16.to_le_bytes());
        packet[p_offset+4..p_offset+6].copy_from_slice(&(-400i16).to_le_bytes());
        packet[p_offset+6..p_offset+8].copy_from_slice(&3000u16.to_le_bytes());

        let result = parse_pcars_packet(&packet).unwrap();
        match result {
            PcarsPacket::V1Telemetry(frame) => {
                assert!((frame.world_x - 300.0).abs() < 0.1);
                assert!((frame.world_y - 10.0).abs() < 0.1);
                assert!((frame.world_z - (-400.0)).abs() < 0.1);
                assert!((frame.spline_position - 0.5).abs() < 0.01);
                assert_eq!(frame.current_lap_distance, Some(3000));
            }
            _ => panic!("Expected V1Telemetry"),
        }
    }
}