cu-feetech 0.15.0

//! Feetech STS/SCS serial bus servo bridge for Copper.
//!
//! This crate provides a [`CuBridge`] that communicates with Feetech STS/SCS
//! serial bus servos (such as the **STS3215** used in SO-100 / SO-101 robot
//! arms) over a half-duplex serial (UART) bus.
//!
//! # Protocol overview
//!
//! Feetech servos use a Dynamixel-style packet protocol:
//!
//! ```text
//! TX:  [0xFF 0xFF] [ID] [LENGTH] [INSTRUCTION] [PARAM …] [CHECKSUM]
//! RX:  [0xFF 0xFF] [ID] [LENGTH] [ERROR]       [DATA …]  [CHECKSUM]
//! ```
//!
//! The bus is half-duplex: after each instruction packet the master reads back
//! a status packet (except for broadcast sync-write, which has no response).
//!
//! # Channels
//!
//! | Direction | Channel id         | Payload                       | Description                        |
//! |-----------|--------------------|-------------------------------|------------------------------------|
//! | Rx        | `positions`        | [`JointPositions`]     | Present positions read from servos |
//! | Tx        | `goal_positions`   | [`JointPositions`]     | Goal positions written to servos   |
//!
//! # Position values
//!
//! The unit of published / consumed positions depends on the `"units"` config
//! key:
//!
//! | Value   | Meaning                                        | Calibration required? |
//! |---------|------------------------------------------------|-----------------------|
//! | `"raw"`       | Raw 16-bit register values (0..65535). Default. | No  |
//! | `"deg"`       | Degrees relative to calibration center.         | Yes |
//! | `"rad"`       | Radians relative to calibration center.         | Yes |
//! | `"normalize"` | [-1, 1] over calibrated min..max (same scale for leader/follower). | Yes |
//!
//! When using `"deg"`, `"rad"`, or `"normalize"`, set `"calibration_file"` to the path of a
//! JSON file generated by the `feetech-calibrate` tool.  The center (zero) of
//! each servo is the midpoint of its calibrated min/max range.  Optionally
//! set `"ticks_per_rev"` (raw units per 360°); the value is model-dependent
//! (default 4096, e.g. for STS3215).
//!
//! # Torque behaviour
//!
//! - When **Tx writers are connected** (commander mode) the bridge enables
//!   torque on [`start`](CuBridge::start) so the servos track goal positions.
//! - When **no Tx writers are connected** (follower / teach mode) torque is
//!   left **disabled** so the arm can be moved freely by hand while positions
//!   are read back.
//! - On [`stop`](CuBridge::stop) torque is always disabled for safety.

pub mod calibration;
pub mod messages;

use crate::calibration::{CalibrationData, Units};
use crate::messages::{JointPositions, MAX_SERVOS};
use cu_linux_resources::LinuxSerialPort;
use cu29::cubridge::{
    BridgeChannel, BridgeChannelConfig, BridgeChannelInfo, BridgeChannelSet, CuBridge,
};
use cu29::prelude::*;
use cu29::resources;
use heapless::Vec as HeaplessVec;
use std::io::{self, Read, Write};

// ===========================================================================
// Feetech STS/SCS protocol constants
// ===========================================================================

/// Every Feetech packet starts with two 0xFF bytes.
const HEADER: [u8; 2] = [0xFF, 0xFF];

/// Maximum size of a Feetech packet payload (params or response data).
/// Sync-write with MAX_SERVOS (8) = 2 (start addr + len) + 8*3 (ID + 2 bytes data) = 26 bytes.
/// Adding header (2) + ID (1) + length (1) + instruction (1) + checksum (1) = 32 bytes total.
const MAX_PACKET_SIZE: usize = 32;

/// Maximum size of a status packet response buffer.
/// Length byte is u8 (max 255), but realistic responses are much smaller.
/// Using 64 bytes should cover all practical cases while staying on the stack.
const MAX_STATUS_PACKET_SIZE: usize = 64;

/// Broadcast address — targets all servos on the bus.
/// Used by sync-write so a single packet sets every servo's goal position.
#[allow(dead_code)]
const BROADCAST_ID: u8 = 0xFE;

/// Instruction bytes recognised by STS/SCS servos.
#[allow(dead_code)]
mod instr {
    /// Ask a servo to respond with its status (no data).
    pub const PING: u8 = 0x01;
    /// Read `N` bytes starting at a register address.
    pub const READ: u8 = 0x02;
    /// Write data starting at a register address.
    pub const WRITE: u8 = 0x03;
    /// Buffer a write; execute later with ACTION.
    pub const REG_WRITE: u8 = 0x04;
    /// Trigger all pending REG_WRITE commands.
    pub const ACTION: u8 = 0x05;
    /// Write the same register(s) to multiple servos in one packet.
    pub const SYNC_WRITE: u8 = 0x83;
    /// Factory-reset a servo.
    pub const RESET: u8 = 0x06;
}

/// STS3215 register map (addresses and widths).
///
/// Only the registers relevant to position control are included here.
/// See the Feetech STS3215 datasheet for the full map.
#[allow(dead_code)]
mod reg {
    // ---- EEPROM (persisted across power cycles) ----
    pub const MODEL_NUMBER: u8 = 3; // 2 bytes — model identifier
    pub const ID: u8 = 5; // 1 byte  — servo bus ID (1..253)
    pub const BAUD_RATE: u8 = 6; // 1 byte  — baud rate index
    pub const MIN_ANGLE_LIMIT: u8 = 9; // 2 bytes — CW angle limit
    pub const MAX_ANGLE_LIMIT: u8 = 11; // 2 bytes — CCW angle limit

    // ---- RAM (volatile, reset on power cycle) ----
    pub const TORQUE_ENABLE: u8 = 40; // 1 byte  — 0 = free, 1 = hold
    pub const GOAL_POSITION: u8 = 42; // 2 bytes — target position (0..65535)
    pub const GOAL_TIME: u8 = 44; // 2 bytes — time to reach goal (ms)
    pub const GOAL_SPEED: u8 = 46; // 2 bytes — max speed
    pub const PRESENT_POSITION: u8 = 56; // 2 bytes — current position (0..65535)
    pub const PRESENT_SPEED: u8 = 58; // 2 bytes — current speed
    pub const PRESENT_LOAD: u8 = 60; // 2 bytes — current load
    pub const PRESENT_VOLTAGE: u8 = 62; // 1 byte  — supply voltage
    pub const PRESENT_TEMPERATURE: u8 = 63; // 1 byte  — internal temperature
    pub const MOVING: u8 = 66; // 1 byte  — 1 while in motion
}

// ===========================================================================
// Checksum
// ===========================================================================

/// Compute the Feetech packet checksum.
///
/// The checksum covers everything between the header and the checksum byte
/// itself (ID + Length + Instruction/Error + Params):
///
/// ```text
/// checksum = ~(sum of bytes) & 0xFF
/// ```
#[inline]
fn compute_checksum(data: &[u8]) -> u8 {
    let mut sum: u8 = 0;
    for &b in data {
        sum = sum.wrapping_add(b);
    }
    !sum
}

// ===========================================================================
// Bridge channel declarations
// ===========================================================================

// Declare the Rx (bridge → task) channel carrying present positions.
rx_channels! {
    positions => JointPositions
}

// Declare the Tx (task → bridge) channel carrying goal positions.
tx_channels! {
    goal_positions => JointPositions
}

// ===========================================================================
// Resource binding
// ===========================================================================

// The bridge takes exclusive ownership of a serial port provided by the
// resource manager (configured in `copperconfig.ron` under `resources`).
resources!({
    serial => Owned<LinuxSerialPort>,
});

// ===========================================================================
// FeetechBridge
// ===========================================================================

/// Bidirectional bridge for Feetech STS/SCS serial bus servos.
///
/// Created by the Copper runtime from configuration.  Each cycle it:
///
/// 1. **Receives** (`receive`): reads present positions from all servos and
///    publishes them on the `positions` Rx channel.
/// 2. **Sends** (`send`): if a `goal_positions` Tx message is available,
///    writes goal positions to all servos via a single sync-write packet.
///
/// Positions are converted to the unit specified by the `"units"` config key
/// (`"raw"`, `"deg"`, `"rad"`, or `"normalize"`).  When using `"deg"`, `"rad"`, or
/// `"normalize"`, a calibration file (`"calibration_file"` key) must be provided.
#[derive(Reflect)]
#[reflect(from_reflect = false)]
pub struct FeetechBridge {
    /// Handle to the half-duplex serial port (UART).
    #[reflect(ignore)]
    port: LinuxSerialPort,

    /// Servo IDs on the bus.  Only the first `num_servos` entries are valid.
    /// Populated from `copperconfig.ron` keys `servo0` .. `servo7`.
    ids: [u8; MAX_SERVOS],

    /// How many servos are configured (1..=[`MAX_SERVOS`]).
    num_servos: u8,

    /// `true` when at least one Tx (goal_positions) writer is connected.
    /// Controls whether torque is enabled at startup:
    /// - `true`  → commander mode: torque ON, servos track goals.
    /// - `false` → follower / teach mode: torque OFF, arm moves freely.
    has_writers: bool,

    /// Cached raw positions from the last `read_all_positions` call.
    /// One entry per configured servo; remaining slots are unused.
    cached_positions: [u16; MAX_SERVOS],

    /// Output unit for published positions.
    #[reflect(ignore)]
    units: Units,

    /// Per-servo calibration center (raw ticks), indexed by servo slot.
    /// Only meaningful when `units != Raw`.
    centers: [f32; MAX_SERVOS],

    /// Ticks per revolution (raw units per 360°) for deg/rad conversion. Model-dependent.
    #[reflect(ignore)]
    ticks_per_rev: u32,

    /// Per-servo half-range (max - min) / 2 for normalize unit. Only used when `units == Normalize`.
    #[reflect(ignore)]
    half_ranges: [f32; MAX_SERVOS],
}

impl Freezable for FeetechBridge {
    // No mutable runtime state beyond what the hardware provides.
    // Default freeze / thaw (no-op) is fine.
}

// ===========================================================================
// Low-level protocol helpers
// ===========================================================================

impl FeetechBridge {
    /// Build and send an instruction packet, then flush the serial port.
    ///
    /// Packet layout:
    /// ```text
    /// [0xFF 0xFF] [id] [length] [instruction] [params…] [checksum]
    /// ```
    /// where `length = len(params) + 2` (instruction + checksum).
    fn send_packet(&mut self, id: u8, instruction: u8, params: &[u8]) -> io::Result<()> {
        let length = (params.len() + 2) as u8;
        let packet_size = 2 + 1 + 1 + 1 + params.len() + 1; // header + ID + length + instruction + params + checksum
        if packet_size > MAX_PACKET_SIZE {
            return Err(io::Error::other("Feetech: packet too large"));
        }
        let mut packet = [0u8; MAX_PACKET_SIZE];
        packet[0..2].copy_from_slice(&HEADER);
        packet[2] = id;
        packet[3] = length;
        packet[4] = instruction;
        packet[5..5 + params.len()].copy_from_slice(params);
        // Checksum covers everything after the header (ID onward).
        let checksum = compute_checksum(&packet[2..5 + params.len()]);
        packet[5 + params.len()] = checksum;
        self.port.write_all(&packet[..packet_size])?;
        self.port.flush()?;
        Ok(())
    }

    /// Read and validate a status packet returned by a servo.
    ///
    /// Status packet layout:
    /// ```text
    /// [0xFF 0xFF] [id] [length] [error] [data…] [checksum]
    /// ```
    ///
    /// Returns `(id, error_byte, data)` on success.
    fn read_status_packet(
        &mut self,
    ) -> io::Result<(u8, u8, HeaplessVec<u8, MAX_STATUS_PACKET_SIZE>)> {
        // Read the fixed-size portion: header (2) + id (1) + length (1).
        let mut header = [0u8; 4];
        self.port.read_exact(&mut header)?;

        if header[0] != 0xFF || header[1] != 0xFF {
            return Err(io::Error::other("Feetech: invalid response header"));
        }
        let id = header[2];
        let length = header[3] as usize;
        if length < 2 {
            return Err(io::Error::other("Feetech: response length too short"));
        }
        if length > MAX_STATUS_PACKET_SIZE {
            return Err(io::Error::other("Feetech: response packet too large"));
        }

        // Read the variable-size portion: error + data + checksum.
        let mut remaining = [0u8; MAX_STATUS_PACKET_SIZE];
        self.port.read_exact(&mut remaining[..length])?;

        // Verify checksum (covers id, length, and all of `remaining` except
        // the last byte which is the checksum itself).
        let received_checksum = remaining[length - 1];
        // Checksum covers: [id, length, error, data...] (everything except checksum)
        let mut checksum_sum: u8 = id;
        checksum_sum = checksum_sum.wrapping_add(length as u8);
        for &b in &remaining[..length - 1] {
            checksum_sum = checksum_sum.wrapping_add(b);
        }
        let expected_checksum = !checksum_sum;
        if received_checksum != expected_checksum {
            return Err(io::Error::other("Feetech: checksum mismatch"));
        }

        let error_byte = remaining[0];
        // Data sits between the error byte and the checksum.
        let data_len = length - 2; // length - error byte - checksum
        let mut data = HeaplessVec::new();
        data.extend_from_slice(&remaining[1..1 + data_len])
            .map_err(|_| io::Error::other("Feetech: failed to create data vector"))?;
        Ok((id, error_byte, data))
    }

    // =======================================================================
    // High-level servo operations
    // =======================================================================

    /// Ping a servo by ID.  Returns `Ok(())` if it responds without error.
    #[allow(dead_code)]
    pub fn ping(&mut self, id: u8) -> CuResult<()> {
        self.send_packet(id, instr::PING, &[])
            .map_err(|e| CuError::new_with_cause("Feetech: ping write failed", e))?;
        let (resp_id, error, _) = self
            .read_status_packet()
            .map_err(|e| CuError::new_with_cause("Feetech: ping read failed", e))?;
        if error != 0 {
            return Err(
                format!("Feetech: servo {} returned error 0x{:02X}", resp_id, error).into(),
            );
        }
        if resp_id != id {
            return Err(format!("Feetech: ping expected ID {} but got {}", id, resp_id).into());
        }
        Ok(())
    }

    /// Read `count` bytes starting at `address` from a single servo.
    fn read_register(
        &mut self,
        id: u8,
        address: u8,
        count: u8,
    ) -> io::Result<HeaplessVec<u8, MAX_STATUS_PACKET_SIZE>> {
        // READ instruction params: [start_address, byte_count].
        self.send_packet(id, instr::READ, &[address, count])?;
        let (_id, _error, data) = self.read_status_packet()?;
        Ok(data)
    }

    /// Write `data` starting at `address` to a single servo.
    #[allow(dead_code)]
    fn write_register(&mut self, id: u8, address: u8, data: &[u8]) -> io::Result<()> {
        // WRITE instruction params: [start_address, data…].
        // Max params size: address (1) + data (typically small, max ~20 bytes for multi-register writes)
        if 1 + data.len() > MAX_PACKET_SIZE - 5 {
            return Err(io::Error::other("Feetech: write data too large"));
        }
        let mut params = [0u8; MAX_PACKET_SIZE - 5];
        params[0] = address;
        params[1..1 + data.len()].copy_from_slice(data);
        self.send_packet(id, instr::WRITE, &params[..1 + data.len()])?;
        // Every non-broadcast write returns a status acknowledgment.
        let _ = self.read_status_packet()?;
        Ok(())
    }

    /// Read the raw present position (2 bytes, little-endian) from one servo.
    ///
    /// Returns a value in 0..65535 (16-bit register).
    fn read_present_position(&mut self, id: u8) -> CuResult<u16> {
        let data = self
            .read_register(id, reg::PRESENT_POSITION, 2)
            .map_err(|e| {
                CuError::new_with_cause(
                    &format!("Feetech: failed to read position from servo {}", id),
                    e,
                )
            })?;
        if data.len() < 2 {
            return Err(format!(
                "Feetech: short read for position from servo {} (got {} bytes)",
                id,
                data.len()
            )
            .into());
        }
        Ok(u16::from_le_bytes([data[0], data[1]]))
    }

    /// Poll present positions from every configured servo into `cached_positions`.
    ///
    /// On a read failure for any individual servo the previously cached value
    /// is kept and a debug message is logged — the bus continues with the
    /// remaining servos.
    fn read_all_positions(&mut self) -> CuResult<()> {
        for i in 0..self.num_servos as usize {
            match self.read_present_position(self.ids[i]) {
                Ok(raw) => self.cached_positions[i] = raw,
                Err(e) => {
                    debug!(
                        "Feetech: failed to read servo {} (ID {}): {}",
                        i, self.ids[i], e
                    );
                }
            }
        }
        Ok(())
    }

    /// Write goal positions to all configured servos using **sync-write**.
    ///
    /// Sync-write (instruction 0x83) packs every servo's data into a single
    /// broadcast packet, which is much faster than writing to each servo
    /// individually and doesn't produce a status response.
    ///
    /// Packet params layout:
    /// ```text
    /// [start_address] [bytes_per_servo] [ID_0] [lo_0] [hi_0] [ID_1] …
    /// ```
    fn sync_write_positions(&mut self, positions: &JointPositions) -> CuResult<()> {
        let vals = positions.as_slice();
        // Write at most as many servos as we have configured, even if the
        // payload carries fewer (or more) entries.
        let n = (self.num_servos as usize).min(vals.len());
        if n == 0 {
            return Ok(());
        }

        let data_len_per_servo: u8 = 2; // 2 bytes for GOAL_POSITION
        // Max params: 2 (start addr + len) + MAX_SERVOS*3 (ID + 2 bytes data) = 26 bytes
        let params_size = 2 + n * 3;
        if params_size > MAX_PACKET_SIZE - 5 {
            return Err(CuError::from("Feetech: sync-write params too large"));
        }
        let mut params = [0u8; MAX_PACKET_SIZE - 5];
        params[0] = reg::GOAL_POSITION; // start address
        params[1] = data_len_per_servo;
        let mut offset = 2;
        for (i, val) in vals.iter().enumerate().take(n) {
            let param = self.param_for_slot(i);
            let raw = self.units.to_raw(*val, self.centers[i], param);
            params[offset] = self.ids[i]; // servo ID
            params[offset + 1] = (raw & 0xFF) as u8; // position low byte
            params[offset + 2] = (raw >> 8) as u8; // position high byte
            offset += 3;
        }

        self.send_packet(BROADCAST_ID, instr::SYNC_WRITE, &params[..params_size])
            .map_err(|e| CuError::new_with_cause("Feetech: sync-write failed", e))?;
        Ok(())
    }

    /// Enable or disable torque on a single servo.
    ///
    /// When torque is **enabled** the servo actively holds its position.
    /// When **disabled** the servo can be moved freely by hand.
    #[allow(dead_code)]
    pub fn set_torque(&mut self, id: u8, enable: bool) -> io::Result<()> {
        self.write_register(id, reg::TORQUE_ENABLE, &[enable as u8])
    }

    /// Parameter for from_raw/to_raw: ticks_per_rev for Deg/Rad, half_ranges[i] for Normalize.
    #[inline]
    fn param_for_slot(&self, i: usize) -> f32 {
        if self.units == Units::Normalize {
            let hr = self.half_ranges[i];
            if hr > 0.0 { hr } else { 1.0 } // avoid div-by-zero
        } else {
            self.ticks_per_rev as f32
        }
    }

    /// Enable torque on every configured servo.
    fn enable_all_torque(&mut self) -> CuResult<()> {
        for i in 0..self.num_servos as usize {
            self.set_torque(self.ids[i], true).map_err(|e| {
                CuError::new_with_cause(
                    &format!("Feetech: failed to enable torque on servo {}", self.ids[i]),
                    e,
                )
            })?;
        }
        Ok(())
    }
}

// ===========================================================================
// CuBridge trait implementation
// ===========================================================================

impl CuBridge for FeetechBridge {
    type Tx = TxChannels;
    type Rx = RxChannels;
    type Resources<'r> = Resources;

    /// Construct the bridge from configuration.
    ///
    /// Expected `copperconfig.ron` keys under the bridge's `config` block:
    ///
    /// | Key                | Type   | Description                                   |
    /// |--------------------|--------|-----------------------------------------------|
    /// | `servo0`           | u8     | Bus ID of the first servo                     |
    /// | `servo1`           | u8     | Bus ID of the second servo                    |
    /// | …                  | …      | Up to `servo7`                                |
    /// | `units`            | string | `"raw"` (default), `"deg"`, `"rad"`, or `"normalize"` |
    /// | `calibration_file` | string | Path to calibration JSON (required for deg/rad/normalize) |
    /// | `ticks_per_rev`    | integer | Raw units per 360° (model-dependent; default 4096) |
    ///
    /// At least `servo0` must be present.
    fn new(
        config: Option<&ComponentConfig>,
        tx_channels: &[BridgeChannelConfig<<Self::Tx as BridgeChannelSet>::Id>],
        _rx_channels: &[BridgeChannelConfig<<Self::Rx as BridgeChannelSet>::Id>],
        resources: Self::Resources<'_>,
    ) -> CuResult<Self>
    where
        Self: Sized,
    {
        let cfg = config.ok_or("FeetechBridge requires a config block with servo IDs")?;

        // Collect servo IDs from sequential keys: "servo0", "servo1", …
        // Stop at the first missing key (after servo0, which is mandatory).
        let mut ids = [0u8; MAX_SERVOS];
        let mut num_servos: u8 = 0;
        for (i, id_slot) in ids.iter_mut().enumerate().take(MAX_SERVOS) {
            let key = format!("servo{}", i);
            match cfg.get::<u8>(&key)? {
                Some(id) => {
                    *id_slot = id;
                    num_servos = (i + 1) as u8;
                }
                None if i == 0 => {
                    return Err(
                        "FeetechBridge: you must configure at least one servo ID (\"servo0\")"
                            .into(),
                    );
                }
                None => break, // no more servos configured
            }
        }

        // ---- Parse output units ----
        let units = match cfg.get::<String>("units")? {
            Some(s) => s.parse().map_err(|_| {
                CuError::from(format!(
                    "FeetechBridge: unknown units \"{s}\". Use \"raw\", \"deg\", \"rad\", or \"normalize\"."
                ))
            })?,
            None => Units::Raw,
        };

        // ---- Load calibration (required for deg / rad / normalize) ----
        let mut centers = [0.0f32; MAX_SERVOS];
        let mut half_ranges = [0.0f32; MAX_SERVOS];
        if units != Units::Raw {
            let cal_path = cfg
                .get::<String>("calibration_file")?
                .ok_or("FeetechBridge: \"calibration_file\" is required when units != raw")?;
            let cal = CalibrationData::load(std::path::Path::new(&cal_path)).map_err(|e| {
                CuError::new_with_cause(
                    &format!("FeetechBridge: failed to load calibration from \"{cal_path}\""),
                    e,
                )
            })?;
            for i in 0..num_servos as usize {
                centers[i] = cal.center_for(ids[i]).ok_or_else(|| {
                    CuError::from(format!(
                        "FeetechBridge: no calibration entry for servo ID {} in \"{cal_path}\"",
                        ids[i]
                    ))
                })?;
                if units == Units::Normalize {
                    half_ranges[i] = cal.half_range_for(ids[i]).ok_or_else(|| {
                        CuError::from(format!(
                            "FeetechBridge: no calibration entry for servo ID {} in \"{cal_path}\" (normalize)",
                            ids[i]
                        ))
                    })?;
                }
            }
        }

        // ---- Ticks per revolution (model-dependent; used for deg/rad) ----
        let ticks_per_rev = cfg.get::<u32>("ticks_per_rev")?.unwrap_or(4096);

        let port = resources.serial.0;

        // If no Tx channels are wired up in this mission, nobody will send
        // goal positions → the arm is in read-only (follower / teach) mode.
        let has_writers = !tx_channels.is_empty();

        Ok(FeetechBridge {
            port,
            ids,
            num_servos,
            has_writers,
            cached_positions: [0u16; MAX_SERVOS],
            units,
            centers,
            ticks_per_rev,
            half_ranges,
        })
    }

    /// Called once before the first processing cycle.
    ///
    /// Enables torque only when writers are connected (commander mode).
    /// In follower mode torque stays off so the arm moves freely.
    fn start(&mut self, _ctx: &CuContext) -> CuResult<()> {
        if self.has_writers {
            self.enable_all_torque()?;
            debug!(
                "FeetechBridge: enabled torque on {} servos",
                self.num_servos
            );
        } else {
            debug!(
                "FeetechBridge: read-only mode, torque left disabled on {} servos",
                self.num_servos
            );
        }
        Ok(())
    }

    /// Handle an outgoing message on a Tx channel.
    ///
    /// For `goal_positions`: sync-writes the raw positions to the servo bus.
    fn send<'a, Payload>(
        &mut self,
        _ctx: &CuContext,
        channel: &'static BridgeChannel<<Self::Tx as BridgeChannelSet>::Id, Payload>,
        msg: &CuMsg<Payload>,
    ) -> CuResult<()>
    where
        Payload: CuMsgPayload + 'a,
    {
        match channel.id() {
            TxId::GoalPositions => {
                let goal_msg: &CuMsg<JointPositions> = msg.downcast_ref()?;
                if let Some(positions) = goal_msg.payload() {
                    self.sync_write_positions(positions)?;
                }
            }
        }
        Ok(())
    }

    /// Produce an incoming message on an Rx channel.
    ///
    /// For `positions`: reads every servo's present position and publishes
    /// them as a [`JointPositions`].
    fn receive<'a, Payload>(
        &mut self,
        ctx: &CuContext,
        channel: &'static BridgeChannel<<Self::Rx as BridgeChannelSet>::Id, Payload>,
        msg: &mut CuMsg<Payload>,
    ) -> CuResult<()>
    where
        Payload: CuMsgPayload + 'a,
    {
        // Poll all servos and update the cache.
        self.read_all_positions()?;

        // Stamp the message with the current robot time.
        msg.tov = Tov::Time(ctx.now());

        match channel.id() {
            RxId::Positions => {
                // Build the payload, converting each raw position to the
                // configured output unit (raw / deg / rad).
                let mut payload = JointPositions::new();
                payload.fill_from_iter(
                    self.cached_positions[..self.num_servos as usize]
                        .iter()
                        .enumerate()
                        .map(|(i, &raw)| {
                            self.units
                                .from_raw(raw, self.centers[i], self.param_for_slot(i))
                        }),
                );
                let pos_msg: &mut CuMsg<JointPositions> = msg.downcast_mut()?;
                pos_msg.set_payload(payload);
            }
        }
        Ok(())
    }

    /// Called once after the last processing cycle.
    ///
    /// Disables torque on every servo for safety (prevents the arm from
    /// holding position with power applied after the application exits).
    fn stop(&mut self, _ctx: &CuContext) -> CuResult<()> {
        for i in 0..self.num_servos as usize {
            if let Err(e) = self.set_torque(self.ids[i], false) {
                debug!(
                    "FeetechBridge: failed to disable torque on servo {}: {}",
                    self.ids[i],
                    e.to_string()
                );
            }
        }
        debug!(
            "FeetechBridge: disabled torque on {} servos",
            self.num_servos
        );
        Ok(())
    }
}

// ===========================================================================
// Tests
// ===========================================================================

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

    #[test]
    fn checksum_matches_known_values() {
        // Hand-calculated example:
        //   ID=1, Length=4, Instruction=WRITE(3), Addr=40, Data=1
        //   body = [0x01, 0x04, 0x03, 0x28, 0x01]
        //   sum  = 1+4+3+40+1 = 49 = 0x31
        //   ~0x31 = 0xCE
        let body = [0x01u8, 0x04, 0x03, 0x28, 0x01];
        assert_eq!(compute_checksum(&body), 0xCE);
    }

    #[test]
    fn joint_positions_from_slice() {
        let mut p = JointPositions::new();
        p.fill_from_iter([0.0f32, 32768.0, 65535.0]);
        assert_eq!(p.as_slice(), &[0.0, 32768.0, 65535.0]);
    }

    #[test]
    fn units_raw_roundtrip() {
        use crate::calibration::Units;
        let u = Units::Raw;
        let tpr = 4096.0;
        assert_eq!(u.from_raw(2048, 0.0, tpr), 2048.0);
        assert_eq!(u.to_raw(2048.0, 0.0, tpr), 2048);
    }

    #[test]
    fn units_deg_roundtrip() {
        use crate::calibration::{DEFAULT_TICKS_PER_REV, Units};
        let u = Units::Deg;
        let center = 2048.0;
        // Center should map to 0°.
        assert!((u.from_raw(2048, center, DEFAULT_TICKS_PER_REV as f32)).abs() < 1e-6);
        // Converting 0° back should give the center tick.
        assert_eq!(u.to_raw(0.0, center, DEFAULT_TICKS_PER_REV as f32), 2048);
    }

    #[test]
    fn units_rad_roundtrip() {
        use crate::calibration::{DEFAULT_TICKS_PER_REV, Units};
        let u = Units::Rad;
        let center = 2048.0;
        let rad = u.from_raw(3072, center, DEFAULT_TICKS_PER_REV as f32);
        let back = u.to_raw(rad, center, DEFAULT_TICKS_PER_REV as f32);
        assert_eq!(back, 3072);
    }

    #[test]
    fn units_normalize_roundtrip() {
        use crate::calibration::Units;
        let u = Units::Normalize;
        let center = 2048.0;
        let half_range = 1024.0; // min=1024, max=3072
        assert!((u.from_raw(2048, center, half_range)).abs() < 1e-6);
        assert!((u.from_raw(1024, center, half_range) + 1.0).abs() < 1e-6);
        assert!((u.from_raw(3072, center, half_range) - 1.0).abs() < 1e-6);
        assert_eq!(u.to_raw(0.0, center, half_range), 2048);
        assert_eq!(u.to_raw(-1.0, center, half_range), 1024);
        assert_eq!(u.to_raw(1.0, center, half_range), 3072);
    }
}