motorcortex_rust/client/

subscription.rs

use crate::GetParameterValue;
use crate::msg::{DataType, GroupStatusMsg};
use crate::parameter_value::GetParameterTuple;
use crate::parameter_value::decode_parameter_value;
use chrono::{DateTime, Local, Utc};
use std::sync::{Arc, RwLock, RwLockReadGuard};

#[derive(Debug, Clone, Copy)]
pub struct TimeSpec {
    pub sec: i64,  // Seconds
    pub nsec: i64, // Nanoseconds
}

impl TimeSpec {
    pub fn from_buffer(buffer: &[u8]) -> Option<Self> {
        // Ensure the buffer has enough size for the TimeSpec struct
        if buffer.len() < std::mem::size_of::<TimeSpec>() {
            return None;
        }

        // Safely cast the buffer into a TimeSpec struct
        let ts = unsafe { std::ptr::read(buffer.as_ptr() as *const TimeSpec) };

        Some(ts)
    }
    pub fn to_date_time(&self) -> DateTime<Local> {
        // Create a NaiveDateTime from seconds and nanoseconds since the UNIX epoch
        self.to_utc_date_time().with_timezone(&Local)
    }
    pub fn to_utc_date_time(&self) -> DateTime<Utc> {
        // Create a NaiveDateTime from seconds and nanoseconds since the UNIX epoch
        DateTime::from_timestamp(self.sec, self.nsec as u32).expect("Invalid TimeSpec")
    }
}

pub struct Subscription {
    data_types: Vec<u32>,
    description: GroupStatusMsg,
    buffer: Vec<u8>,
    callback: Arc<RwLock<Option<Box<dyn Fn(&Subscription) + Send + Sync + 'static>>>>,
}

impl Subscription {
    pub fn new(group_msg: GroupStatusMsg) -> Self {
        Self {
            data_types: group_msg
                .params
                .iter()
                .map(|param| DataType::try_from(param.info.data_type as i32).unwrap() as u32)
                .collect(),
            description: group_msg,
            buffer: Vec::new(),
            callback: Arc::new(RwLock::new(None)),
        }
    }

    pub fn id(&self) -> u32 {
        self.description.id
    }
    pub fn update(&mut self, buffer: Vec<u8>) {
        self.buffer = buffer;
        if let Some(cb) = &*self.callback.read().unwrap() {
            cb(self); // Call the callback
        }
    }

    pub fn read<V>(&self) -> Option<(TimeSpec, V)>
    where
        V: GetParameterTuple,
    {
        if !self.buffer.is_empty() {
            const HEADER_LEN: usize = 4usize;
            let protocol_version = self.buffer[3];
            if protocol_version == 0 {
                return self.read_protocol0(&self.buffer[HEADER_LEN..]);
            }
            if protocol_version == 1 {
                return self.read_protocol1(&self.buffer[HEADER_LEN..]);
            }
        }
        None
    }

    /// Reads all subscribed parameters and returns their values as a flat `Vec<V>`.
    ///
    /// Each parameter's elements are decoded individually using `decode_parameter_value`,
    /// regardless of the number of parameters or their array sizes.
    /// The caller chooses the output type:
    ///
    /// ```ignore
    /// sub.read_all::<f64>()     // all values as f64
    /// sub.read_all::<String>()  // all values as their string representation
    /// sub.read_all::<i64>()     // all values as i64
    /// ```
    pub fn read_all<V>(&self) -> Option<(TimeSpec, Vec<V>)>
    where
        V: GetParameterValue + Default,
    {
        if self.buffer.is_empty() {
            return None;
        }
        const HEADER_LEN: usize = 4usize;
        let protocol_version = self.buffer[3];
        if protocol_version != 1 {
            return None;
        }

        let buffer = &self.buffer[HEADER_LEN..];
        let ts = TimeSpec::from_buffer(buffer)?;
        const TS_SIZE: usize = size_of::<TimeSpec>();
        let body = &buffer[TS_SIZE..];

        let mut values = Vec::new();
        let mut cursor = 0usize;

        for (param, &data_type) in self.description.params.iter().zip(self.data_types.iter()) {
            let size = param.size as usize;
            let data_size = param.info.data_size as usize;
            let num_elements = param.info.number_of_elements as usize;
            let param_bytes = &body[cursor..cursor + size];

            for i in 0..num_elements {
                let start = i * data_size;
                let end = start + data_size;
                let element_bytes = &param_bytes[start..end];
                values.push(decode_parameter_value::<V>(data_type, element_bytes));
            }

            cursor += size;
        }

        Some((ts, values))
    }

    pub fn notify<F>(&mut self, cb: F)
    where
        F: Fn(&Subscription) + Send + 'static + Sync,
    {
        let mut callback_lock = self.callback.write().unwrap();
        *callback_lock = Some(Box::new(cb));
    }

    pub fn name(&self) -> String {
        self.description.alias.to_string()
    }

    fn read_protocol0<V>(&self, _buffer: &[u8]) -> Option<(TimeSpec, V)>
    where
        V: GetParameterTuple,
    {
        None
    }

    fn combined_iterator<'a>(
        &'a self,
        body: &'a [u8],
    ) -> impl Iterator<Item = (&'a u32, &'a [u8])> + 'a {
        self.description
            .params
            .iter()
            .zip(self.data_types.iter())
            .scan(0, move |cursor, (param, data_type)| {
                let size = param.size as usize;
                let slice = &body[*cursor..*cursor + size];
                *cursor += size; // Advance the cursor
                Some((data_type, slice)) // Yield the pair (data_type, slice)
            })
    }

    fn read_protocol1<V>(&self, buffer: &[u8]) -> Option<(TimeSpec, V)>
    where
        V: GetParameterTuple,
    {
        let ts = TimeSpec::from_buffer(buffer).unwrap();
        const TS_SIZE: usize = size_of::<TimeSpec>();
        let body = &buffer[TS_SIZE..];

        Some((ts, V::get_parameters(self.combined_iterator(body)).unwrap()))
    }
}

pub struct ReadOnlySubscription {
    inner: Arc<RwLock<Subscription>>, // Internal shared ownership
}

impl ReadOnlySubscription {
    /// Creates a new readonly wrapper around the `Arc<RwLock<Subscription>>`.
    pub fn new(subscription: Arc<RwLock<Subscription>>) -> Self {
        Self {
            inner: subscription,
        }
    }

    /// Provides thread-safe, readonly access to the subscription.
    pub fn get(&self) -> RwLockReadGuard<'_, Subscription> {
        self.inner.read().unwrap()
    }

    /// Optionally, expose specific readonly fields for convenience
    pub fn read<V>(&self) -> Option<V>
    where
        V: GetParameterTuple,
    {
        self.inner.read().unwrap().read().map(|(_, v)| v)
    }

    /// Reads all subscribed parameters as a flat `Vec<V>`.
    /// See [`Subscription::read_all`] for details.
    pub fn read_all<V>(&self) -> Option<(TimeSpec, Vec<V>)>
    where
        V: GetParameterValue + Default,
    {
        self.inner.read().unwrap().read_all()
    }

    pub fn notify<F>(&self, cb: F)
    where
        F: Fn(&Subscription) + Send + 'static + Sync,
    {
        self.inner.write().unwrap().notify(cb);
    }

    pub fn read_with_timestamp<V>(&self) -> Option<(TimeSpec, V)>
    where
        V: GetParameterTuple,
    {
        self.inner.read().unwrap().read()
    }

    pub fn name(&self) -> String {
        self.inner.read().unwrap().name()
    }

    pub fn id(&self) -> u32 {
        self.inner.read().unwrap().id()
    }
}