#[cfg(feature = "hal")]
use defmt::debug;
#[cfg(feature = "hal")]
use embedded_hal::digital::OutputPin;
#[cfg(feature = "hal")]
use embedded_hal_async::spi::SpiDevice;

#[cfg(feature = "hal")]
use crate::{
    RadioError,
    radio::{PaSelection, PacketType, Radio, RampTime, irq},
    traits::{Configure, Transmit},
};

#[cfg(not(feature = "hal"))]
use std::vec::Vec;

/// BPSK bitrate
/// Formula: register = (32 * 32_000_000) / bps
#[derive(Clone, Copy)]
#[cfg_attr(feature = "hal", derive(defmt::Format))]
pub enum Bitrate {
    /// 100 bits per second
    Bps100,
    /// 600 bits per second
    Bps600,
    /// Arbitrary bitrate in bits per second
    Custom(u32),
}

impl Bitrate {
    /// Get the 3-byte register value for this bitrate
    pub fn to_bytes(self) -> [u8; 3] {
        let val = match self {
            Bitrate::Bps100 => 0x9C4000,
            Bitrate::Bps600 => 0x1A0AAA,
            Bitrate::Custom(bps) => (32 * 32_000_000) / bps,
        };
        [(val >> 16) as u8, (val >> 8) as u8, val as u8]
    }
}

#[derive(Clone, Copy)]
#[cfg_attr(feature = "hal", derive(defmt::Format))]
pub enum CrcType {
    None,
    /// Using a common 0x07 polynomial
    Crc8,
    /// CRC-16 CCITT using 0x1021 polynomial
    Crc16,
}

impl CrcType {
    pub fn compute(self, data: &[u8]) -> (u16, usize) {
        match self {
            CrcType::None => (0, 0),
            CrcType::Crc8 => {
                let mut crc: u8 = 0x00;
                for &byte in data {
                    crc ^= byte;
                    for _ in 0..8 {
                        if crc & 0x80 != 0 {
                            crc = (crc << 1) ^ 0x07;
                        } else {
                            crc <<= 1;
                        }
                    }
                }
                (crc as u16, 1)
            }
            CrcType::Crc16 => {
                let mut crc: u16 = 0xFFFF;
                for &byte in data {
                    crc ^= (byte as u16) << 8;
                    for _ in 0..8 {
                        if crc & 0x8000 != 0 {
                            crc = (crc << 1) ^ 0x1021;
                        } else {
                            crc <<= 1;
                        }
                    }
                }
                (crc, 2)
            }
        }
    }

    pub fn write(self, crc: u16, buf: &mut [u8]) {
        match self {
            CrcType::None => {}
            CrcType::Crc8 => {
                buf[0] = crc as u8;
            }
            CrcType::Crc16 => {
                buf[0] = (crc >> 8) as u8;
                buf[1] = crc as u8;
            }
        }
    }
}

#[derive(Clone, Copy)]
#[cfg_attr(feature = "hal", derive(defmt::Format))]
pub enum Whitening {
    None,
    Ccitt,
}

impl Whitening {
    pub fn apply(self, seed: u16, data: &mut [u8]) {
        match self {
            Whitening::None => return,
            Whitening::Ccitt => {}
        }

        // Calculate CCITT whitening using x^9 + x^4 + 1 polynomial and LFSR
        let mut lfsr: u16 = seed & 0x1FF;
        for byte in data.iter_mut() {
            let mut mask = 0u8;
            for bit in 0..8 {
                let feedback = ((lfsr >> 8) ^ (lfsr >> 3)) & 1;
                lfsr = ((lfsr << 1) | feedback) & 0x1FF;
                mask |= (feedback as u8) << (7 - bit);
            }
            *byte ^= mask;
        }
    }
}

#[cfg(not(feature = "hal"))]
pub struct DecodeResult {
    /// On which bit the sync word was found
    pub bit_offset: usize,
    /// Signal was phase-inverted
    pub inverted: bool,
    /// Decoded payload
    pub payload: Vec<u8>,
    /// CRC matches
    pub crc_valid: bool,
}

#[derive(Clone, Copy)]
#[cfg_attr(feature = "hal", derive(defmt::Format))]
pub enum BpskPacket {
    /// No framing, just send raw data
    Raw,
    /// Use a configurable framing
    Framing {
        /// Length of the preamble (0xAA) in bytes
        preamble_len: usize,
        /// Synchronization word (max 32 bytes)
        sync_word: [u8; 32],
        /// Sync word length
        sync_word_len: usize,
        /// CRC size (0, 1 or 2 bytes)
        crc_type: CrcType,
        /// Whitening algorithm
        whitening: Whitening,
        /// Whitening LFSR seed (9-bit, 0x000..0x1FF)
        whitening_seed: u16,
    },
}

impl BpskPacket {
    pub fn default() -> Self {
        Self::Framing {
            preamble_len: 32,
            // Baker-13 code (2 bytes)
            sync_word: [
                0x1F, 0x35, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
                0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
                0x00, 0x00, 0x00, 0x00,
            ],
            sync_word_len: 2,
            crc_type: CrcType::Crc16,
            whitening: Whitening::Ccitt,
            whitening_seed: 0x1FF,
        }
    }

    #[cfg(feature = "hal")]
    pub fn to_bytes(self, payload: &[u8], buf: &mut [u8]) -> Result<u8, RadioError> {
        match self {
            BpskPacket::Raw => {
                // Simple copy operation, no modifications made
                buf[..payload.len()].copy_from_slice(payload);
                payload
                    .len()
                    .try_into()
                    .map_err(|_| RadioError::PayloadTooLarge)
            }
            BpskPacket::Framing {
                preamble_len,
                sync_word,
                sync_word_len,
                crc_type,
                whitening,
                whitening_seed,
            } => {
                let len_field_size = 1;
                let crc_size = match crc_type {
                    CrcType::None => 0,
                    CrcType::Crc8 => 1,
                    CrcType::Crc16 => 2,
                };
                // Validate packet length
                let total =
                    preamble_len + sync_word_len + len_field_size + payload.len() + crc_size;

                if total > buf.len() {
                    return Err(RadioError::PayloadTooLarge);
                }

                // Keeps track of the current position in the buffer
                let mut pos = 0;

                // Write preamble which consists of 0xAA symbols
                buf[pos..pos + preamble_len].fill(0xAA);
                pos += preamble_len;

                // Write sync word
                buf[pos..pos + sync_word_len].copy_from_slice(&sync_word[..sync_word_len]);
                pos += sync_word_len;

                // Actual payload starts here
                let data_start = pos;

                // Write length info
                let payload_len: u8 = payload
                    .len()
                    .try_into()
                    .map_err(|_| RadioError::PayloadTooLarge)?;
                buf[pos] = payload_len;
                pos += 1;

                // Copy the original payload itself
                buf[pos..pos + payload.len()].copy_from_slice(payload);
                pos += payload.len();

                // Compute CRC before the whitening
                let (crc, crc_len) = crc_type.compute(&buf[data_start..pos]);
                crc_type.write(crc, &mut buf[pos..]);
                pos += crc_len;

                // Apply whitening
                whitening.apply(whitening_seed, &mut buf[data_start..pos]);

                // Additional validation - if buffer position can't fit in u8, it's invalid
                pos.try_into().map_err(|_| RadioError::PayloadTooLarge)
            }
        }
    }

    #[cfg(not(feature = "hal"))]
    pub fn decode(&self, data: &[u8]) -> Vec<DecodeResult> {
        match self {
            // Can't do much with Raw packets so just return the same data and accept as valid
            BpskPacket::Raw => vec![DecodeResult {
                bit_offset: 0,
                inverted: false,
                payload: data.to_vec(),
                crc_valid: true,
            }],
            BpskPacket::Framing {
                preamble_len: _,
                sync_word,
                sync_word_len,
                crc_type,
                whitening,
                whitening_seed,
            } => {
                let sync_word = &sync_word[..*sync_word_len];
                let sync_bits = sync_word_len * 8;
                let total_bits = data.len() * 8;
                let mut results = Vec::new();

                let crc_len = match crc_type {
                    CrcType::None => 0,
                    CrcType::Crc8 => 1,
                    CrcType::Crc16 => 2,
                };

                // Go through every bit, giving space to the sync word length
                for i in 0..total_bits.saturating_sub(sync_bits) {
                    let mut matching: usize = 0;
                    for j in 0..sync_bits {
                        // Compare the sync word bit-by-bit against the data stream
                        let data_bit = (data[(i + j) / 8] >> (7 - ((i + j) % 8))) & 1;
                        let sync_bit = (sync_word[j / 8] >> (7 - (j % 8))) & 1;

                        if data_bit == sync_bit {
                            matching += 1;
                        }
                    }

                    // Allow up to 2 bit errors, if <= 2 bits match or unmatch, it's an accepted
                    // normal or phase-inverted data
                    let inverted = if sync_bits - matching <= 2 {
                        false
                    } else if matching <= 2 {
                        true
                    } else {
                        continue;
                    };

                    // Extract bytes that happen after the sync word is found at any bit offset
                    let bit_start = i + sync_bits;
                    let remaining_bytes = (total_bits - bit_start) / 8;
                    if remaining_bytes == 0 {
                        continue;
                    }

                    // Reassemble bytes from arbitrary bit positions after sync word is found at offset `i`
                    let mut raw = vec![0u8; remaining_bytes];
                    for b in 0..remaining_bytes {
                        let mut byte = 0u8;
                        for bit in 0..8 {
                            let idx = bit_start + b * 8 + bit;
                            let mut val = (data[idx / 8] >> (7 - (idx % 8))) & 1;
                            // Handle the 180 degree phase ambiguity
                            if inverted {
                                val ^= 1;
                            }

                            byte |= val << (7 - bit);
                        }
                        raw[b] = byte;
                    }

                    // De-whiten by applying the same whitening operation again (it's reversible)
                    whitening.apply(*whitening_seed, &mut raw);

                    // Extract payload length
                    let (payload_start, payload_len) = (1, raw[0] as usize);

                    if payload_len == 0 {
                        continue;
                    }

                    if payload_start + payload_len + crc_len > raw.len() {
                        continue;
                    }

                    // Verify CRC over len field and payload
                    let crc_data = &raw[..payload_start + payload_len];
                    let (crc_computed, _) = crc_type.compute(crc_data);

                    let crc_valid = match crc_type {
                        CrcType::None => true,
                        CrcType::Crc8 => raw[payload_start + payload_len] == crc_computed as u8,
                        CrcType::Crc16 => {
                            // Assemble u16 from two bytes
                            let received = ((raw[payload_start + payload_len] as u16) << 8)
                                | raw[payload_start + payload_len + 1] as u16;
                            received == crc_computed
                        }
                    };

                    results.push(DecodeResult {
                        bit_offset: i,
                        inverted,
                        payload: raw[payload_start..payload_start + payload_len].to_vec(),
                        crc_valid,
                    });
                }

                results
            }
        }
    }
}

#[cfg(feature = "hal")]
#[derive(Clone, Copy, defmt::Format)]
pub struct BpskConfig {
    pub frequency: u32,
    pub bitrate: Bitrate,
    pub pa: PaSelection,
    pub power_dbm: i8,
    pub ramp: RampTime,
    pub packet: BpskPacket,
}

#[cfg(feature = "hal")]
impl Default for BpskConfig {
    fn default() -> Self {
        Self {
            frequency: 868_100_000,
            bitrate: Bitrate::Bps600,
            pa: PaSelection::LowPower,
            power_dbm: 14,
            ramp: RampTime::Us40,
            packet: BpskPacket::default(),
        }
    }
}

/// BPSK modulation - borrows a Radio, implements Configure + Transmit
#[cfg(feature = "hal")]
pub struct BpskRadio<'a, SPI: SpiDevice, TX: OutputPin, RX: OutputPin, EN: OutputPin> {
    radio: &'a mut Radio<SPI, TX, RX, EN>,
    payload_len: u8,
    config: BpskConfig,
}

#[cfg(feature = "hal")]
impl<'a, SPI: SpiDevice, TX: OutputPin, RX: OutputPin, EN: OutputPin>
    BpskRadio<'a, SPI, TX, RX, EN>
{
    pub fn new(radio: &'a mut Radio<SPI, TX, RX, EN>) -> Self {
        Self {
            radio,
            payload_len: 0,
            config: BpskConfig::default(),
        }
    }

    /// Re-send SetPacketParams with updated payload length (called before each tx/rx)
    async fn update_payload_len(&mut self, len: u8) -> Result<(), RadioError> {
        debug!("Updating payload length to {}", len);
        if len == self.payload_len {
            return Ok(());
        }
        self.payload_len = len;
        self.send_packet_params(len).await
    }

    /// Send the full SetPacketParams command with the given payload length
    async fn send_packet_params(&mut self, payload_len: u8) -> Result<(), RadioError> {
        self.radio.set_packet_params(&[payload_len]).await
    }
}

#[cfg(feature = "hal")]
impl<SPI: SpiDevice, TX: OutputPin, RX: OutputPin, EN: OutputPin> Configure
    for BpskRadio<'_, SPI, TX, RX, EN>
{
    type Config = BpskConfig;

    async fn configure(&mut self, config: &Self::Config) -> Result<(), RadioError> {
        self.config = *config;

        // Select BPSK packet type
        self.radio.set_packet_type(PacketType::Bpsk).await?;

        // Payload length updated per tx
        self.send_packet_params(0).await?;

        // RF frequency
        self.radio.set_rf_frequency(config.frequency).await?;

        // Set modulation params (bitrate + Gaussian BT 0.5 pulse shape)
        let br = config.bitrate.to_bytes();
        self.radio
            .set_modulation_params(&[br[0], br[1], br[2], 0x16])
            .await?;

        // PA config + TX power
        self.radio
            .set_output_power(config.pa, config.power_dbm, config.ramp)
            .await?;

        Ok(())
    }
}

#[cfg(feature = "hal")]
impl<SPI: SpiDevice, TX: OutputPin, RX: OutputPin, EN: OutputPin> Transmit
    for BpskRadio<'_, SPI, TX, RX, EN>
{
    async fn tx(&mut self, data: &[u8]) -> Result<(), RadioError> {
        let mut buf = [0u8; 255];
        // Convert buffer to packet with chosen framing
        let len = self.config.packet.to_bytes(data, &mut buf)?;

        // Write payload to radio buffer
        self.radio.set_buffer_base(0x00, 0x00).await?;
        self.radio.write_buffer(0x00, &buf[..len as usize]).await?;

        // Update packet params with actual payload length
        self.update_payload_len(len).await?;

        // Clear any stale IRQ flags before starting TX
        self.radio.clear_irq(irq::ALL).await?;

        // Enable TxDone IRQ on DIO1
        self.radio.set_dio1_irq(irq::TX_DONE | irq::TIMEOUT).await?;

        // Start TX
        self.radio.set_tx(0).await?;

        // Wait until it's done or until timeout
        let status = self.radio.poll_irq(irq::TX_DONE | irq::TIMEOUT).await?;
        if status & irq::TIMEOUT != 0 {
            return Err(RadioError::Timeout);
        }

        Ok(())
    }
}