zendo-protocol 0.1.2

//! Pure encoding of typed frames back into binary form.
//!
//! These mirror [`decode`](crate::decode()) exactly and are used to build test
//! fixtures and round-trip tests. They never allocate: each returns a
//! fixed-size array.

use crate::constants::{
    BODY_JOINT_COUNT, BODY_LANDMARK_COUNT, F64_BYTES, HAND_JOINT_COUNT, HAND_LANDMARK_COUNT,
    MSG_BODY_LANDMARK, MSG_BODY_QUATERNION, MSG_HAND_LANDMARK, MSG_HAND_QUATERNION, MSG_HELLO,
    TUPLE_BYTES,
};
use crate::frames::{
    BodyLandmarkFrame, BodyQuaternionFrame, HandLandmarkFrame, HandQuaternionFrame,
};
use crate::types::{HandSide, Landmark, Quaternion};

/// Encoded size of a body-quaternion frame, in bytes.
pub const ENCODED_BODY_QUATERNION_LEN: usize = 1 + BODY_JOINT_COUNT * TUPLE_BYTES;

/// Encoded size of a body-landmark frame, in bytes.
pub const ENCODED_BODY_LANDMARK_LEN: usize = 1 + BODY_LANDMARK_COUNT * TUPLE_BYTES;

/// Encoded size of a hand-quaternion frame, in bytes.
pub const ENCODED_HAND_QUATERNION_LEN: usize = 2 + HAND_JOINT_COUNT * TUPLE_BYTES;

/// Encoded size of a hand-landmark frame, in bytes.
pub const ENCODED_HAND_LANDMARK_LEN: usize = 2 + HAND_LANDMARK_COUNT * TUPLE_BYTES;

/// Encoded size of a hello frame, in bytes (tag + `u16` version).
pub const ENCODED_HELLO_LEN: usize = 1 + 2;

/// Encodes a body-quaternion frame (`0x02`).
pub fn encode_body_quaternions(frame: &BodyQuaternionFrame) -> [u8; ENCODED_BODY_QUATERNION_LEN] {
    let mut out = [0u8; ENCODED_BODY_QUATERNION_LEN];
    out[0] = MSG_BODY_QUATERNION;
    write_quaternions(&mut out[1..], &frame.to_array());
    out
}

/// Encodes a body-landmark frame (`0x03`).
pub fn encode_body_landmarks(frame: &BodyLandmarkFrame) -> [u8; ENCODED_BODY_LANDMARK_LEN] {
    let mut out = [0u8; ENCODED_BODY_LANDMARK_LEN];
    out[0] = MSG_BODY_LANDMARK;
    write_landmarks(&mut out[1..], &frame.to_array());
    out
}

/// Encodes a hand-quaternion frame (`0x04`).
pub fn encode_hand_quaternions(
    side: HandSide,
    frame: &HandQuaternionFrame,
) -> [u8; ENCODED_HAND_QUATERNION_LEN] {
    let mut out = [0u8; ENCODED_HAND_QUATERNION_LEN];
    out[0] = MSG_HAND_QUATERNION;
    out[1] = side.as_byte();
    write_quaternions(&mut out[2..], &frame.to_array());
    out
}

/// Encodes a hand-landmark frame (`0x05`).
pub fn encode_hand_landmarks(
    side: HandSide,
    frame: &HandLandmarkFrame,
) -> [u8; ENCODED_HAND_LANDMARK_LEN] {
    let mut out = [0u8; ENCODED_HAND_LANDMARK_LEN];
    out[0] = MSG_HAND_LANDMARK;
    out[1] = side.as_byte();
    write_landmarks(&mut out[2..], &frame.to_array());
    out
}

/// Encodes a hello frame (`0x01`) announcing the protocol version.
pub fn encode_hello(protocol_version: u16) -> [u8; ENCODED_HELLO_LEN] {
    let mut out = [0u8; ENCODED_HELLO_LEN];
    out[0] = MSG_HELLO;
    out[1..].copy_from_slice(&protocol_version.to_le_bytes());
    out
}

fn write_quaternions(dst: &mut [u8], quats: &[Quaternion]) {
    for (i, q) in quats.iter().enumerate() {
        let base = i * TUPLE_BYTES;
        write_f64(dst, base, q.w);
        write_f64(dst, base + F64_BYTES, q.x);
        write_f64(dst, base + 2 * F64_BYTES, q.y);
        write_f64(dst, base + 3 * F64_BYTES, q.z);
    }
}

fn write_landmarks(dst: &mut [u8], landmarks: &[Landmark]) {
    for (i, lm) in landmarks.iter().enumerate() {
        let base = i * TUPLE_BYTES;
        write_f64(dst, base, lm.x);
        write_f64(dst, base + F64_BYTES, lm.y);
        write_f64(dst, base + 2 * F64_BYTES, lm.z);
        write_f64(dst, base + 3 * F64_BYTES, lm.confidence);
    }
}

fn write_f64(dst: &mut [u8], offset: usize, value: f64) {
    dst[offset..offset + F64_BYTES].copy_from_slice(&value.to_le_bytes());
}

#[cfg(test)]
mod tests {
    use super::*;
    use crate::decode::{decode, decode_hello, Message};

    fn sample_quaternion_frame() -> BodyQuaternionFrame {
        let mut quats = [Quaternion::default(); BODY_JOINT_COUNT];
        for (i, q) in quats.iter_mut().enumerate() {
            let f = i as f64;
            *q = Quaternion {
                w: f,
                x: f + 0.1,
                y: f + 0.2,
                z: f + 0.3,
            };
        }
        BodyQuaternionFrame::from_array(quats)
    }

    fn sample_landmark_frame() -> HandLandmarkFrame {
        let mut landmarks = [Landmark::default(); HAND_LANDMARK_COUNT];
        for (i, lm) in landmarks.iter_mut().enumerate() {
            let f = i as f64;
            *lm = Landmark {
                x: f,
                y: -f,
                z: f * 2.0,
                confidence: 0.5,
            };
        }
        HandLandmarkFrame::from_array(landmarks)
    }

    #[test]
    fn body_quaternions_round_trip() {
        // Arrange
        let frame = sample_quaternion_frame();

        // Act
        let encoded = encode_body_quaternions(&frame);
        let decoded = decode(&encoded).expect("round-trips");

        // Assert
        assert_eq!(decoded, Message::BodyQuaternions(frame));
    }

    #[test]
    fn hand_landmarks_round_trip() {
        // Arrange
        let frame = sample_landmark_frame();

        // Act
        let encoded = encode_hand_landmarks(HandSide::Left, &frame);
        let decoded = decode(&encoded).expect("round-trips");

        // Assert
        assert_eq!(
            decoded,
            Message::HandLandmarks {
                side: HandSide::Left,
                frame
            }
        );
    }

    #[test]
    fn hello_round_trips() {
        // Arrange / Act
        let encoded = encode_hello(7);
        let version = decode_hello(&encoded).expect("round-trips");

        // Assert
        assert_eq!(version, 7);
    }

    #[test]
    fn encoded_lengths_are_correct() {
        // Arrange / Act / Assert
        assert_eq!(ENCODED_BODY_QUATERNION_LEN, 417);
        assert_eq!(ENCODED_BODY_LANDMARK_LEN, 609);
        assert_eq!(ENCODED_HAND_QUATERNION_LEN, 514);
        assert_eq!(ENCODED_HAND_LANDMARK_LEN, 674);
        assert_eq!(ENCODED_HELLO_LEN, 3);
    }
}