1use std::{any::type_name, fmt::Debug};
2
3use serde_repr::{Deserialize_repr, Serialize_repr};
4
5pub trait StateSerde {
6 fn state_from_str(s: &str) -> Self;
7 fn state_to_string(&self) -> String;
8}
9
10macro_rules! impl_state_serde {
11 (enum $name:ty) => {
12 impl StateSerde for $name {
13 fn state_from_str(s: &str) -> Self {
14 let d: u8 = s[1..s.len() - 1].parse().unwrap();
15 bincode::deserialize(&[d]).unwrap()
16 }
17 fn state_to_string(&self) -> String {
18 String::from_utf8(bincode::serialize(self).unwrap()).unwrap()
19 }
20 }
21 };
22}
23
24#[derive(Debug, Default, Clone)]
25pub struct RobotState {
26 pub joint_actual_position: [f64; 9],
27 pub actual_position: [f64; 9],
28 pub din: BigArray<bool, 64>,
29 pub dout: BigArray<bool, 64>,
30 pub ain: [[f64; 16]; 1],
31 pub aout: [[f64; 16]; 1],
32 pub tio_din: [bool; 8],
33 pub tio_dout: [bool; 8],
34 pub tio_ain: [f64; 1],
35 pub task_state: TaskState,
36 pub homed: [f64; 9],
37 pub task_mode: TaskMode,
38 pub interp_mode: InterpState,
39 pub enabled: [bool; 1],
40 pub paused: [bool; 1],
41 pub rapidrate: [bool; 1],
42 pub current_tool_id: [bool; 1],
43 pub protective_stop: [bool; 1],
44 pub on_soft_limit: [bool; 1],
45 pub emergency_stop: [bool; 1],
46 pub drag_near_limit: [[bool; 6]; 1],
47}
48
49#[derive(Debug, Clone)]
50pub struct BigArray<T, const N: usize>([T; N]);
51
52impl<T, const N: usize> Default for BigArray<T, N>
53where
54 T: Default + Clone + Copy,
55{
56 fn default() -> Self {
57 BigArray([T::default(); N])
58 }
59}
60
61#[derive(Debug, Default, Serialize_repr, Deserialize_repr, Clone)]
62#[repr(u8)]
63pub enum TaskState {
64 #[default]
65 PowerOff = 1,
66 PowerOn = 2,
67 Disable = 3,
68 Enable = 4,
69}
70impl_state_serde!(enum TaskState);
71
72#[derive(Debug, Default, Serialize_repr, Deserialize_repr, Clone)]
73#[repr(u8)]
74pub enum TaskMode {
75 #[default]
76 Manal = 1,
77 Auto = 2,
78 Guiding = 3,
79}
80impl_state_serde!(enum TaskMode);
81
82#[derive(Debug, Default, Serialize_repr, Deserialize_repr, Clone)]
83#[repr(u8)]
84pub enum InterpState {
85 #[default]
86 Idle = 0,
87 Loading = 1,
88 Pause = 2,
89 Running = 3,
90}
91impl_state_serde!(enum InterpState);
92
93impl StateSerde for f64 {
94 fn state_from_str(s: &str) -> Self {
95 if s.ends_with(',') {
96 return s[1..s.len() - 1].parse().unwrap();
97 }
98 s.parse().unwrap()
99 }
100
101 fn state_to_string(&self) -> String {
102 self.to_string()
103 }
104}
105
106impl StateSerde for bool {
107 fn state_from_str(s: &str) -> Self {
108 match s {
109 "0" => false,
110 "1" => true,
111 _ => {
112 print!("Invalid bool value: {s}");
113 false
114 }
115 }
116 }
117
118 fn state_to_string(&self) -> String {
119 if *self { "1" } else { "0" }.to_string()
120 }
121}
122
123impl<T, const N: usize> StateSerde for [T; N]
124where
125 T: StateSerde + Debug + Copy + Clone,
126{
127 fn state_from_str(s: &str) -> Self {
128 println!("[{}; {}] from {}", type_name::<T>(), N, s);
129 if N == 1 {
130 return [T::state_from_str(&s[1..s.len() - 1]); N];
131 }
132 s[1..s.len() - 1]
133 .split(',')
134 .map(|x| T::state_from_str(x))
135 .collect::<Vec<T>>()
136 .try_into()
137 .unwrap_or_else(|_| panic!("Failed to convert to array: [{}; {}]", type_name::<T>(), N))
138 }
139
140 fn state_to_string(&self) -> String {
141 let data = self
142 .iter()
143 .map(|x| x.state_to_string())
144 .collect::<Vec<String>>()
145 .join(",");
146 format!("({data})")
147 }
148}
149
150impl<T, const N: usize> StateSerde for BigArray<T, N>
151where
152 T: StateSerde + Debug + Copy + Clone,
153{
154 fn state_from_str(s: &str) -> Self {
155 let data = <[T; N]>::state_from_str(s);
156 BigArray(data)
157 }
158
159 fn state_to_string(&self) -> String {
160 self.0.state_to_string()
161 }
162}
163
164impl StateSerde for RobotState {
165 fn state_from_str(s: &str) -> Self {
166 let mut state = RobotState::default();
167 let parts = s.split('\n');
168 for part in parts {
169 let (key, value) = part.split_once(':').unwrap();
170 match key {
171 "joint_actual_position" => {
172 state.joint_actual_position = <[f64; 9]>::state_from_str(value)
173 }
174 "actual_position" => state.actual_position = <[f64; 9]>::state_from_str(value),
175 "din" => state.din = BigArray::<bool, 64>::state_from_str(value),
176 "dout" => state.dout = BigArray::<bool, 64>::state_from_str(value),
177 "ain" => state.ain = <[[f64; 16]; 1]>::state_from_str(value),
178 "aout" => state.aout = <[[f64; 16]; 1]>::state_from_str(value),
179 "tio_din" => state.tio_din = <[bool; 8]>::state_from_str(value),
180 "tio_dout" => state.tio_dout = <[bool; 8]>::state_from_str(value),
181 "tio_ain" => state.tio_ain = <[f64; 1]>::state_from_str(value),
182 "task_state" => state.task_state = TaskState::state_from_str(value),
183 "homed" => state.homed = <[f64; 9]>::state_from_str(value),
184 "task_mode" => state.task_mode = TaskMode::state_from_str(value),
185 "interp_mode" => state.interp_mode = InterpState::state_from_str(value),
186 "enabled" => state.enabled = <[bool; 1]>::state_from_str(value),
187 "paused" => state.paused = <[bool; 1]>::state_from_str(value),
188 "rapidrate" => state.rapidrate = <[bool; 1]>::state_from_str(value),
189 "current_tool_id" => state.current_tool_id = <[bool; 1]>::state_from_str(value),
190 "protective_stop" => state.protective_stop = <[bool; 1]>::state_from_str(value),
191 "on_soft_limit" => state.on_soft_limit = <[bool; 1]>::state_from_str(value),
192 "emergency_stop" => state.emergency_stop = <[bool; 1]>::state_from_str(value),
193 "drag_near_limit" => {
194 state.drag_near_limit = <[[bool; 6]; 1]>::state_from_str(value)
195 }
196 _ => (),
197 }
198 }
199 state
200 }
201
202 fn state_to_string(&self) -> String {
203 format!(
204 "joint_actual_position:{}",
205 self.joint_actual_position.state_to_string()
206 ) + &format!("actual_position:{}", self.actual_position.state_to_string())
207 + &format!("din:{}", self.din.state_to_string())
208 + &format!("dout:{}", self.dout.state_to_string())
209 + &format!("ain:{}", self.ain.state_to_string())
210 + &format!("aout:{}", self.aout.state_to_string())
211 + &format!("tio_din:{}", self.tio_din.state_to_string())
212 + &format!("tio_dout:{}", self.tio_dout.state_to_string())
213 + &format!("tio_ain:{}", self.tio_ain.state_to_string())
214 + &format!("task_state:{}", self.task_state.state_to_string())
215 + &format!("homed:{}", self.homed.state_to_string())
216 + &format!("task_mode:{}", self.task_mode.state_to_string())
217 + &format!("interp_mode:{}", self.interp_mode.state_to_string())
218 + &format!("enabled:{}\n", self.enabled.state_to_string())
219 + &format!("paused:{}\n", self.paused.state_to_string())
220 + &format!("rapidrate:{}\n", self.rapidrate.state_to_string())
221 + &format!(
222 "current_tool_id:{}\n",
223 self.current_tool_id.state_to_string()
224 )
225 + &format!(
226 "protective_stop:{}\n",
227 self.protective_stop.state_to_string()
228 )
229 + &format!("on_soft_limit:{}\n", self.on_soft_limit.state_to_string())
230 + &format!("emergency_stop:{}\n", self.emergency_stop.state_to_string())
231 + &format!("drag_near_limit:{}", self.drag_near_limit.state_to_string())
232 }
233}
234
235#[cfg(test)]
236mod tests {
237 use super::*;
238
239 #[test]
240 fn test_state_serde() {
241 let state_str = r#"joint_actual_position:(0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0)
242actual_position:(0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0)
243din:(0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0)
244dout:(0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0)
245ain:((0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0))
246aout:((0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0))
247tio_din:(0,0,0,0,0,0,0,0)
248tio_dout:(0,0,0,0,0,0,0,0)
249tio_ain:(0.0,)
250task_state:(1)
251homed:(0,0,0,0,0,0,0,0,0)
252task_mode:(1)
253interp_mode:(1)
254enabled:(0)
255paused:(0)
256rapidrate:(1.0)
257current_tool_id:(0)
258protective_stop:(0)
259on_soft_limit:(0)
260emergency_stop:(0)
261drag_near_limit:((0,0,0,0,0,0))"#;
262
263 let _ = RobotState::state_from_str(state_str);
264 }
265}