// #![cfg_attr(not(test), no_std)]

use bitfield::bitfield;
use crc::{Crc, CRC_8_DVB_S2};
use heapless::{Deque, Vec};
use num_enum::{IntoPrimitive, TryFromPrimitive};

#[derive(Debug, PartialEq, Eq, Clone, Copy, IntoPrimitive, TryFromPrimitive)]
#[repr(u8)]
pub enum FrameType {
    Gps = 0x02,
    Vario = 0x07,
    BatterySensor = 0x08,
    BaroAltitude = 0x09,
    LinkStatistics = 0x14,
    OpenTxSync = 0x10,
    RadioId = 0x3A,
    RcChannelsPacked = 0x16,
    Attitude = 0x1E,
    FlightMode = 0x21,
    // Extended Header Frames
    DevicePing = 0x28,
    DeviceInfo = 0x29,
    ParameterSettingsEntry = 0x2B,
    ParameterRead = 0x2C,
    ParameterWrite = 0x2D,
    ELRSStatus = 0x2E,
    Command = 0x32,
    // KISS frames
    KissReq = 0x78,
    KissResp = 0x79,
    // MSP commands
    MspReq = 0x7A,
    MspResp = 0x7B,
    MspWrite = 0x7C,
    // Ardupilot frames
    ArdupilotResp = 0x80,
}

#[derive(Debug, PartialEq, Eq, Clone, Copy)]
pub enum CrsfPacket {
    RcChannels([u16; 16]),
}

impl CrsfPacket {
    fn from_type_and_data(
        frame_type: FrameType,
        data: impl Iterator<Item = u8> + ExactSizeIterator,
    ) -> Option<Self> {
        match frame_type {
            FrameType::RcChannelsPacked => {
                if data.len() != 22 {
                    return None;
                }
                let mut buf = Vec::<u8, 22>::new();
                buf.extend(data);
                let channels = RcChannelsPacked(&mut buf[..]);
                Some(Self::RcChannels([
                    channels.ch1(),
                    channels.ch2(),
                    channels.ch3(),
                    channels.ch4(),
                    channels.ch5(),
                    channels.ch6(),
                    channels.ch7(),
                    channels.ch8(),
                    channels.ch9(),
                    channels.ch10(),
                    channels.ch11(),
                    channels.ch12(),
                    channels.ch13(),
                    channels.ch14(),
                    channels.ch15(),
                    channels.ch16(),
                ]))
            }
            _ => None,
        }
    }
}

#[derive(Debug, PartialEq, Eq, Clone, Copy, IntoPrimitive, TryFromPrimitive)]
#[repr(u8)]
pub enum Address {
    Broadcast = 0x00,
    Usb = 0x10,
    TbsCorePnpPro = 0x80,
    Reserved1 = 0x8A,
    CurrentSensor = 0xC0,
    Gps = 0xC2,
    TbsBlackbox = 0xC4,
    FlightController = 0xC8,
    Reserved2 = 0xCA,
    RaceTag = 0xCC,
    RadioTransmitter = 0xEA,
    CrsfReceiver = 0xEC,
    CrsfTrasmitter = 0xEE,
    ElrsLua = 0xEF,
}

#[derive(Debug, PartialEq, Eq, Clone, Copy)]
pub struct Header {
    address: Address,
    size: u8,
    frame_type: FrameType,
}

impl Header {
    const fn valid(self) -> bool {
        match self.frame_type {
            FrameType::RcChannelsPacked => self.size == 24,
            _ => true,
        }
    }
}

bitfield! {
    struct RcChannelsPacked([u8]);
    impl Debug;
    u16;
    ch1, set_ch1: Self::upper(1), Self::lower(1);
    ch2, set_ch2: Self::upper(2), Self::lower(2);
    ch3, set_ch3: Self::upper(3), Self::lower(3);
    ch4, set_ch4: Self::upper(4), Self::lower(4);
    ch5, set_ch5: Self::upper(5), Self::lower(5);
    ch6, set_ch6: Self::upper(6), Self::lower(6);
    ch7, set_ch7: Self::upper(7), Self::lower(7);
    ch8, set_ch8: Self::upper(8), Self::lower(8);
    ch9, set_ch9: Self::upper(9), Self::lower(9);
    ch10, set_ch10: Self::upper(10), Self::lower(10);
    ch11, set_ch11: Self::upper(11), Self::lower(11);
    ch12, set_ch12: Self::upper(12), Self::lower(12);
    ch13, set_ch13: Self::upper(13), Self::lower(13);
    ch14, set_ch14: Self::upper(14), Self::lower(14);
    ch15, set_ch15: Self::upper(15), Self::lower(15);
    ch16, set_ch16: Self::upper(16), Self::lower(16);
}

impl<T> RcChannelsPacked<T> {
    const BITS_PER_CHANNEL: usize = 11;

    const fn upper(ch: usize) -> usize {
        Self::lower(ch) + Self::BITS_PER_CHANNEL - 1
    }

    const fn lower(ch: usize) -> usize {
        Self::BITS_PER_CHANNEL * (ch - 1)
    }
}

pub struct CrsfParser {
    buffer: Deque<u8, { Self::MAX_PACKET_SIZE }>,
}

impl CrsfParser {
    const MAX_PACKET_SIZE: usize = 64;

    #[must_use]
    pub const fn new() -> Self {
        let buffer = Deque::new();
        Self { buffer }
    }

    pub fn push(&mut self, byte: u8) {
        if self.buffer.is_full() {
            // deque used as ringbuffer. The oldest data is overwritten.
            self.buffer.pop_front();
        }
        let _ = self.buffer.push_front(byte);
    }

    pub fn next_packet(&mut self) -> Option<(Address, CrsfPacket)> {
        const CRC8_ALG: Crc<u8> = Crc::<u8>::new(&CRC_8_DVB_S2);

        while Address::try_from(*self.buffer.back()?).is_err() {
            // The buffer doesn't start with a valid Address
            self.buffer.pop_back()?;
        }
        if self.buffer.len() < 4 {
            // The buffer doesn't even have enough data to contain a valid header + crc byte
            return None;
        }

        // Parse the packet header
        let mut iter = self.buffer.iter().rev().copied();
        let address = Address::try_from(iter.next()?).ok()?;
        let size = iter.next()?;
        let frame_type = FrameType::try_from(iter.next()?).ok()?;
        let header = Header {
            address,
            size,
            frame_type,
        };
        // TODO: check the frame size against the frame type.
        if !header.valid() {
            self.buffer.pop_back();
            return None;
        }

        // The size field contains the number of bytes coming after it.
        let packet_length = header.size + 2;
        if self.buffer.len() < usize::from(packet_length) {
            // The buffer doesn't have enough data to contain the specified packet length
            return None;
        }

        let mut crc_calc = CRC8_ALG.digest();
        crc_calc.update(&[header.frame_type.into()]);
        let payload_iter = iter.clone().take(usize::from(packet_length - 4));
        for _ in 0..packet_length - 4 {
            crc_calc.update(&[iter.next()?]);
        }
        let crc8 = crc_calc.finalize();
        let crc = iter.next()?;
        if crc != crc8 {
            // Invalid checksum. Make sure to pop The oldest byte so we don't parse this packet
            // again
            self.buffer.pop_back()?;
            return None;
        }

        // decode payload using type field
        let packet = CrsfPacket::from_type_and_data(header.frame_type, payload_iter);

        // sucessfully decoded a packet
        for _ in 0..packet_length {
            self.buffer.pop_back()?;
        }
        packet.map(|packet| (header.address, packet))
    }
}

#[cfg(test)]
mod tests {
    use crate::{Address, CrsfPacket, CrsfParser};

    #[test]
    fn rc_channels() {
        let input = [
            0xc8, 0x18, 0x16, 0xe0, 0x03, 0x1f, 0xb3, 0xc0, 0xf7, 0x0b, 0xee, 0x69, 0x8f, 0x15,
            0xe0, 0x03, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x78, 0xd9, 0xdb, 0xb4,
        ];

        let mut parser = CrsfParser::new();
        for byte in input {
            assert_eq!(parser.next_packet(), None);
            parser.push(byte);
        }

        assert_eq!(
            parser.next_packet(),
            Some((
                Address::FlightController,
                CrsfPacket::RcChannels([
                    992, 992, 716, 992, 191, 988, 986, 172, 992, 0, 0, 0, 0, 0, 1630, 1758
                ])
            ))
        );
        assert_eq!(parser.next_packet(), None);
    }
}