// src/software_uart/uart_emulation.rs
use heapless::Vec;

#[derive(Clone, Copy, Debug, PartialEq, Eq)]
pub enum Parity {
    None,
    Even,
    Odd,
}

#[derive(Clone, Copy, Debug, PartialEq, Eq)]
pub enum StopBits {
    One,
    Two,
}

#[derive(Clone, Copy, Debug)]
pub struct UartConfig {
    pub data_bits: u8,
    pub parity: Parity,
    pub stop_bits: StopBits,
}

impl Default for UartConfig {
    fn default() -> Self {
        Self {
            data_bits: 8,
            parity: Parity::None,
            stop_bits: StopBits::One,
        }
    }
}

/// Encodes one byte into a sequence of GPIO BSRR words
pub fn encode_uart_byte_cfg(
    pin_bit: u8,
    data: u8,
    cfg: &UartConfig,
    out: &mut [u32; 12],
) -> usize {
    // GPIOx_BSRR register str. 636 kap. 13.4.7
    let set_high = |bit: u8| -> u32 { 1u32 << bit };
    // let set_low = |bit: u8| -> u32 { 0 }; // ODR
    let set_low = |bit: u8| -> u32 { 1u32 << (bit as u32 + 16) }; // BSRR

    let mut idx = 0usize;

    // START bit (LOW)
    out[idx] = set_low(pin_bit);
    idx += 1;

    // Data bits, LSB-first
    let nbits = cfg.data_bits.clamp(5, 8);
    for i in 0..nbits {
        let one = ((data >> i) & 1) != 0;
        out[idx] = if one { set_high(pin_bit) } else { set_low(pin_bit) };
        idx += 1;
    }

    // Parity
    match cfg.parity {
        Parity::None => {}
        Parity::Even | Parity::Odd => {
            let mask: u8 = if nbits == 8 { 0xFF } else { (1u16 << nbits) as u8 - 1 };
            let ones = (data & mask).count_ones() & 1;
            let par_bit_is_one = match cfg.parity {
                Parity::Even => ones == 1,
                Parity::Odd => ones == 0,
                _ => false,
            };
            out[idx] = if par_bit_is_one {
                set_high(pin_bit)
            } else {
                set_low(pin_bit)
            };
            idx += 1;
        }
    }

    // STOP bits (HIGH)
    let stop_ticks = match cfg.stop_bits {
        StopBits::One => 1usize,
        StopBits::Two => 2usize,
    };
    for _ in 0..stop_ticks {
        out[idx] = set_high(pin_bit);
        idx += 1;
    }

    idx
}

/// Decode an oversampled stream of logic levels into UART bytes.
/// Returns (decoded bytes, number of samples consumed/processed).
pub fn decode_uart_samples(
    samples: &[u8],
    oversample: u16,
    cfg: &UartConfig,
) -> (heapless::Vec<u8, 256>, usize) {
    let mut out = Vec::<u8, 256>::new();
    let mut idx = 0usize;
    let nbits = cfg.data_bits as usize;
    let ovs = oversample as usize;

    // Calculate total frame width in samples to ensure we have enough data
    // 1 start + n data + parity? + stops
    let parity_bits = match cfg.parity {
        Parity::None => 0,
        _ => 1,
    };
    let stop_bits_count = match cfg.stop_bits {
        StopBits::One => 1,
        StopBits::Two => 2,
    };
    let frame_bits = 1 + nbits + parity_bits + stop_bits_count;
    let frame_len = frame_bits * ovs;

    // Majority vote over 3 samples centered at `i`
    let get_bit = |i: usize| -> u8 {
        let mut votes = 0;
        // Check i-1, i, i+1. Saturating sub/add handles boundaries roughly.
        if i > 0 && samples.get(i - 1).map_or(true, |&x| x != 0) {
            votes += 1;
        }
        if samples.get(i).map_or(true, |&x| x != 0) {
            votes += 1;
        }
        if samples.get(i + 1).map_or(true, |&x| x != 0) {
            votes += 1;
        }

        if votes >= 2 {
            1
        } else {
            0
        }
    };

    // Loop while we have enough remaining samples for a full frame
    while idx + frame_len <= samples.len() {
        // Wait for falling edge (High -> Low)
        // samples[idx] == 1 (Idle/Stop) && samples[idx+1] == 0 (Start)
        if samples[idx] != 0 && samples[idx + 1] == 0 {
            // Align to center of START bit
            // Start bit begins at idx+1. Center is at idx + 1 + (ovs/2)
            let center_offset = 1 + (ovs / 2);
            let mut scan_idx = idx + center_offset;

            // Validate Start Bit
            if get_bit(scan_idx) != 0 {
                idx += 1; // False start (noise), move on
                continue;
            }

            // Move to center of first data bit
            scan_idx += ovs;

            // Read Data Bits
            let mut data: u8 = 0;
            for bit in 0..nbits {
                if get_bit(scan_idx) == 1 {
                    data |= 1 << bit;
                }
                scan_idx += ovs;
            }

            // Skip Parity
            if cfg.parity != Parity::None {
                scan_idx += ovs;
            }

            // Validate Stop Bit (Must be 1)
            // If stop bit is 0, it's a framing error. We reject the whole byte.
            if get_bit(scan_idx) == 0 {
                idx += 1; // Next sample
                continue;
            }

            // Byte is valid
            let _ = out.push(data);

            // Active Resync: Fast-forward through the stop bit(s) and idle time
            // scan_idx is currently at the center of the Stop bit.
            idx = scan_idx;
            // Advance while we are reading High (1).
            // As soon as we see Low (0), we stop. That 0 is the beginning of the NEXT start bit.
            // The outer loop expects `idx` to be the High *before* the start bit, so we will handle that.
            while idx < samples.len() && samples[idx] != 0 {
                idx += 1;
            }
            // Back up one step.
            // The outer loop logic is: `if samples[idx] != 0 && samples[idx+1] == 0`.
            // If we stopped at `idx` because it was 0, then `idx-1` was the last 1 (Idle).
            if idx > 0 {
                idx -= 1;
            }
        } else {
            // No start bit detected here, move to next sample
            idx += 1;
        }
    }

    (out, idx)
}