1use std::{fmt::Debug, sync::Arc};
2
3use crate::{DekeError, DekeResult, SRobotQ, SRobotQLike};
4
5
6pub trait ValidatorContext: Sized {}
7
8#[doc(hidden)]
9pub trait Leaf: ValidatorContext {}
10
11impl ValidatorContext for () {}
12
13#[macro_export]
14macro_rules! validator_context_type_impl {
15 ($($ident:ident),*) => {
16 $(
17 impl $crate::ValidatorContext for $ident {}
18 impl $crate::Leaf for $ident {}
19 )*
20 };
21}
22
23impl<A: ValidatorContext, B: ValidatorContext> ValidatorContext for (A, B) {}
24
25pub trait FromFlattened<Flattened>: ValidatorContext {
26 fn nest(flattened: Flattened) -> Self;
27}
28
29macro_rules! validator_context_tuple_impl {
30 ($tup:tt) => {
31 validator_context_tuple_impl!(@rewrite $tup [emit_impl]);
32 };
33
34 (@rewrite ($l:tt, $r:tt) [$($cb:tt)*]) => {
35 validator_context_tuple_impl!(@rewrite $l [pair_right $r [$($cb)*]]);
36 };
37 (@rewrite $ident:ident [$($cb:tt)*]) => {
38 validator_context_tuple_impl!(@invoke [$($cb)*] [$ident] $ident);
39 validator_context_tuple_impl!(@invoke [$($cb)*] [] ());
40 };
41
42 (@invoke [pair_right $r:tt [$($cb:tt)*]] [$($kept_l:ident)*] $rew_l:tt) => {
43 validator_context_tuple_impl!(@rewrite $r [pair_combine [$($kept_l)*] $rew_l [$($cb)*]]);
44 };
45 (@invoke [pair_combine [$($kept_l:ident)*] $rew_l:tt [$($cb:tt)*]] [$($kept_r:ident)*] $rew_r:tt) => {
46 validator_context_tuple_impl!(@invoke [$($cb)*] [$($kept_l)* $($kept_r)*] ($rew_l, $rew_r));
47 };
48 (@invoke [emit_impl] [$($kept:ident)*] $shape:tt) => {
49 #[allow(non_snake_case)]
50 impl<$($kept: Leaf),*> FromFlattened<($($kept,)*)> for $shape {
51 #[inline]
52 fn nest(flattened: ($($kept,)*)) -> Self {
53 let ($($kept,)*) = flattened;
54 $shape
55 }
56 }
57 };
58}
59
60validator_context_tuple_impl! { (A, (B, C)) }
61validator_context_tuple_impl! { ((A, B), C) }
62
63validator_context_tuple_impl! { (A, (B, (C, D))) }
64validator_context_tuple_impl! { (A, ((B, C), D)) }
65validator_context_tuple_impl! { ((A, B), (C, D)) }
66validator_context_tuple_impl! { ((A, (B, C)), D) }
67validator_context_tuple_impl! { (((A, B), C), D) }
68
69validator_context_tuple_impl! { (A, (B, (C, (D, E)))) }
70validator_context_tuple_impl! { (A, (B, ((C, D), E))) }
71validator_context_tuple_impl! { (A, ((B, C), (D, E))) }
72validator_context_tuple_impl! { (A, ((B, (C, D)), E)) }
73validator_context_tuple_impl! { (A, (((B, C), D), E)) }
74validator_context_tuple_impl! { ((A, B), (C, (D, E))) }
75validator_context_tuple_impl! { ((A, B), ((C, D), E)) }
76validator_context_tuple_impl! { ((A, (B, C)), (D, E)) }
77validator_context_tuple_impl! { (((A, B), C), (D, E)) }
78validator_context_tuple_impl! { ((A, (B, (C, D))), E) }
79validator_context_tuple_impl! { ((A, ((B, C), D)), E) }
80validator_context_tuple_impl! { (((A, B), (C, D)), E) }
81validator_context_tuple_impl! { (((A, (B, C)), D), E) }
82validator_context_tuple_impl! { ((((A, B), C), D), E) }
83
84validator_context_tuple_impl! { ((A, B), ((C, D), (E, F))) }
85validator_context_tuple_impl! { (((A, B), (C, D)), (E, F)) }
86validator_context_tuple_impl! { ((A, (B, C)), ((D, E), F)) }
87validator_context_tuple_impl! { (((A, B), C), ((D, E), F)) }
88
89#[doc(hidden)]
90mod sealed {
91 pub trait Sealed {}
92}
93
94pub trait ValidatorRet: Sized + sealed::Sealed + Copy {
95 fn as_f64(&self) -> f64;
96}
97
98impl sealed::Sealed for () {}
99impl ValidatorRet for () {
100 #[inline]
101 fn as_f64(&self) -> f64 {
102 f64::INFINITY
103 }
104}
105
106impl sealed::Sealed for f32 {}
107impl ValidatorRet for f32 {
108 #[inline]
109 fn as_f64(&self) -> f64 {
110 *self as f64
111 }
112}
113
114impl sealed::Sealed for f64 {}
115impl ValidatorRet for f64 {
116 #[inline]
117 fn as_f64(&self) -> f64 {
118 *self
119 }
120}
121
122pub trait Validator<const N: usize, R: ValidatorRet = ()>: Sized + Clone + Debug + Send + Sync + 'static {
123 type Context<'ctx>: ValidatorContext;
124
125 fn validate<'ctx, E: Into<DekeError>, A: SRobotQLike<N, E>>(
126 &self,
127 q: A,
128 ctx: &Self::Context<'ctx>,
129 ) -> DekeResult<R>;
130 fn validate_motion<'ctx>(
131 &self,
132 qs: &[SRobotQ<N>],
133 ctx: &Self::Context<'ctx>,
134 ) -> DekeResult<R>;
135}
136
137#[derive(Debug, Clone)]
138pub struct ValidatorAnd<A, B>(pub A, pub B);
139
140#[derive(Debug, Clone)]
141pub struct ValidatorOr<A, B>(pub A, pub B);
142
143#[derive(Debug, Clone)]
144pub struct ValidatorNot<A>(pub A);
145
146impl<const N: usize, A, B> Validator<N> for ValidatorAnd<A, B>
147where
148 A: Validator<N>,
149 B: Validator<N>,
150{
151 type Context<'ctx> = (A::Context<'ctx>, B::Context<'ctx>);
152
153 #[inline]
154 fn validate<'ctx, E: Into<DekeError>, Q: SRobotQLike<N, E>>(
155 &self,
156 q: Q,
157 ctx: &Self::Context<'ctx>,
158 ) -> DekeResult<()> {
159 let q = q.to_srobotq().map_err(Into::into)?;
160 self.0.validate(q, &ctx.0)?;
161 self.1.validate(q, &ctx.1)
162 }
163
164 #[inline]
165 fn validate_motion<'ctx>(
166 &self,
167 qs: &[SRobotQ<N>],
168 ctx: &Self::Context<'ctx>,
169 ) -> DekeResult<()> {
170 self.0.validate_motion(qs, &ctx.0)?;
171 self.1.validate_motion(qs, &ctx.1)
172 }
173}
174
175impl<const N: usize, A, B> Validator<N> for ValidatorOr<A, B>
176where
177 A: Validator<N>,
178 B: Validator<N>,
179{
180 type Context<'ctx> = (A::Context<'ctx>, B::Context<'ctx>);
181
182 #[inline]
183 fn validate<'ctx, E: Into<DekeError>, Q: SRobotQLike<N, E>>(
184 &self,
185 q: Q,
186 ctx: &Self::Context<'ctx>,
187 ) -> DekeResult<()> {
188 let q = q.to_srobotq().map_err(Into::into)?;
189 match self.0.validate(q, &ctx.0) {
190 Ok(()) => Ok(()),
191 Err(_) => self.1.validate(q, &ctx.1),
192 }
193 }
194
195 #[inline]
196 fn validate_motion<'ctx>(
197 &self,
198 qs: &[SRobotQ<N>],
199 ctx: &Self::Context<'ctx>,
200 ) -> DekeResult<()> {
201 match self.0.validate_motion(qs, &ctx.0) {
202 Ok(()) => Ok(()),
203 Err(_) => self.1.validate_motion(qs, &ctx.1),
204 }
205 }
206}
207
208impl<const N: usize, A> Validator<N> for ValidatorNot<A>
209where
210 A: Validator<N>,
211{
212 type Context<'ctx> = A::Context<'ctx>;
213
214 #[inline]
215 fn validate<'ctx, E: Into<DekeError>, Q: SRobotQLike<N, E>>(
216 &self,
217 q: Q,
218 ctx: &Self::Context<'ctx>,
219 ) -> DekeResult<()> {
220 let q = q.to_srobotq().map_err(Into::into)?;
221 match self.0.validate(q, ctx) {
222 Ok(()) => Err(DekeError::SuperError),
223 Err(_) => Ok(()),
224 }
225 }
226
227 #[inline]
228 fn validate_motion<'ctx>(
229 &self,
230 qs: &[SRobotQ<N>],
231 ctx: &Self::Context<'ctx>,
232 ) -> DekeResult<()> {
233 match self.0.validate_motion(qs, ctx) {
234 Ok(()) => Err(DekeError::SuperError),
235 Err(_) => Ok(()),
236 }
237 }
238}
239
240#[derive(Clone)]
241pub struct JointValidator<const N: usize> {
242 lower: SRobotQ<N>,
243 upper: SRobotQ<N>,
244 extras: Option<Arc<[Box<dyn Fn(&SRobotQ<N>) -> bool + Send + Sync>]>>,
245}
246
247impl<const N: usize> Debug for JointValidator<N> {
248 fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
249 f.debug_struct("JointValidator")
250 .field("lower", &self.lower)
251 .field("upper", &self.upper)
252 .field(
253 "extras",
254 &format!(
255 "[{} extra checks]",
256 self.extras.as_ref().map(|e| e.len()).unwrap_or(0)
257 ),
258 )
259 .finish()
260 }
261}
262
263impl<const N: usize> JointValidator<N> {
264 pub fn new(lower: SRobotQ<N>, upper: SRobotQ<N>) -> Self {
265 Self {
266 lower,
267 upper,
268 extras: None,
269 }
270 }
271
272 pub fn new_with_extras(
273 lower: SRobotQ<N>,
274 upper: SRobotQ<N>,
275 extras: Vec<Box<dyn Fn(&SRobotQ<N>) -> bool + Send + Sync>>,
276 ) -> Self {
277 Self {
278 lower,
279 upper,
280 extras: Some(extras.into()),
281 }
282 }
283}
284
285impl<const N: usize> Validator<N> for JointValidator<N> {
286 type Context<'ctx> = ();
287
288 #[inline]
289 fn validate<'ctx, E: Into<DekeError>, Q: SRobotQLike<N, E>>(
290 &self,
291 q: Q,
292 _ctx: &Self::Context<'ctx>,
293 ) -> DekeResult<()> {
294 let q = q.to_srobotq().map_err(Into::into)?;
295 if q.any_lt(&self.lower) || q.any_gt(&self.upper) {
296 return Err(DekeError::ExceedJointLimits);
297 }
298 if let Some(extras) = &self.extras {
299 for check in extras.iter() {
300 if !check(&q) {
301 return Err(DekeError::ExceedJointLimits);
302 }
303 }
304 }
305 Ok(())
306 }
307
308 #[inline]
309 fn validate_motion<'ctx>(
310 &self,
311 qs: &[SRobotQ<N>],
312 _ctx: &Self::Context<'ctx>,
313 ) -> DekeResult<()> {
314 for q in qs {
315 self.validate(*q, _ctx)?;
316 }
317 Ok(())
318 }
319}