//! UART/ BLE I/O Protocol

pub mod msg {
    #![allow(clippy::all, clippy::pedantic, clippy::nursery, dead_code)]
    include!(concat!(env!("OUT_DIR"), "/c_its_io.rs"));
}

use alloc::vec::Vec;

#[cfg(feature = "uart")]
use c_its_parser::gn as geonetworking;

#[cfg(feature = "uart")]
impl From<msg::RawMsgRx> for msg::SerialOutputMsg {
    fn from(value: msg::RawMsgRx) -> Self {
        use msg::serial_output_msg::Payload;

        Self {
            payload: Some(Payload::RxMsg(value)),
        }
    }
}

#[cfg(feature = "uart")]
impl From<msg::DenmEvent> for msg::SerialOutputMsg {
    fn from(value: msg::DenmEvent) -> Self {
        use msg::serial_output_msg::Payload;

        Self {
            payload: Some(Payload::Denm(value)),
        }
    }
}

impl msg::RawMsgRx {
    #[cfg(feature = "uart")]
    pub fn serialize_uart_proto(&self) -> Vec<u8> {
        serialize_serial_msg(&Into::<msg::SerialOutputMsg>::into(self.clone()))
    }
}
impl msg::DenmEvent {
    #[cfg(feature = "uart")]
    #[allow(unused)]
    pub fn serialize_uart_proto(&self) -> Vec<u8> {
        serialize_serial_msg(&Into::<msg::SerialOutputMsg>::into(*self))
    }

    #[cfg(feature = "denm")]
    pub fn serialize_ble_proto(&self) -> Vec<u8> {
        use prost::Message as _;

        self.encode_to_vec()
    }
}
impl msg::CamEvent {
    #[cfg(feature = "cam")]
    pub fn serialize_ble_proto(&self) -> Vec<u8> {
        use prost::Message as _;

        self.encode_to_vec()
    }
}

#[cfg(feature = "uart")]
#[derive(Debug, Default)]
pub struct Parser {
    buffer: alloc::vec::Vec<u8>,
}

#[cfg(feature = "uart")]
impl Parser {
    const SOF_LEN: u16 = 4;
    const SOF1: u8 = 0x56;
    const SOF2: u8 = 0x32;
    const SOF3: u8 = 0x58;
    const SOF4: u8 = 0x2B;
    const EOF_LEN: u16 = 2;
    #[allow(unused)]
    const EOF1: u8 = 0x0d;
    #[allow(unused)]
    const EOF2: u8 = 0x0a;
    const LENGTH_LEN: u16 = 2;

    /// Parses incoming data and return list of messages
    pub fn parse(&mut self, input: &[u8]) -> alloc::vec::Vec<msg::SerialInputMsg> {
        use prost::Message as _;

        // combine with existing data
        self.buffer.append(&mut input.to_vec());

        // find start of frame and parse
        let mut output = alloc::vec![];
        while let Some(pos) = Self::find_sof(&self.buffer) {
            // drop everything before the SOF
            self.buffer.drain(..pos);

            // get length field and check buffer size
            if let Some(len_bytes) = self
                .buffer
                .get(usize::from(Self::SOF_LEN)..usize::from(Self::SOF_LEN + Self::LENGTH_LEN))
            {
                // unwrap is fine since we queried 2 bytes before, so we can also convert to 2-byte slice
                let msg_size = u16::from_be_bytes(len_bytes.try_into().unwrap());

                let required_buf_size = Self::SOF_LEN + Self::LENGTH_LEN + msg_size + Self::EOF_LEN;
                if let Some(payload) = self.buffer.get(
                    usize::from(Self::SOF_LEN + Self::LENGTH_LEN)
                        ..usize::from(required_buf_size - 2),
                ) {
                    match msg::SerialInputMsg::decode(payload) {
                        Ok(data) => {
                            output.push(data);
                        }
                        Err(err) => log::error!("Failed to parse input: {err:?}"),
                    }

                    // consume buffer
                    self.buffer.drain(..usize::from(required_buf_size));
                } else {
                    break;
                }
            }
        }

        output
    }

    /// Returns the start index of the SOF sequence, if present
    fn find_sof(input: &[u8]) -> Option<usize> {
        // search for SOF
        if let Some(sof1_pos) = input.iter().position(|i| *i == Self::SOF1) {
            // drop everything before sof1_pos and check if remainder continues with SOF sequence
            let (_, msg) = input.split_at(sof1_pos);
            if msg.starts_with(&[Self::SOF1, Self::SOF2, Self::SOF3, Self::SOF4]) {
                // found the SOF sequence
                return Some(sof1_pos);
            }
        }

        // if we reach this position, SOF sequence wasn't found
        None
    }
}

#[cfg(feature = "uart")]
fn serialize_serial_msg<T>(msg: &T) -> Vec<u8>
where
    T: prost::Message,
{
    let mut msg_buf = msg.encode_to_vec();

    let sof = [0x56u8, 0x32, 0x58, 0x2B];
    let eof = [0x0du8, 0x0a];
    #[allow(clippy::cast_possible_truncation)]
    let len: [u8; 2] = (msg_buf.len() as u16).to_be_bytes();

    let mut out = sof.to_vec();
    out.extend_from_slice(&len);
    out.append(&mut msg_buf);
    out.extend_from_slice(&eof);

    out
}

#[cfg(feature = "uart")]
impl From<msg::PositionState> for crate::applogic::GnssFix {
    fn from(value: msg::PositionState) -> Self {
        let time =
            chrono::DateTime::from_timestamp_millis(value.timestamp_ms).map(|v| v.naive_utc());

        let latitude_deg = value.position.latitude_deg.into();
        let longitude_deg = value.position.longitude_deg.into();
        let heading_deg = value.heading.map(|v| {
            #[allow(clippy::cast_precision_loss)]
            let val = (v as f32) / 10.;
            val
        });
        let speed_mps = value.speed.map(|v| {
            #[allow(clippy::cast_precision_loss)]
            let val = (v as f32) / 100.;
            val
        });

        Self {
            time,
            latitude_deg,
            longitude_deg,
            heading_deg,
            speed_mps,
        }
    }
}

#[cfg(feature = "uart")]
impl From<msg::PositionState> for esp32_cits_core::PosVel {
    fn from(value: msg::PositionState) -> Self {
        let position = geo_types::Point::new(
            value.position.longitude_deg.into(),
            value.position.latitude_deg.into(),
        );
        let heading_deg = value.heading.map(|v| {
            #[allow(clippy::cast_precision_loss)]
            let val = (v as f32) / 10.;
            val
        });
        let speed_mps = value.speed.map(|v| {
            #[allow(clippy::cast_precision_loss)]
            let val = (v as f32) / 100.;
            val
        });

        Self {
            position,
            heading_deg,
            speed_mps,
        }
    }
}

#[cfg(feature = "uart")]
impl From<msg::ItsPosition> for esp32_cits_core::PosVel {
    fn from(value: msg::ItsPosition) -> Self {
        let latitude_deg = f64::from(value.latitude) / 10_000_000.;
        let longitude_deg = f64::from(value.longitude) / 10_000_000.;
        let position = geo_types::Point::new(longitude_deg, latitude_deg);

        Self {
            position,
            heading_deg: None,
            speed_mps: None,
        }
    }
}

#[cfg(feature = "uart")]
impl esp32_cits_core::tx::GnItsPayload for msg::RawMsgTx {
    fn make_eh(
        &self,
        address: [u8; 6],
        own_position: &esp32_cits_core::PosVel,
        time: chrono::DateTime<chrono::Utc>,
        seq_no: &mut u16,
    ) -> Result<
        (
            c_its_parser::gn::en302636_4_1::ExtendedHeader,
            Option<c_its_parser::gn::en302636_4_1::AreaType>,
            u8,
        ),
        alloc::string::String,
    > {
        use alloc::string::ToString as _;

        let station_type = self
            .station_type
            .and_then(|v| {
                #[allow(clippy::cast_possible_truncation)]
                let raw_val = v as u8;
                geonetworking::en302636_4_1::StationType::try_from(raw_val).ok()
            })
            .unwrap_or_default();
        #[allow(clippy::cast_possible_truncation)]
        let hop_limit = self.hop_limit as u8;

        Ok(match self.gn_transport() {
            msg::GnTransport::Gac => {
                if self.gn_area.position.is_none() {
                    return Err("Failed to create GAC: gnArea.position missing".to_string());
                }

                *seq_no += 1;

                let (atype, geoanycast) = Self::make_geobcast(
                    address,
                    station_type,
                    own_position,
                    time,
                    self.gn_area,
                    *seq_no,
                )?;
                let eh = geonetworking::en302636_4_1::ExtendedHeader::GAC(geoanycast);
                (eh, Some(atype), hop_limit)
            }
            msg::GnTransport::Gbc => {
                if self.gn_area.position.is_none() {
                    return Err("Failed to create GAC: gnArea.position missing".to_string());
                }

                *seq_no += 1;

                let (atype, geoanycast) = Self::make_geobcast(
                    address,
                    station_type,
                    own_position,
                    time,
                    self.gn_area,
                    *seq_no,
                )?;
                let eh = geonetworking::en302636_4_1::ExtendedHeader::GBC(geoanycast);
                (eh, Some(atype), hop_limit)
            }
            msg::GnTransport::Tsb => {
                *seq_no += 1;

                (
                    esp32_cits_core::tx::gn::make_tsb_eh(
                        address,
                        station_type,
                        own_position,
                        time,
                        *seq_no,
                    )?,
                    None,
                    hop_limit,
                )
            }
            msg::GnTransport::Shb => (
                esp32_cits_core::tx::gn::make_shb_eh(address, station_type, own_position, time)?,
                None,
                1,
            ),
        })
    }

    fn make_payload(
        &self,
    ) -> Result<(Vec<u8>, c_its_parser::standards::extensions::ItsMessageId), alloc::string::String>
    {
        #[allow(clippy::cast_possible_truncation)]
        let message_id = (self.message_type as u8).try_into()?;
        Ok((self.payload.clone(), message_id))
    }
}

#[cfg(feature = "uart")]
impl msg::RawMsgTx {
    fn make_geobcast(
        address: [u8; 6],
        station_type: geonetworking::en302636_4_1::StationType,
        own_position: &esp32_cits_core::PosVel,
        time: chrono::DateTime<chrono::Utc>,
        gn_area: msg::GnArea,
        sequence_number: u16,
    ) -> Result<
        (
            c_its_parser::gn::en302636_4_1::AreaType,
            geonetworking::en302636_4_1::GeoBroadcast,
        ),
        alloc::string::String,
    > {
        let source_position_vector =
            esp32_cits_core::tx::gn::make_lpv(address, station_type, own_position, time)?;

        let (atype, dist_a, dist_b, angle) = match gn_area.area() {
            msg::GnAreaShape::EtsiAreashapeCircle => (
                c_its_parser::gn::en302636_4_1::AreaType::Circular,
                gn_area.dist_a,
                None,
                None,
            ),
            msg::GnAreaShape::EtsiAreashapeRectangle => (
                c_its_parser::gn::en302636_4_1::AreaType::Rectangular,
                gn_area.dist_a,
                gn_area.dist_b,
                gn_area.angle,
            ),
            msg::GnAreaShape::EtsiAreashapeEllipsis => (
                c_its_parser::gn::en302636_4_1::AreaType::Ellipsoidal,
                gn_area.dist_a,
                gn_area.dist_b,
                gn_area.angle,
            ),
        };

        #[allow(clippy::cast_possible_truncation)]
        let latitude_deg = own_position.position.y() as f32;
        #[allow(clippy::cast_possible_truncation)]
        let longitude_deg = own_position.position.x() as f32;
        let distance_a = dist_a
            .map(|v| {
                #[allow(clippy::cast_possible_truncation, clippy::cast_sign_loss)]
                let val = v as u16;
                val
            })
            .unwrap_or_default();
        let distance_b = dist_b
            .map(|v| {
                #[allow(clippy::cast_possible_truncation, clippy::cast_sign_loss)]
                let val = v as u16;
                val
            })
            .unwrap_or_default();
        let angle = angle
            .map(|v| {
                #[allow(clippy::cast_possible_truncation, clippy::cast_sign_loss)]
                let val = v as u16;
                val
            })
            .unwrap_or_default();

        let gac = geonetworking::en302636_4_1::GeoAnycast::try_from_values(
            sequence_number,
            source_position_vector,
            latitude_deg,
            longitude_deg,
            distance_a,
            distance_b,
            angle,
        )
        .map_err(|err| alloc::format!("{err}"))?;

        Ok((atype, gac))
    }
}

#[cfg(feature = "cam")]
impl From<alloc::boxed::Box<c_its_parser::standards::cam_1_4_1::cam_pdu_descriptions::CAM>>
    for msg::CamEvent
{
    fn from(
        value: alloc::boxed::Box<c_its_parser::standards::cam_1_4_1::cam_pdu_descriptions::CAM>,
    ) -> Self {
        use c_its_parser::standards::cam_1_4_1::cam_pdu_descriptions;

        let station_id = value.header.station_id.0;
        let station_type = u32::from(value.cam.cam_parameters.basic_container.station_type.0);
        let position = value
            .cam
            .cam_parameters
            .basic_container
            .reference_position
            .into();

        let vehicle_role = value
            .cam
            .cam_parameters
            .low_frequency_container
            .and_then(|v| match v {
                cam_pdu_descriptions::LowFrequencyContainer::basicVehicleContainerLowFrequency(
                    basic_vehicle_container_low_frequency,
                ) => Some(basic_vehicle_container_low_frequency.vehicle_role as u32),
                _ => None,
            });

        let vehicle_data = match value.cam.cam_parameters.high_frequency_container {
            cam_pdu_descriptions::HighFrequencyContainer::basicVehicleContainerHighFrequency(
                basic_vehicle_container_high_frequency,
            ) => Some(basic_vehicle_container_high_frequency.into()),
            _ => None,
        };

        Self {
            station_id,
            station_type,
            position,
            vehicle_role,
            vehicle_data,
        }
    }
}

#[cfg(feature = "cam")]
impl From<c_its_parser::standards::cam_1_4_1::cam_pdu_descriptions::BasicVehicleContainerHighFrequency> for msg::VehicleContainer {
    fn from(value: c_its_parser::standards::cam_1_4_1::cam_pdu_descriptions::BasicVehicleContainerHighFrequency) -> Self {
        let heading_deg = if value.heading.heading_value.is_unavailable() {
            None
        } else {
            Some(u32::from(value.heading.heading_value.0))
        };
        let speed = if value.speed.speed_value.is_unavailable() {
            None
        } else {
            Some(u32::from(value.speed.speed_value.0))
        };
        let vehicle_length_dm = if value.vehicle_length.vehicle_length_value.is_unavailable() {
            None
        } else {
            Some(u32::from(value.vehicle_length.vehicle_length_value.0))
        };
        let vehicle_width_dm = if value.vehicle_width.is_unavailable() {
            None
        } else {
            Some(u32::from(value.vehicle_width.0))
        };


        Self { heading_deg, speed, vehicle_length_dm, vehicle_width_dm }
    }
}

#[cfg(feature = "denm")]
impl From<alloc::boxed::Box<c_its_parser::standards::denm_2_2_1::denm_pdu_description::DENM>>
    for msg::DenmEvent
{
    fn from(
        value: alloc::boxed::Box<c_its_parser::standards::denm_2_2_1::denm_pdu_description::DENM>,
    ) -> Self {
        let denm_mgmt = &value.denm.management;

        let station_id = value.header.station_id.0; // TODO: or from action ID?

        let ref_time = chrono::DateTime::<chrono::Utc>::from(denm_mgmt.reference_time.clone());
        let end_time = ref_time + chrono::Duration::seconds(denm_mgmt.validity_duration.0.into());

        #[allow(clippy::cast_sign_loss)]
        let timestamp = ref_time.timestamp_millis() as u64;
        #[allow(clippy::cast_sign_loss)]
        let validity_end_ts = end_time.timestamp_millis() as u64;
        let seq_num = denm_mgmt.action_id.sequence_number.0.into();

        let event_position = Some(denm_mgmt.event_position.clone().into());
        let event_heading_deg = value
            .denm
            .location
            .and_then(|v| v.event_position_heading)
            .map(|v| {
                #[allow(clippy::cast_possible_truncation, clippy::cast_sign_loss)]
                let conv = v.value.as_deg() as u32;
                conv
            });
        let direction = denm_mgmt
            .traffic_direction
            .map(|v| msg::TrafficDir::from(v) as i32);
        let cc_tuple = value
            .denm
            .situation
            .map(|v| v.event_type.cc_and_scc.to_u8_tuple());
        let cause_code = cc_tuple.map(|v| v.0.into());
        let sub_cause_code = cc_tuple.map(|v| v.1.into());

        Self {
            timestamp,
            validity_end_ts,
            station_id,
            seq_num,
            event_position,
            event_heading_deg,
            direction,
            cause_code,
            sub_cause_code,
        }
    }
}

impl From<c_its_parser::standards::cdd_1_3_1_1::its_container::ReferencePosition>
    for msg::Position
{
    fn from(value: c_its_parser::standards::cdd_1_3_1_1::its_container::ReferencePosition) -> Self {
        #[allow(clippy::cast_precision_loss, clippy::cast_possible_truncation)]
        let latitude_deg = value.latitude.as_deg() as f32;
        #[allow(clippy::cast_precision_loss, clippy::cast_possible_truncation)]
        let longitude_deg = value.longitude.as_deg() as f32;

        Self {
            latitude_deg,
            longitude_deg,
        }
    }
}

impl From<c_its_parser::standards::cdd_2_2_1::etsi_its_cdd::ReferencePosition> for msg::Position {
    fn from(value: c_its_parser::standards::cdd_2_2_1::etsi_its_cdd::ReferencePosition) -> Self {
        #[allow(clippy::cast_precision_loss, clippy::cast_possible_truncation)]
        let latitude_deg = value.latitude.as_deg() as f32;
        #[allow(clippy::cast_precision_loss, clippy::cast_possible_truncation)]
        let longitude_deg = value.longitude.as_deg() as f32;

        Self {
            latitude_deg,
            longitude_deg,
        }
    }
}

impl From<c_its_parser::standards::cdd_2_2_1::etsi_its_cdd::TrafficDirection> for msg::TrafficDir {
    fn from(value: c_its_parser::standards::cdd_2_2_1::etsi_its_cdd::TrafficDirection) -> Self {
        use c_its_parser::standards::cdd_2_2_1::etsi_its_cdd::TrafficDirection;

        match value {
            TrafficDirection::allTrafficDirections => Self::AllDirections,
            TrafficDirection::sameAsReferenceDirection_upstreamOfReferencePosition => {
                Self::Upstream
            }
            TrafficDirection::sameAsReferenceDirection_downstreamOfReferencePosition => {
                Self::Downstream
            }
            TrafficDirection::oppositeToReferenceDirection => Self::Opposite,
        }
    }
}

#[cfg(test)]
mod tests {

    use super::*;
    use crate::init_test_env_logger;

    #[cfg(feature = "uart")]
    impl From<msg::RawMsgTx> for msg::SerialInputMsg {
        fn from(value: msg::RawMsgTx) -> Self {
            use msg::serial_input_msg::Payload;

            Self {
                payload: Some(Payload::TxMsg(value)),
            }
        }
    }

    #[cfg(feature = "uart")]
    impl From<msg::PositionState> for msg::SerialInputMsg {
        fn from(value: msg::PositionState) -> Self {
            use msg::serial_input_msg::Payload;

            Self {
                payload: Some(Payload::Position(value)),
            }
        }
    }

    // manual example data:
    // SerialInputMsg(RawMsgTx): 5632582b0040123e0a340202c173256817284029d5b3400f1feaf01ffffffc23b7743e00000fc0007e8138250737feebfff600000dffff7ffff1ce40400010011807220028020d0a
    // SerialInputMsg(PositionState): 5632582b00170a1509a0bbe2a29f010000120a0d00002842150000b8410d0a

    #[cfg(feature = "uart")]
    #[test]
    fn uart_parser() {
        // generate some test UART input messages
        let raw_tx = msg::RawMsgTx {
            payload: crate::testdata::CAM_SMALL.to_vec(),
            message_type: 1,
            gn_transport: msg::GnTransport::Shb.into(),
            gn_area: msg::GnArea {
                area: None,
                position: None,
                dist_a: None,
                dist_b: None,
                angle: None,
            },
            hop_limit: 2,
            station_type: None,
        };
        let raw_tx_buf = serialize_serial_msg::<msg::SerialInputMsg>(&raw_tx.clone().into());

        let pos_state = msg::PositionState {
            timestamp_ms: 1785144196000,
            position: msg::Position {
                latitude_deg: 42.,
                longitude_deg: 23.,
            },
            heading: None,
            speed: None,
        };
        let pos_state_buf = serialize_serial_msg::<msg::SerialInputMsg>(&pos_state.clone().into());

        let mut parser = Parser::default();

        // feed individual complete messages to parser
        let output = parser.parse(&raw_tx_buf);
        assert_eq!(1, output.len());
        assert_eq!(
            &msg::SerialInputMsg::from(raw_tx.clone()),
            output.get(0).unwrap()
        );

        let output = parser.parse(&pos_state_buf);
        assert_eq!(1, output.len());
        assert_eq!(
            &msg::SerialInputMsg::from(pos_state.clone()),
            output.get(0).unwrap()
        );

        // feed multiple complete messages to parser
        let mut combined_buf = pos_state_buf.clone();
        combined_buf.extend_from_slice(&raw_tx_buf);

        let output = parser.parse(&combined_buf);
        assert_eq!(2, output.len());
        assert_eq!(
            &msg::SerialInputMsg::from(pos_state.clone()),
            output.get(0).unwrap()
        );
        assert_eq!(
            &msg::SerialInputMsg::from(raw_tx.clone()),
            output.get(1).unwrap()
        );

        // feed partial messages to parser
        let buf1 = &raw_tx_buf[0..3];
        let buf2 = &raw_tx_buf[3..];

        let output = parser.parse(buf1);
        assert_eq!(0, output.len());

        let output = parser.parse(buf2);
        assert_eq!(1, output.len());
        assert_eq!(
            &msg::SerialInputMsg::from(raw_tx.clone()),
            output.get(0).unwrap()
        );

        // feed messages with garbage in between
        let mut buf1 = vec![0xabu8, 0xcd]; // prepend some garbage
        buf1.extend_from_slice(&raw_tx_buf[0..3]);
        let mut buf2 = (&raw_tx_buf[3..]).to_vec();
        buf2.extend_from_slice(&[0xfeu8, 0x42]); // append some garbage
        buf2.extend_from_slice(&pos_state_buf); // append second message
        buf2.extend_from_slice(&[0x11u8, 0x22]); // append some more garbage

        let output = parser.parse(&buf1);
        assert_eq!(0, output.len());

        let output = parser.parse(&buf2);
        assert_eq!(2, output.len());
        assert_eq!(
            &msg::SerialInputMsg::from(raw_tx.clone()),
            output.get(0).unwrap()
        );
        assert_eq!(
            &msg::SerialInputMsg::from(pos_state.clone()),
            output.get(1).unwrap()
        );
    }
}