hermes_ble/

parse.rs

// SPDX-License-Identifier: GPL-3.0-or-later
// Copyright © 2026 Eugene Hauptmann, Frédéric Simard

//! Binary decoders for Hermes V1 BLE notification payloads.
//!
//! All public functions in this module are pure (no I/O, no allocation beyond
//! the returned collections) and are safe to call from any async or sync context.

use crate::protocol::{
    ads1299_to_microvolts, ADS1299_GAIN, ADS1299_VREF, ACC_SENSITIVITY, EEG_FRAME_BYTES,
    EEG_NUM_CHANNELS, GYRO_SENSITIVITY, MAG_SENSITIVITY,
};
use crate::types::{EegSample, MotionData, XyzSample};

// ── EEG ──────────────────────────────────────────────────────────────────────

/// Decode a signed 24-bit big-endian integer from 3 bytes.
///
/// The ADS1299 encodes each channel sample as a 24-bit two's-complement
/// big-endian integer.
///
/// # Example
///
/// ```
/// # use hermes_ble::parse::decode_i24_be;
/// assert_eq!(decode_i24_be(&[0x00, 0x03, 0xE8]), 1000);
/// assert_eq!(decode_i24_be(&[0xFF, 0xFF, 0xFF]), -1);
/// ```
pub fn decode_i24_be(bytes: &[u8]) -> i32 {
    debug_assert!(bytes.len() >= 3);
    let unsigned = ((bytes[0] as u32) << 16) | ((bytes[1] as u32) << 8) | (bytes[2] as u32);
    // Sign-extend from 24 bits to 32 bits.
    if unsigned & 0x80_0000 != 0 {
        (unsigned | 0xFF00_0000) as i32
    } else {
        unsigned as i32
    }
}

/// Parse one EEG BLE notification into a vector of [`EegSample`]s.
///
/// # Wire format
///
/// ```text
/// byte[0]        packet index (0–127)
/// bytes[1..]     N sample frames, each 24 bytes (8 channels × 3 bytes)
/// ```
///
/// Trailing bytes that do not form a complete 24-byte frame are silently
/// ignored (a warning is logged).
///
/// `timestamp` is the wall-clock time in milliseconds since epoch for the
/// notification arrival; individual sample timestamps are not available from
/// the device, so all samples in one packet share the same timestamp.
///
/// Returns an empty `Vec` if the payload is too short to contain even one
/// complete sample frame.
pub fn parse_eeg_packet(data: &[u8], timestamp: f64) -> Vec<EegSample> {
    if data.len() < 1 + EEG_FRAME_BYTES {
        return vec![];
    }

    let packet_index = data[0];
    let payload = &data[1..];
    let n_complete = payload.len() / EEG_FRAME_BYTES;

    if payload.len() % EEG_FRAME_BYTES != 0 {
        log::warn!(
            "EEG packet {packet_index}: {} bytes payload is not a multiple of {EEG_FRAME_BYTES}; \
             dropping {} trailing byte(s)",
            payload.len(),
            payload.len() % EEG_FRAME_BYTES,
        );
    }

    (0..n_complete)
        .map(|sample_idx| {
            let offset = sample_idx * EEG_FRAME_BYTES;
            let mut channels = [0.0_f64; EEG_NUM_CHANNELS];
            for ch in 0..EEG_NUM_CHANNELS {
                let raw = decode_i24_be(&payload[offset + ch * 3..]);
                channels[ch] = ads1299_to_microvolts(raw, ADS1299_GAIN, ADS1299_VREF);
            }
            EegSample {
                packet_index,
                sample_index: sample_idx,
                timestamp,
                channels,
            }
        })
        .collect()
}

/// Detect missing packets between `last` and `current` in the 0–127 ring.
///
/// Returns a list of the missing packet indices.  Returns an empty list when
/// `last` is `None` (first packet) or `current` is the expected successor.
///
/// # Example
///
/// ```
/// # use hermes_ble::parse::detect_missing_packets;
/// assert!(detect_missing_packets(None, 5).is_empty());
/// assert!(detect_missing_packets(Some(5), 6).is_empty());
/// assert_eq!(detect_missing_packets(Some(5), 8), vec![6, 7]);
/// assert_eq!(detect_missing_packets(Some(126), 1), vec![127, 0]);
/// ```
pub fn detect_missing_packets(last: Option<u8>, current: u8) -> Vec<u8> {
    let Some(last) = last else {
        return vec![];
    };
    let expected = (last.wrapping_add(1)) % 128;
    if expected == current {
        return vec![];
    }
    let mut missing = Vec::new();
    let mut idx = expected;
    while idx != current {
        missing.push(idx);
        idx = (idx.wrapping_add(1)) % 128;
    }
    missing
}

// ── Motion ────────────────────────────────────────────────────────────────────

/// Parse a 9-DOF motion notification into a [`MotionData`].
///
/// Wire format: 18 bytes = 9 × `i16` little-endian:
/// `[ax, ay, az, gx, gy, gz, mx, my, mz]`
///
/// Returns `None` if the payload is shorter than 18 bytes.
pub fn parse_motion(data: &[u8], timestamp: f64) -> Option<MotionData> {
    if data.len() < 18 {
        return None;
    }
    let i16_at = |off: usize| -> i16 { i16::from_le_bytes([data[off], data[off + 1]]) };

    let ax = i16_at(0) as f32 * ACC_SENSITIVITY;
    let ay = i16_at(2) as f32 * ACC_SENSITIVITY;
    let az = i16_at(4) as f32 * ACC_SENSITIVITY;

    let gx = i16_at(6) as f32 * GYRO_SENSITIVITY;
    let gy = i16_at(8) as f32 * GYRO_SENSITIVITY;
    let gz = i16_at(10) as f32 * GYRO_SENSITIVITY;

    let mx = i16_at(12) as f32 * MAG_SENSITIVITY;
    let my = i16_at(14) as f32 * MAG_SENSITIVITY;
    let mz = i16_at(16) as f32 * MAG_SENSITIVITY;

    Some(MotionData {
        timestamp,
        accel: XyzSample {
            x: ax,
            y: ay,
            z: az,
        },
        gyro: XyzSample {
            x: gx,
            y: gy,
            z: gz,
        },
        mag: XyzSample {
            x: mx,
            y: my,
            z: mz,
        },
    })
}

#[cfg(test)]
mod tests {
    use super::*;

    #[test]
    fn decode_i24_be_positive() {
        assert_eq!(decode_i24_be(&[0x00, 0x03, 0xE8]), 1000);
    }

    #[test]
    fn decode_i24_be_negative() {
        assert_eq!(decode_i24_be(&[0xFF, 0xFF, 0xFF]), -1);
        assert_eq!(decode_i24_be(&[0xFF, 0xFC, 0x18]), -1000);
    }

    #[test]
    fn detect_missing_none() {
        assert!(detect_missing_packets(None, 0).is_empty());
    }

    #[test]
    fn detect_missing_sequential() {
        assert!(detect_missing_packets(Some(0), 1).is_empty());
        assert!(detect_missing_packets(Some(127), 0).is_empty());
    }

    #[test]
    fn detect_missing_gap() {
        assert_eq!(detect_missing_packets(Some(5), 8), vec![6, 7]);
    }

    #[test]
    fn detect_missing_wrap() {
        assert_eq!(detect_missing_packets(Some(126), 1), vec![127, 0]);
    }

    #[test]
    fn ads1299_scale() {
        let uv = ads1299_to_microvolts(1, 12.0, 4.5);
        // LSB = (2 * 4.5 * 1e6) / (12 * 2^24) ≈ 0.04470 µV
        assert!((uv - 0.04470).abs() < 0.001);
    }
}