embedded-camsense-x1 0.1.0

use crate::constants::{
    NUMBER_OF_POINTS_PER_MEASUREMENT, NUMBER_OF_POINTS_PER_SCAN, PAYLOAD_SIZE_IN_BYTES,
};

/// Errors that can occur during Camsense-X1 communication or packet parsing.
#[derive(Clone, Copy, Debug)]
#[cfg_attr(feature = "defmt", derive(defmt::Format))]
pub enum Error<E> {
    /// Wrapped UART Error
    UART(E),
    /// Checksum mismatch error
    ChecksumMismatch(u32, u32),
    /// Other error
    Other,
}

/// Raw distance measurement extracted from a single LiDAR packet.
#[derive(Clone, Copy, Debug)]
#[cfg_attr(feature = "defmt", derive(defmt::Format))]
pub struct RawDistance {
    /// 14-bit unsigned distance in mm, top 2 bits masked off
    value: u16,
    /// Signal quality/intensity indicator (0-255).
    quality: u8,
    /// true = invalid return (bit 7 of high byte)
    flag: bool,
}

/// Parsed raw data from a single 36-byte LiDAR packet.
#[derive(Clone, Copy, Debug)]
#[cfg_attr(feature = "defmt", derive(defmt::Format))]
pub struct RawMeasurement {
    pub speed: u16,
    /// Start angle of measurement in degrees
    pub start_angle: u16,
    /// End angle of measurement in degrees
    pub end_angle: u16,
    /// Array of distance measurements
    pub distances: [RawDistance; NUMBER_OF_POINTS_PER_MEASUREMENT],
    /// 16-bit Checksum
    pub checksum: u16,
}

/// Verifies the Camsense-X1 checksum.
/// Accepts the 36-byte payload (header, data, checksum).
///
/// The exact algorithm was taken from the official Camsense-X1 C++ SDK:
/// https://github.com/camsense/SDK_V3.0/blob/17e0264302e2ca4cf14d5402af7437d16a37ab95/src/base/ReadParsePackage.cpp#L148
#[inline]
pub fn check_lidar_checksum(data: &[u8; PAYLOAD_SIZE_IN_BYTES]) -> Result<(), Error<()>> {
    let mut accumulator: u32 = 0;

    // Process all words in the slice
    let num_data_words = data.len() / 2 - 1;
    for i in 0..num_data_words {
        let word = u16::from_le_bytes([data[2 * i], data[2 * i + 1]]);
        accumulator = (accumulator << 1) + word as u32;
    }

    // 15-bit folding: equivalent to acc % 32767
    let computed_checksum = ((accumulator & 0x7FFF) + (accumulator >> 15)) & 0x7FFF;

    // Compare with the last word (checksum) in Little-Endian
    let expected_checksum = u16::from_le_bytes([data[data.len() - 2], data[data.len() - 1]]) as u32;

    if computed_checksum == expected_checksum {
        Ok(())
    } else {
        Err(Error::ChecksumMismatch(
            expected_checksum,
            computed_checksum,
        ))
    }
}

impl TryFrom<[u8; 36]> for RawMeasurement {
    type Error = Error<()>; // No UART context during pure byte parsing
    fn try_from(data: [u8; PAYLOAD_SIZE_IN_BYTES]) -> Result<Self, Self::Error> {
        // Validate checksum
        check_lidar_checksum(&data)?;

        let speed = u16::from_le_bytes([data[4], data[5]]);
        let start_angle = u16::from_le_bytes([data[6], data[7]]);
        let end_angle = u16::from_le_bytes([data[32], data[33]]);
        let checksum = u16::from_le_bytes([data[34], data[35]]);

        let mut distances = [RawDistance {
            value: 0,
            quality: 0,
            flag: false,
        }; 8];
        for i in 0..8 {
            let distance_bytes = [data[8 + i * 3], data[9 + i * 3], data[10 + i * 3]];
            // Mask left-most bit of second byte
            let value_bytes = [distance_bytes[0], distance_bytes[1] & 0x3F];
            let value = u16::from_le_bytes(value_bytes);
            let quality = distance_bytes[2];
            // Flag this value as invalid if the flag bit is set
            let flag = (distance_bytes[1] >> 7) & 0x01 != 0;

            let distance = RawDistance {
                value,
                quality,
                flag,
            };
            distances[i] = distance;
        }

        Ok(Self {
            speed,
            start_angle,
            end_angle,
            distances,
            checksum,
        })
    }
}

/// A single validated LiDAR point with computed angle.
#[derive(Clone, Copy, Default, Debug)]
#[cfg_attr(feature = "defmt", derive(defmt::Format))]
pub struct Point {
    pub distance: u16,
    pub angle: f32,
}

/// A single partial scan containing up to [`NUMBER_OF_POINTS_PER_MEASUREMENT`] valid points.
///
/// Represents the decoded and filtered data from one raw LiDAR packet.
/// Invalid points, zero-quality returns, and flagged measurements are represented as None.
#[derive(Clone, Copy, Debug)]
#[cfg_attr(feature = "defmt", derive(defmt::Format))]
pub struct PartialScan {
    /// Rotation frequency in Hz.
    pub frequency: f32,
    /// Start angle of the packet in degrees.
    pub start_angle: f32,
    /// End angle of the packet in degrees.
    pub end_angle: f32,
    /// Array of [`NUMBER_OF_POINTS_PER_MEASUREMENT`] points within this partial scan.
    pub points: [Option<Point>; NUMBER_OF_POINTS_PER_MEASUREMENT],
}

impl From<(RawMeasurement, f32)> for PartialScan {
    fn from((raw, angle_offset): (RawMeasurement, f32)) -> Self {
        let frequency = raw.speed as f32 / 3840.0;
        let start_angle = raw.start_angle as f32 / 64.0 - 640.0;
        let end_angle = raw.end_angle as f32 / 64.0 - 640.0;
        let step = if end_angle > start_angle {
            (end_angle - start_angle) / 7.0
        } else {
            (end_angle - (start_angle - 360.0)) / 7.0
        };

        let mut points = [None; NUMBER_OF_POINTS_PER_MEASUREMENT];
        for (i, raw_distance) in raw.distances.iter().enumerate() {
            if raw_distance.flag || raw_distance.quality == 0 || raw_distance.value == 0 {
                continue;
            }

            let angle = (start_angle + step * i as f32 + angle_offset) % 360.0;
            points[i] = Some(Point {
                distance: raw_distance.value,
                angle,
            });
        }
        Self {
            frequency,
            start_angle,
            end_angle,
            points,
        }
    }
}

/// A complete 360° LiDAR scan.
///
/// Aggregates multiple partial scans into a fixed-size array of [`NUMBER_OF_POINTS_PER_SCAN`] points.
#[derive(Clone, Copy, Debug)]
#[cfg_attr(feature = "defmt", derive(defmt::Format))]
pub struct Scan {
    /// Full array of [`NUMBER_OF_POINTS_PER_SCAN`] points. `None` indicates no valid return at that angle/index.
    pub points: [Option<Point>; NUMBER_OF_POINTS_PER_SCAN],
}