stepper_motion/trajectory/
registry.rs

1//! Trajectory registry for named trajectory lookup.
2
3use heapless::{FnvIndexMap, String};
4
5use crate::config::TrajectoryConfig;
6use crate::error::{Error, Result, TrajectoryError};
7
8/// Maximum number of trajectories in the registry.
9pub const MAX_TRAJECTORIES: usize = 32;
10
11/// Registry for named trajectories.
12#[derive(Debug)]
13pub struct TrajectoryRegistry {
14    trajectories: FnvIndexMap<String<32>, TrajectoryConfig, MAX_TRAJECTORIES>,
15}
16
17impl Default for TrajectoryRegistry {
18    fn default() -> Self {
19        Self::new()
20    }
21}
22
23impl TrajectoryRegistry {
24    /// Create a new empty registry.
25    pub fn new() -> Self {
26        Self {
27            trajectories: FnvIndexMap::new(),
28        }
29    }
30
31    /// Register a trajectory with a name.
32    ///
33    /// # Errors
34    ///
35    /// Returns an error if the registry is full.
36    pub fn register(&mut self, name: &str, trajectory: TrajectoryConfig) -> Result<()> {
37        let name_str = String::try_from(name).map_err(|_| {
38            Error::Trajectory(TrajectoryError::InvalidName(
39                String::try_from("name too long").unwrap(),
40            ))
41        })?;
42
43        self.trajectories
44            .insert(name_str, trajectory)
45            .map_err(|_| {
46                Error::Trajectory(TrajectoryError::InvalidName(
47                    String::try_from("registry full").unwrap(),
48                ))
49            })?;
50
51        Ok(())
52    }
53
54    /// Get a trajectory by name.
55    pub fn get(&self, name: &str) -> Option<&TrajectoryConfig> {
56        let name_str = String::try_from(name).ok()?;
57        self.trajectories.get(&name_str)
58    }
59
60    /// Get a trajectory by name, returning an error with available names if not found.
61    ///
62    /// # Errors
63    ///
64    /// Returns `TrajectoryError::NotFoundWithNames` if the trajectory doesn't exist,
65    /// including a list of available trajectory names for debugging.
66    pub fn get_or_error(&self, name: &str) -> Result<&TrajectoryConfig> {
67        self.get(name).ok_or_else(|| {
68            // Build list of available names for the error message
69            let mut available: heapless::String<256> = heapless::String::new();
70            let mut first = true;
71            for traj_name in self.names() {
72                if !first {
73                    let _ = available.push_str(", ");
74                }
75                let _ = available.push_str(traj_name);
76                first = false;
77            }
78            
79            let mut msg: heapless::String<64> = heapless::String::new();
80            let _ = msg.push_str("'");
81            let _ = msg.push_str(name);
82            let _ = msg.push_str("' not found. Available: ");
83            let _ = msg.push_str(&available);
84            
85            Error::Trajectory(TrajectoryError::InvalidName(msg))
86        })
87    }
88
89    /// Check if a trajectory exists.
90    pub fn contains(&self, name: &str) -> bool {
91        if let Ok(name_str) = String::try_from(name) {
92            self.trajectories.contains_key(&name_str)
93        } else {
94            false
95        }
96    }
97
98    /// Remove a trajectory by name.
99    pub fn remove(&mut self, name: &str) -> Option<TrajectoryConfig> {
100        let name_str = String::try_from(name).ok()?;
101        self.trajectories.remove(&name_str)
102    }
103
104    /// Get the number of registered trajectories.
105    pub fn len(&self) -> usize {
106        self.trajectories.len()
107    }
108
109    /// Check if the registry is empty.
110    pub fn is_empty(&self) -> bool {
111        self.trajectories.is_empty()
112    }
113
114    /// Get an iterator over trajectory names.
115    pub fn names(&self) -> impl Iterator<Item = &str> {
116        self.trajectories.keys().map(|s| s.as_str())
117    }
118
119    /// Get an iterator over trajectories.
120    pub fn iter(&self) -> impl Iterator<Item = (&str, &TrajectoryConfig)> {
121        self.trajectories
122            .iter()
123            .map(|(k, v)| (k.as_str(), v))
124    }
125
126    /// Clear all trajectories.
127    pub fn clear(&mut self) {
128        self.trajectories.clear();
129    }
130
131    /// Load trajectories from a SystemConfig.
132    pub fn from_config(config: &crate::config::SystemConfig) -> Self {
133        let mut registry = Self::new();
134        for (name, trajectory) in &config.trajectories {
135            let _ = registry.register(name.as_str(), trajectory.clone());
136        }
137        registry
138    }
139}
stepper_motion/trajectory/registry.rs

stepper_motion/trajectory/
registry.rs
