path: root/src/protocol/parser.rs

//! UPP protocol parser for decoding USB frames

use super::frame::{UPPFlags, UPPFrame, UPPFrameHeader, UPPUsbFrame};
use crate::error::{ProtocolError, Result};
use std::mem;
use tracing::{debug, trace, warn};

/// UPP protocol parser
pub struct UPPParser {
    enable_debug: bool,
}

impl UPPParser {
    /// Create a new UPP parser
    pub fn new() -> Self {
        Self {
            enable_debug: false,
        }
    }

    /// Create a new UPP parser with debug enabled
    pub fn new_with_debug(enable_debug: bool) -> Self {
        Self { enable_debug }
    }

    /// Parse a raw USB frame into a UPP frame
    pub fn parse_frame(&self, data: &[u8]) -> Result<UPPFrame> {
        trace!("Parsing UPP frame: {} bytes", data.len());

        // Parse USB frame header
        let usb_frame = self.parse_usb_header(data)?;

        // Validate frame length - based on C++ POC analysis
        let expected_total_size = usb_frame.total_size();
        let actual_size = data.len();
        
        if self.enable_debug {
            trace!(
                "Frame size validation: expected={}, actual={}, difference={}",
                expected_total_size, actual_size, actual_size as i32 - expected_total_size as i32
            );
        }
        
        // Based on C++ POC: the length field represents total payload size
        // If we have less data than expected, this is a partial frame (chunked)
        if actual_size < expected_total_size {
            return Err(ProtocolError::FrameTooSmall {
                expected: expected_total_size,
                actual: actual_size,
            }
            .into());
        }

        // Parse camera frame header
        let camera_header = self.parse_camera_header(data, &usb_frame)?;

        // Extract frame data - use the expected data size
        let data_start = mem::size_of::<UPPUsbFrame>() + 7; // Manual header size: 7 bytes
        let frame_data = if data_start < data.len() {
            let end_index = std::cmp::min(data_start + usb_frame.expected_data_size(), data.len());
            data[data_start..end_index].to_vec()
        } else {
            Vec::new()
        };

        if self.enable_debug {
            trace!(
                "Parsed UPP frame: ID={}, Camera={}, Data={} bytes, Total={} bytes, Expected={} bytes",
                camera_header.frame_id,
                camera_header.camera_number,
                frame_data.len(),
                data.len(),
                expected_total_size
            );
        }

        Ok(UPPFrame {
            header: camera_header,
            data: frame_data,
        })
    }

    /// Parse a raw USB frame with better error handling and diagnostics
    pub fn parse_frame_robust(&self, data: &[u8]) -> Result<UPPFrame> {
        trace!("Parsing UPP frame robustly: {} bytes", data.len());

        // First, try to find a valid frame start
        let frame_start = self.find_frame_start(data)?;
        let frame_data = &data[frame_start..];
        
        if self.enable_debug {
            debug!("Found frame start at offset {}, processing {} bytes", frame_start, frame_data.len());
        }

        // Parse the frame from the found start position
        self.parse_frame(frame_data)
    }

    /// Find the start of a valid UPP frame in potentially misaligned data
    fn find_frame_start(&self, data: &[u8]) -> Result<usize> {
        if data.len() < 12 { // Minimum frame size
            return Err(ProtocolError::FrameTooSmall {
                expected: 12,
                actual: data.len(),
            }
            .into());
        }

        // Look for the magic number (0xBBAA) in the data
        for i in 0..=data.len().saturating_sub(12) {
            if data.len() >= i + 2 {
                let magic_bytes = [data[i], data[i + 1]];
                let magic = u16::from_le_bytes(magic_bytes);
                
                if magic == super::UPP_USB_MAGIC {
                    if self.enable_debug {
                        debug!("Found magic number 0x{:04X} at offset {}", magic, i);
                    }
                    return Ok(i);
                }
            }
        }

        Err(ProtocolError::InvalidMagic {
            expected: super::UPP_USB_MAGIC,
            actual: 0, // Unknown
        }
        .into())
    }

    /// Parse USB frame header
    pub(crate) fn parse_usb_header(&self, data: &[u8]) -> Result<UPPUsbFrame> {
        if data.len() < mem::size_of::<UPPUsbFrame>() {
            return Err(ProtocolError::FrameTooSmall {
                expected: mem::size_of::<UPPUsbFrame>(),
                actual: data.len(),
            }
            .into());
        }

        // Safe to transmute since we've checked the size
        let usb_frame = unsafe { *(data.as_ptr() as *const UPPUsbFrame) };

        // Validate magic number
        if !usb_frame.is_valid() {
            return Err(ProtocolError::InvalidMagic {
                expected: super::UPP_USB_MAGIC,
                actual: usb_frame.magic,
            }
            .into());
        }

        // Validate camera ID
        if usb_frame.cid != super::UPP_CAMID_7 {
            return Err(ProtocolError::UnknownCameraId(usb_frame.cid).into());
        }

        if self.enable_debug {
            let magic = usb_frame.magic;
            let cid = usb_frame.cid;
            let length = usb_frame.length;
            trace!(
                "USB frame: magic=0x{:04X}, cid={}, length={}",
                magic, cid, length
            );
        }

        Ok(usb_frame)
    }

    /// Parse camera frame header
    pub(crate) fn parse_camera_header(&self, data: &[u8], _usb_frame: &UPPUsbFrame) -> Result<UPPFrameHeader> {
        let header_offset = mem::size_of::<UPPUsbFrame>();
        // Manual header size: frame_id(1) + camera_number(1) + flags(1) + g_sensor(4) = 7 bytes
        let header_end = header_offset + 7;

        if data.len() < header_end {
            return Err(ProtocolError::FrameTooSmall {
                expected: header_end,
                actual: data.len(),
            }
            .into());
        }

        // Manually parse the header to avoid packed struct issues
        let frame_id = data[header_offset];
        let camera_number = data[header_offset + 1];
        
        // Read flags byte and extract individual bits
        let flags_byte = data[header_offset + 2];
        let has_g = (flags_byte & 0x01) != 0;        // Bit 0
        let button_press = (flags_byte & 0x02) != 0; // Bit 1
        let other = (flags_byte >> 2) & 0x3F;        // Bits 2-7 (6 bits)

        if self.enable_debug {
            trace!(
                "Raw flags byte: 0x{:02X}, has_g={}, button_press={}, other=0x{:02X}",
                flags_byte, has_g, button_press, other
            );
        }

        // Read G-sensor data (4 bytes, little-endian)
        let g_sensor_bytes = [
            data[header_offset + 3],
            data[header_offset + 4],
            data[header_offset + 5],
            data[header_offset + 6],
        ];

        if self.enable_debug {
            trace!(
                "G-sensor bytes: {:02X} {:02X} {:02X} {:02X}",
                g_sensor_bytes[0], g_sensor_bytes[1], g_sensor_bytes[2], g_sensor_bytes[3]
            );
        }

        let g_sensor = u32::from_le_bytes(g_sensor_bytes);

        let camera_header = UPPFrameHeader {
            frame_id,
            camera_number,
            flags: UPPFlags {
                has_g,
                button_press,
                other,
            },
            g_sensor,
        };

        // Validate frame header
        self.validate_camera_header(&camera_header)?;

        if self.enable_debug {
            trace!(
                "Camera header: frame_id={}, camera={}, has_g={}, button={}",
                camera_header.frame_id,
                camera_header.camera_number,
                camera_header.flags.has_g,
                camera_header.flags.button_press
            );
        }

        Ok(camera_header)
    }

    /// Validate camera frame header
    fn validate_camera_header(&self, header: &UPPFrameHeader) -> Result<()> {
        // Validate camera number (should be 0 or 1)
        if header.camera_number >= 2 {
            return Err(ProtocolError::InvalidFrameFormat(format!(
                "Invalid camera number: {}",
                header.camera_number
            ))
            .into());
        }

        // Validate flags (G-sensor and other flags should be 0 for now)
        if header.flags.has_g {
            warn!("G-sensor data not yet supported");
        }

        if header.flags.other != 0 {
            warn!("Unknown flags set: 0x{:02X}", header.flags.other);
        }

        Ok(())
    }

    /// Get frame statistics
    pub fn get_stats(&self) -> UPPParserStats {
        UPPParserStats {
            enable_debug: self.enable_debug,
        }
    }
}

impl Default for UPPParser {
    fn default() -> Self {
        Self::new()
    }
}

/// UPP parser statistics
#[derive(Debug, Clone)]
pub struct UPPParserStats {
    pub enable_debug: bool,
}

#[cfg(test)]
mod tests {
    use super::super::UPP_USB_MAGIC;
    use super::*;
    use crate::error::Error;

    // Create test data for a valid UPP frame
    fn create_test_frame(frame_id: u8, camera_number: u8, data_size: usize) -> Vec<u8> {
        let mut frame = Vec::new();

        // USB header (5 bytes)
        frame.extend_from_slice(&UPP_USB_MAGIC.to_le_bytes());
        frame.push(7); // Camera ID
        frame.extend_from_slice(&(data_size as u16 + 7).to_le_bytes()); // Length (camera header + data)

        // Camera header (7 bytes)
        frame.push(frame_id);
        frame.push(camera_number);
        // Flags struct (1 byte - packed bit fields)
        let flags_byte = 0u8; // has_g: false, button_press: false, other: 0
        frame.push(flags_byte);
        frame.extend_from_slice(&0u32.to_le_bytes()); // G-sensor data (4 bytes)

        // Frame data
        frame.extend(std::iter::repeat(0xAA).take(data_size));

        frame
    }

    #[test]
    fn test_upp_parser_creation() {
        let parser = UPPParser::new();
        assert!(!parser.enable_debug);

        let parser = UPPParser::new_with_debug(true);
        assert!(parser.enable_debug);
    }

    #[test]
    fn test_upp_parser_parse_valid_frame() {
        let parser = UPPParser::new();
        let test_frame = create_test_frame(1, 0, 10);

        let result = parser.parse_frame(&test_frame);
        assert!(result.is_ok());

        let frame = result.unwrap();
        assert_eq!(frame.frame_id(), 1);
        assert_eq!(frame.camera_number(), 0);
        assert_eq!(frame.data.len(), 10);
        assert!(!frame.button_pressed());
        assert!(frame.g_sensor_data().is_none());
    }

    #[test]
    fn test_upp_parser_frame_too_small() {
        let parser = UPPParser::new();
        let short_data = &[0xFF, 0xAA]; // Too short for any header

        let result = parser.parse_frame(short_data);
        assert!(result.is_err());
        assert!(matches!(
            result.unwrap_err(),
            Error::Protocol(ProtocolError::FrameTooSmall { .. })
        ));
    }

    #[test]
    fn test_upp_parser_invalid_magic() {
        let parser = UPPParser::new();
        let mut test_frame = create_test_frame(1, 0, 5);
        test_frame[0] = 0x12; // Corrupt magic

        let result = parser.parse_frame(&test_frame);
        assert!(result.is_err());
        assert!(matches!(
            result.unwrap_err(),
            Error::Protocol(ProtocolError::InvalidMagic { .. })
        ));
    }

    #[test]
    fn test_upp_parser_unknown_camera_id() {
        let parser = UPPParser::new();
        let mut test_frame = create_test_frame(1, 0, 5);
        test_frame[2] = 5; // Unknown camera ID

        let result = parser.parse_frame(&test_frame);
        assert!(result.is_err());
        assert!(matches!(
            result.unwrap_err(),
            Error::Protocol(ProtocolError::UnknownCameraId(5))
        ));
    }

    #[test]
    fn test_upp_parser_button_press() {
        let parser = UPPParser::new();
        let mut test_frame = create_test_frame(1, 0, 5);
        test_frame[7] = 0x02; // Set button press flag (bit 1 of flags byte at index 7)

        let result = parser.parse_frame(&test_frame);
        assert!(result.is_ok());

        let frame = result.unwrap();
        assert!(frame.button_pressed());
    }

    #[test]
    fn test_upp_parser_g_sensor() {
        let parser = UPPParser::new();
        let mut test_frame = create_test_frame(1, 0, 5);
        test_frame[7] = 0x01; // Set G-sensor flag (bit 0 of flags byte at index 7)
        test_frame[8..12].copy_from_slice(&0x12345678u32.to_le_bytes()); // G-sensor data (indices 8-11)

        let result = parser.parse_frame(&test_frame);
        assert!(result.is_ok());

        let frame = result.unwrap();
        assert!(frame.g_sensor_data().is_some());
        assert_eq!(frame.g_sensor_data().unwrap(), 0x12345678);
    }

    #[test]
    fn test_upp_parser_stats() {
        let parser = UPPParser::new();
        let stats = parser.get_stats();
        assert!(!stats.enable_debug);
    }
}