//! Fixed-size local UDP records for Kaptan's host-only simulator harness.
//!
//! This transport is deliberately not MAVLink and not part of Kaptan's flight
//! dialect. It is confined to one host network namespace and is versioned so
//! malformed simulator traffic never becomes estimator or motor authority.

use kaptan_err_reportable::KaptanError;
use std::net::SocketAddr;

pub const SENSOR_PORT: u16 = 14_660;
pub const ACTUATOR_PORT: u16 = 14_661;
pub const CONTROL_PORT: u16 = 14_662;
pub const VERSION: u8 = 1;
pub const IMU_MAGIC: [u8; 4] = *b"KSI1";
pub const LOCAL_STATE_MAGIC: [u8; 4] = *b"KSV1";
/// Gazebo simulation time, sent once for every unpaused simulation step.
/// This is the sole time base for host-SITL freshness and command leases.
pub const CLOCK_MAGIC: [u8; 4] = *b"KSC1";
pub const OUTPUT_MAGIC: [u8; 4] = *b"KSO1";
pub const GOAL_MAGIC: [u8; 4] = *b"KSG1";
/// A host-SITL-only request to descend to a declared local landing point and
/// revoke motor authority once the landing point has settled.
pub const LAND_MAGIC: [u8; 4] = *b"KSD1";
pub const GOAL_KEEPALIVE_MAGIC: [u8; 4] = *b"KSK1";
pub const STATUS_QUERY_MAGIC: [u8; 4] = *b"KSQ1";
pub const STATUS_MAGIC: [u8; 4] = *b"KSS1";
pub const GOAL_STATUS_MAGIC: [u8; 4] = *b"KSL1";

pub const SENSOR_PACKET_SIZE: usize = 44;
pub const CLOCK_PACKET_SIZE: usize = 16;
pub const OUTPUT_PACKET_SIZE: usize = 40;
pub const GOAL_PACKET_SIZE: usize = 24;
pub const GOAL_KEEPALIVE_PACKET_SIZE: usize = 12;
pub const STATUS_QUERY_PACKET_SIZE: usize = 8;
pub const STATUS_PACKET_SIZE: usize = 20;
pub const GOAL_STATUS_PACKET_SIZE: usize = 16;

pub const STATUS_ESTIMATOR_READY: u16 = 1 << 0;
pub const STATUS_IMU_FRESH: u16 = 1 << 1;
pub const STATUS_LOCAL_AIDING_FRESH: u16 = 1 << 2;
pub const STATUS_OUTPUT_PERMITTED: u16 = 1 << 3;
pub const STATUS_ACTUATOR_TRANSPORT: u16 = 1 << 4;
#[derive(Clone, Copy, Debug, Eq, PartialEq, KaptanError)]
pub enum PacketError {
    #[kaptan_error(id = 12001)]
    Length,
    #[kaptan_error(id = 12002)]
    Version,
    #[kaptan_error(id = 12003)]
    NonFinite,
}

#[derive(Clone, Copy, Debug, PartialEq)]
pub struct ImuPacket {
    pub time_microseconds: u64,
    pub sequence: u32,
    pub acceleration_frd_mps2: [f32; 3],
    pub angular_rate_frd_rad_s: [f32; 3],
}

#[derive(Clone, Copy, Debug, PartialEq)]
pub struct LocalStatePacket {
    pub time_microseconds: u64,
    pub sequence: u32,
    pub position_ned_metres: [f32; 3],
    pub velocity_ned_mps: [f32; 3],
}

#[derive(Clone, Copy, Debug, Eq, PartialEq)]
pub struct ClockPacket {
    pub time_microseconds: u64,
}

#[derive(Clone, Copy, Debug, PartialEq)]
pub struct GoalPacket {
    pub goal_id: u32,
    pub north_metres: f32,
    pub east_metres: f32,
    pub up_metres: f32,
}

/// Renews the command lease for one accepted goal without modifying it.
#[derive(Clone, Copy, Debug, Eq, PartialEq)]
pub struct GoalKeepalivePacket {
    pub goal_id: u32,
}

#[derive(Clone, Copy, Debug, Eq, PartialEq)]
#[repr(u8)]
pub enum GoalLifecycle {
    Rejected = 0,
    Active = 1,
    Settled = 2,
    Landed = 3,
}

#[derive(Clone, Copy, Debug, Eq, PartialEq)]
pub struct GoalStatusPacket {
    pub goal_id: u32,
    pub lifecycle: GoalLifecycle,
}

pub fn decode_imu(bytes: &[u8]) -> Result<ImuPacket, PacketError> {
    let (time_microseconds, sequence, values) = decode_sensor(bytes, IMU_MAGIC)?;
    Ok(ImuPacket {
        time_microseconds,
        sequence,
        acceleration_frd_mps2: [values[0], values[1], values[2]],
        angular_rate_frd_rad_s: [values[3], values[4], values[5]],
    })
}

pub fn decode_local_state(bytes: &[u8]) -> Result<LocalStatePacket, PacketError> {
    let (time_microseconds, sequence, values) = decode_sensor(bytes, LOCAL_STATE_MAGIC)?;
    Ok(LocalStatePacket {
        time_microseconds,
        sequence,
        position_ned_metres: [values[0], values[1], values[2]],
        velocity_ned_mps: [values[3], values[4], values[5]],
    })
}

pub fn decode_clock(bytes: &[u8]) -> Result<ClockPacket, PacketError> {
    if bytes.len() != CLOCK_PACKET_SIZE {
        return Err(PacketError::Length);
    }
    if bytes[0..4] != CLOCK_MAGIC || bytes[4] != VERSION || bytes[5..8] != [0; 3] {
        return Err(PacketError::Version);
    }
    Ok(ClockPacket {
        time_microseconds: read_u64(&bytes[8..16]).ok_or(PacketError::Length)?,
    })
}

pub fn decode_goal(bytes: &[u8]) -> Result<GoalPacket, PacketError> {
    decode_goal_with_magic(bytes, GOAL_MAGIC, false)
}

/// Decodes a host-SITL landing request. Landing always targets the declared
/// local ground plane (`up == 0`), rather than silently treating an arbitrary
/// move goal as a landing operation.
pub fn decode_landing(bytes: &[u8]) -> Result<GoalPacket, PacketError> {
    decode_goal_with_magic(bytes, LAND_MAGIC, true)
}

fn decode_goal_with_magic(
    bytes: &[u8],
    magic: [u8; 4],
    require_ground_plane: bool,
) -> Result<GoalPacket, PacketError> {
    if bytes.len() != GOAL_PACKET_SIZE {
        return Err(PacketError::Length);
    }
    if bytes[0..4] != magic || bytes[4] != VERSION || bytes[5..8] != [0; 3] {
        return Err(PacketError::Version);
    }
    let goal = GoalPacket {
        goal_id: read_u32(&bytes[8..12]).ok_or(PacketError::Length)?,
        north_metres: read_f32(&bytes[12..16]).ok_or(PacketError::Length)?,
        east_metres: read_f32(&bytes[16..20]).ok_or(PacketError::Length)?,
        up_metres: read_f32(&bytes[20..24]).ok_or(PacketError::Length)?,
    };
    if goal.goal_id == 0
        || !goal.north_metres.is_finite()
        || !goal.east_metres.is_finite()
        || !goal.up_metres.is_finite()
        || (require_ground_plane && goal.up_metres != 0.0)
    {
        return Err(PacketError::NonFinite);
    }
    Ok(goal)
}

pub fn decode_goal_keepalive(bytes: &[u8]) -> Result<GoalKeepalivePacket, PacketError> {
    if bytes.len() != GOAL_KEEPALIVE_PACKET_SIZE {
        return Err(PacketError::Length);
    }
    if bytes[0..4] != GOAL_KEEPALIVE_MAGIC || bytes[4] != VERSION || bytes[5..8] != [0; 3] {
        return Err(PacketError::Version);
    }
    let goal_id = read_u32(&bytes[8..12]).ok_or(PacketError::Length)?;
    if goal_id == 0 {
        return Err(PacketError::NonFinite);
    }
    Ok(GoalKeepalivePacket { goal_id })
}

pub fn is_status_query(bytes: &[u8]) -> bool {
    bytes.len() == STATUS_QUERY_PACKET_SIZE
        && bytes[0..4] == STATUS_QUERY_MAGIC
        && bytes[4] == VERSION
        && bytes[5..8] == [0; 3]
}

pub fn encode_output(
    time_microseconds: u64,
    sequence: u32,
    permitted: bool,
    motors: [f32; 4],
) -> Result<[u8; OUTPUT_PACKET_SIZE], PacketError> {
    if motors
        .iter()
        .any(|motor| !motor.is_finite() || !(0.0..=1.0).contains(motor))
        || (!permitted && motors.iter().any(|motor| *motor != 0.0))
    {
        return Err(PacketError::NonFinite);
    }
    let mut bytes = [0_u8; OUTPUT_PACKET_SIZE];
    bytes[0..4].copy_from_slice(&OUTPUT_MAGIC);
    bytes[4] = VERSION;
    bytes[8..16].copy_from_slice(&time_microseconds.to_le_bytes());
    bytes[16..20].copy_from_slice(&sequence.to_le_bytes());
    bytes[20] = u8::from(permitted);
    for (index, motor) in motors.iter().enumerate() {
        bytes[24 + index * 4..28 + index * 4].copy_from_slice(&motor.to_le_bytes());
    }
    Ok(bytes)
}

pub fn encode_status(time_microseconds: u64, flags: u16) -> [u8; STATUS_PACKET_SIZE] {
    let mut bytes = [0_u8; STATUS_PACKET_SIZE];
    bytes[0..4].copy_from_slice(&STATUS_MAGIC);
    bytes[4] = VERSION;
    bytes[8..16].copy_from_slice(&time_microseconds.to_le_bytes());
    bytes[16..18].copy_from_slice(&flags.to_le_bytes());
    bytes
}

pub fn encode_goal_status(status: GoalStatusPacket) -> [u8; GOAL_STATUS_PACKET_SIZE] {
    let mut bytes = [0_u8; GOAL_STATUS_PACKET_SIZE];
    bytes[0..4].copy_from_slice(&GOAL_STATUS_MAGIC);
    bytes[4] = VERSION;
    bytes[8..12].copy_from_slice(&status.goal_id.to_le_bytes());
    bytes[12] = status.lifecycle as u8;
    bytes
}

pub fn localhost(sender: SocketAddr) -> bool {
    sender.ip().is_loopback()
}

fn decode_sensor(bytes: &[u8], magic: [u8; 4]) -> Result<(u64, u32, [f32; 6]), PacketError> {
    if bytes.len() != SENSOR_PACKET_SIZE {
        return Err(PacketError::Length);
    }
    if bytes[0..4] != magic || bytes[4] != VERSION || bytes[5..8] != [0; 3] {
        return Err(PacketError::Version);
    }
    let mut values = [0.0; 6];
    for (index, value) in values.iter_mut().enumerate() {
        *value = read_f32(&bytes[20 + index * 4..24 + index * 4]).ok_or(PacketError::Length)?;
    }
    if values.iter().any(|value| !value.is_finite()) {
        return Err(PacketError::NonFinite);
    }
    Ok((
        read_u64(&bytes[8..16]).ok_or(PacketError::Length)?,
        read_u32(&bytes[16..20]).ok_or(PacketError::Length)?,
        values,
    ))
}

fn read_u32(bytes: &[u8]) -> Option<u32> {
    Some(u32::from_le_bytes(bytes.try_into().ok()?))
}

fn read_u64(bytes: &[u8]) -> Option<u64> {
    Some(u64::from_le_bytes(bytes.try_into().ok()?))
}

fn read_f32(bytes: &[u8]) -> Option<f32> {
    Some(f32::from_le_bytes(bytes.try_into().ok()?))
}

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

    fn sensor(magic: [u8; 4]) -> [u8; SENSOR_PACKET_SIZE] {
        let mut bytes = [0_u8; SENSOR_PACKET_SIZE];
        bytes[..4].copy_from_slice(&magic);
        bytes[4] = VERSION;
        bytes[8..16].copy_from_slice(&123_u64.to_le_bytes());
        bytes[16..20].copy_from_slice(&9_u32.to_le_bytes());
        for index in 0..6 {
            bytes[20 + index * 4..24 + index * 4]
                .copy_from_slice(&(index as f32 + 0.5).to_le_bytes());
        }
        bytes
    }

    #[test]
    fn sensor_records_are_finite() {
        let packet = decode_imu(&sensor(IMU_MAGIC)).unwrap();
        assert_eq!(packet.time_microseconds, 123);
        assert_eq!(packet.acceleration_frd_mps2, [0.5, 1.5, 2.5]);
        assert_eq!(packet.angular_rate_frd_rad_s, [3.5, 4.5, 5.5]);
        assert_eq!(
            decode_imu(&sensor(LOCAL_STATE_MAGIC)),
            Err(PacketError::Version)
        );
    }

    #[test]
    fn simulator_clock_has_no_host_receipt_time() {
        let mut packet = [0_u8; CLOCK_PACKET_SIZE];
        packet[..4].copy_from_slice(&CLOCK_MAGIC);
        packet[4] = VERSION;
        packet[8..16].copy_from_slice(&987_654_u64.to_le_bytes());
        assert_eq!(
            decode_clock(&packet),
            Ok(ClockPacket {
                time_microseconds: 987_654,
            })
        );
        packet[4] = VERSION + 1;
        assert_eq!(decode_clock(&packet), Err(PacketError::Version));
    }

    #[test]
    fn inhibited_output() {
        assert!(encode_output(1, 2, true, [0.1, 0.2, 0.3, 0.4]).is_ok());
        assert_eq!(
            encode_output(1, 2, false, [0.1, 0.0, 0.0, 0.0]),
            Err(PacketError::NonFinite)
        );
    }

    #[test]
    fn goal_keepalive_validation() {
        let mut packet = [0_u8; GOAL_KEEPALIVE_PACKET_SIZE];
        packet[..4].copy_from_slice(&GOAL_KEEPALIVE_MAGIC);
        packet[4] = VERSION;
        packet[8..12].copy_from_slice(&7_u32.to_le_bytes());
        assert_eq!(
            decode_goal_keepalive(&packet),
            Ok(GoalKeepalivePacket { goal_id: 7 })
        );
        packet[8..12].copy_from_slice(&0_u32.to_le_bytes());
        assert_eq!(decode_goal_keepalive(&packet), Err(PacketError::NonFinite));
    }

    #[test]
    fn landing_is_an_explicit_ground_plane_command() {
        let mut packet = [0_u8; GOAL_PACKET_SIZE];
        packet[..4].copy_from_slice(&LAND_MAGIC);
        packet[4] = VERSION;
        packet[8..12].copy_from_slice(&7_u32.to_le_bytes());
        packet[12..16].copy_from_slice(&1.0_f32.to_le_bytes());
        packet[16..20].copy_from_slice(&(-2.0_f32).to_le_bytes());
        assert_eq!(
            decode_landing(&packet),
            Ok(GoalPacket {
                goal_id: 7,
                north_metres: 1.0,
                east_metres: -2.0,
                up_metres: 0.0,
            })
        );
        packet[20..24].copy_from_slice(&0.1_f32.to_le_bytes());
        assert_eq!(decode_landing(&packet), Err(PacketError::NonFinite));
    }
}