arora-hal 3.0.0

//! The Arora Hardware Abstraction Layer.
//!
//! [`Hal`] is the boundary to a real (or fake) device — the thing studio-bridge
//! called a `Controller`. A HAL reads sensors and reported state, accepts
//! actuator/state writes, and pushes a feed of changes the hardware makes.
//!
//! The Arora runtime mirrors a HAL against a
//! [`DataStore`](arora_types::data::DataStore): HAL updates flow into the store,
//! store writes flow to the HAL. The HAL trait depends only on `arora-types`, so
//! any execution engine can drive it without pulling in the bridge.
//!
//! # The I/O seam
//!
//! Consistent with the bridge, the HAL's data plane is an owned inbound stream
//! and a non-blocking outbound push: [`updates`](Hal::updates) hands out the
//! sensor feed as a [`Stream`] (each call an independent subscription, owned by
//! its consumer — the runtime's loop is its one poller), and
//! [`try_send`](Hal::try_send) pushes actuator writes immediately, called
//! directly from the synchronous step. Any real async work is the
//! implementation's own responsibility (its own task/queue), the same way a
//! bridge owns its socket; this crate depends on no async runtime.
//!
//! The device owns its HAL (`Box<dyn Hal>` at the builder). An implementation
//! that also serves an observer (a simulator UI, a test double) shares its
//! internals and hands sibling handles out itself — [`FakeHal`] clones onto
//! the same state.
//!
//! Pick an implementation per robot: [`FakeHal`] here (also the test double),
//! and the real ones (ros2, restful, nao) in their own sibling crates.

use std::pin::Pin;
use std::sync::{Arc, Mutex};

use async_trait::async_trait;
use futures_channel::mpsc::UnboundedSender;
use futures_core::Stream;

use arora_types::data::{Key, State, StateChange};
use arora_types::value::Value;

/// What device a HAL drives.
#[derive(Debug, Clone, Default, PartialEq, Eq)]
pub struct HalDescription {
    pub model_family: Option<String>,
    pub hardware_version: Option<String>,
    pub software_version: Option<String>,
}

/// Something went wrong talking to the hardware.
#[derive(Debug, Clone, PartialEq, Eq)]
pub enum HalError {
    /// The hardware link is broken / unavailable.
    Broken(String),
    /// A key could not be resolved.
    NoSuchKey(String),
    /// Anything else, with a message.
    Other(String),
}

impl std::fmt::Display for HalError {
    fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
        match self {
            HalError::Broken(m) => write!(f, "hardware link broken: {m}"),
            HalError::NoSuchKey(k) => write!(f, "no such key: {k}"),
            HalError::Other(m) => write!(f, "{m}"),
        }
    }
}

impl std::error::Error for HalError {}

pub type HalResult<T> = Result<T, HalError>;

/// The Hardware Abstraction Layer: the boundary to a device.
///
/// Interior-mutable (`&self`): the methods take shared references, so an
/// implementation that hands sibling handles to an observer keeps working —
/// the runtime still owns its HAL by value.
#[async_trait]
pub trait Hal: Send + Sync {
    /// Describe the device (model family, versions).
    async fn describe(&self) -> HalDescription;

    /// Read the current values for the given keys. Each entry is `None` if the
    /// key is unset/absent (further nesting lives inside [`Value`]).
    async fn read(&self, keys: &[Key]) -> HalResult<Vec<Option<Value>>>;

    /// Read everything the HAL currently exposes.
    async fn read_all(&self) -> HalResult<State>;

    /// Apply actuator/state changes. Observers of [`updates`](Hal::updates) see
    /// the resulting changes.
    async fn write(&self, changes: StateChange) -> HalResult<()>;

    /// Push actuator/state changes toward the hardware immediately, without
    /// blocking — the outbound counterpart to the [`updates`](Hal::updates)
    /// sensor feed, and the shape the synchronous runtime step calls directly
    /// (mirroring `Bridge::try_send`).
    ///
    /// **Must not block.** A HAL whose apply is truly immediate (an in-memory
    /// fake, a cache write) applies here directly; a HAL that performs real
    /// async I/O (HTTP, DDS) enqueues onto its own internal task and returns.
    /// There is deliberately no default: every implementation decides how its
    /// hardware absorbs a non-blocking push.
    fn try_send(&self, changes: &StateChange);

    /// A feed of changes the hardware reports (sensors, mirrored actuation, …).
    /// Each call yields an independent, owned stream; the consumer that takes
    /// it is its one poller (natively the runtime's `run` select, on the web
    /// the per-frame sweep).
    fn updates(&self) -> UpdatesStream;
}

/// The sensor feed of a [`Hal`]: an owned stream of the changes the hardware
/// reports. The stream ending means the hardware feed is gone.
pub type UpdatesStream = Pin<Box<dyn Stream<Item = StateChange> + Send>>;

/// Optional extension: HALs that can supply a 3D model (GLB) of the device.
#[async_trait]
pub trait HalAssets: Send + Sync {
    async fn model_glb(&self) -> HalResult<Option<Vec<u8>>>;
}

#[derive(Default)]
struct FakeInner {
    description: HalDescription,
    model_glb: Option<Vec<u8>>,
    state: State,
    subscribers: Vec<UnboundedSender<StateChange>>,
}

impl FakeInner {
    fn notify(&mut self, change: &StateChange) {
        if change.is_empty() {
            return;
        }
        self.subscribers
            .retain(|tx| tx.unbounded_send(change.clone()).is_ok());
    }
}

/// An in-memory fake [`Hal`] for tests and simulators.
///
/// Echoes writes back as state, and fakes joint actuation by mirroring any
/// `*.target_position` write to the corresponding `*.position` (so a consumer
/// that writes a target sees the measured position follow). Cheaply cloneable;
/// clones share the same state. (Backed by [`State`](arora_types::data::State),
/// the trivial owned state type.)
#[derive(Clone, Default)]
pub struct FakeHal {
    inner: Arc<Mutex<FakeInner>>,
}

impl FakeHal {
    pub fn new() -> Self {
        Self::default()
    }

    pub fn with_description(description: HalDescription) -> Self {
        Self {
            inner: Arc::new(Mutex::new(FakeInner {
                description,
                ..Default::default()
            })),
        }
    }

    pub fn set_model_glb(&self, glb: Vec<u8>) {
        self.inner.lock().unwrap().model_glb = Some(glb);
    }

    /// Apply a write synchronously. The fake deals only in setpoints — the
    /// `*.target_position` keys — and ignores every other key in the change,
    /// the way hardware ignores what it has no actuator for. Each setpoint is
    /// held and sensed back as the matching `*.position`, the fake's joint
    /// actuation. Shared by [`Hal::write`] and [`Hal::try_send`].
    fn apply_write(&self, changes: &StateChange) {
        let mut setpoints = StateChange::new();
        let mut sensed = StateChange::new();
        for (key, value) in &changes.set {
            if key.get_component() == Some("target_position") {
                setpoints.set.insert(key.clone(), value.clone());
                sensed
                    .set
                    .insert(key.clone().with_component("position"), value.clone());
            }
        }
        for key in &changes.unset {
            if key.get_component() == Some("target_position") {
                setpoints.unset.insert(key.clone());
                sensed.unset.insert(key.clone().with_component("position"));
            }
        }
        if setpoints.is_empty() {
            return;
        }
        let mut inner = self.inner.lock().unwrap();
        inner.state.apply(setpoints);
        inner.state.apply(sensed.clone());
        inner.notify(&sensed);
    }
}

#[async_trait]
impl Hal for FakeHal {
    async fn describe(&self) -> HalDescription {
        self.inner.lock().unwrap().description.clone()
    }

    async fn read(&self, keys: &[Key]) -> HalResult<Vec<Option<Value>>> {
        let inner = self.inner.lock().unwrap();
        Ok(keys
            .iter()
            .map(|k| inner.state.get(k).cloned().flatten())
            .collect())
    }

    async fn read_all(&self) -> HalResult<State> {
        Ok(self.inner.lock().unwrap().state.clone())
    }

    async fn write(&self, changes: StateChange) -> HalResult<()> {
        self.apply_write(&changes);
        Ok(())
    }

    /// Synchronous, immediate apply — the fake never blocks, so it needs no task
    /// of its own (unlike a real HTTP/DDS HAL).
    fn try_send(&self, changes: &StateChange) {
        self.apply_write(changes);
    }

    fn updates(&self) -> UpdatesStream {
        let (tx, rx) = futures_channel::mpsc::unbounded();
        self.inner.lock().unwrap().subscribers.push(tx);
        Box::pin(rx)
    }
}

#[async_trait]
impl HalAssets for FakeHal {
    async fn model_glb(&self) -> HalResult<Option<Vec<u8>>> {
        Ok(self.inner.lock().unwrap().model_glb.clone())
    }
}

#[cfg(test)]
mod tests {
    use super::*;
    use futures::{FutureExt, StreamExt};

    /// Drain everything the feed has already buffered, without blocking.
    fn drain(feed: &mut UpdatesStream) -> Vec<StateChange> {
        let mut out = Vec::new();
        while let Some(Some(change)) = feed.next().now_or_never() {
            out.push(change);
        }
        out
    }

    #[tokio::test]
    async fn fake_mirrors_target_position() {
        let hal = FakeHal::new();
        let mut sub = hal.updates();
        hal.write(StateChange::set("joint1.target_position", Value::from(1.0)))
            .await
            .unwrap();
        // measured position mirrors the target
        assert_eq!(
            hal.read(&[Key::from("joint1.position")]).await.unwrap(),
            vec![Some(Value::from(1.0))]
        );
        // a subscriber saw the mirrored position change
        let saw_position = drain(&mut sub)
            .iter()
            .any(|c| c.contains(&Key::from("joint1.position")));
        assert!(saw_position);
    }

    #[test]
    fn try_send_applies_synchronously_and_mirrors() {
        // The synchronous seam the runtime's step calls: no async, immediate.
        let hal = FakeHal::new();
        let mut sub = hal.updates();
        hal.try_send(&StateChange::set(
            "joint1.target_position",
            Value::from(1.0),
        ));
        let saw_position = drain(&mut sub)
            .iter()
            .any(|c| c.contains(&Key::from("joint1.position")));
        assert!(
            saw_position,
            "try_send should mirror target to measured position"
        );
    }

    #[tokio::test]
    async fn read_absent_is_none() {
        let hal = FakeHal::new();
        assert_eq!(hal.read(&[Key::from("nope")]).await.unwrap(), vec![None]);
    }

    #[tokio::test]
    async fn describe_and_glb() {
        let hal = FakeHal::with_description(HalDescription {
            model_family: Some("test".into()),
            ..Default::default()
        });
        assert_eq!(hal.describe().await.model_family.as_deref(), Some("test"));
        hal.set_model_glb(vec![1, 2, 3]);
        assert_eq!(hal.model_glb().await.unwrap(), Some(vec![1, 2, 3]));
    }
}