#[cfg(feature = "std")]
pub type UsbDataBuffer = Vec<u8>;
#[cfg(not(feature = "std"))]
pub type UsbDataBuffer = heapless::Vec<u8, 512>;

macro_rules! buffer_push {
    ($buffer:expr, $byte:expr) => {
        #[cfg(feature = "std")]
        $buffer.push($byte);
        #[cfg(not(feature = "std"))]
        $buffer.push($byte).unwrap();
    };
}

pub const CONTROL_CHAR_STX: u8 = 0x02;
pub const CONTROL_CHAR_ETX: u8 = 0x03;
pub const CONTROL_CHAR_DLE: u8 = 0x10;

pub const FRAME_SIZE_MIN: usize = 2; // (STX + ETX)

fn escaped_bytes(payload: &[u8]) -> UsbDataBuffer {
    let mut escaped_bytes = UsbDataBuffer::new();
    for byte in payload {
        if *byte == CONTROL_CHAR_STX || *byte == CONTROL_CHAR_ETX || *byte == CONTROL_CHAR_DLE {
            buffer_push!(escaped_bytes, CONTROL_CHAR_DLE);
        }
        buffer_push!(escaped_bytes, *byte);
    }
    escaped_bytes
}

fn unescape_bytes(escaped: &[u8]) -> UsbDataBuffer {
    let mut unescaped: UsbDataBuffer = UsbDataBuffer::new();
    let mut is_escaped = false;

    for &byte in escaped {
        if is_escaped {
            buffer_push!(unescaped, byte);
            is_escaped = false;
        } else if byte == CONTROL_CHAR_DLE {
            is_escaped = true;
        } else {
            buffer_push!(unescaped, byte);
        }
    }
    unescaped
}

fn frame_bytes(payload: &[u8]) -> UsbDataBuffer {
    let mut frame_bytes = UsbDataBuffer::new();
    buffer_push!(frame_bytes, CONTROL_CHAR_STX);
    frame_bytes.extend(escaped_bytes(payload));
    buffer_push!(frame_bytes, CONTROL_CHAR_ETX);
    frame_bytes
}

#[allow(clippy::result_large_err)]
fn unframe_bytes(frame: &[u8]) -> Result<UsbDataBuffer, crate::Error> {
    // Check minimum frame size
    if frame.len() < FRAME_SIZE_MIN {
        return Err(crate::Error::InsufficientData);
    }

    // Verify STX and ETX
    if frame[0] != CONTROL_CHAR_STX || frame[frame.len() - 1] != CONTROL_CHAR_ETX {
        return Err(crate::Error::InsufficientData);
    }

    // Extract and unescape the payload between STX and ETX
    let payload = &frame[1..frame.len() - 1];
    Ok(unescape_bytes(payload))
}

impl TryFrom<UsbDataBuffer> for crate::message::Message {
    type Error = crate::Error;
    fn try_from(bytes: UsbDataBuffer) -> Result<Self, Self::Error> {
        let unframed_bytes = unframe_bytes(&bytes)?;
        postcard::from_bytes(&unframed_bytes).map_err(|_| crate::Error::Serialization)
    }
}

impl TryFrom<crate::message::Message> for UsbDataBuffer {
    type Error = crate::Error;
    fn try_from(message: crate::message::Message) -> Result<Self, Self::Error> {
        #[cfg(feature = "std")]
        let message_bytes: UsbDataBuffer =
            postcard::to_allocvec(&message).map_err(|_| crate::Error::Serialization)?;
        #[cfg(not(feature = "std"))]
        let message_bytes =
            postcard::to_vec::<_, 1024>(&message).map_err(|_| crate::Error::Serialization)?;
        let framed_bytes = frame_bytes(&message_bytes);
        Ok(framed_bytes)
    }
}
