wm_tools/expansion/
sensorimotor_tools.rs

1//! Sensorimotor tools — sensor reading, actuator control, and reflex loops.
2//!
3//! Provides MCP tools for interacting with the SensorimotorBus:
4//! - `sensor.list` — list all registered sensors
5//! - `sensor.read` — read from a specific sensor
6//! - `sensor.poll` — poll all sensors and return readings
7//! - `sensor.history` — get recent sensor reading history
8//! - `actuator.list` — list all registered actuators
9//! - `actuator.command` — send a command to an actuator
10//! - `actuator.estop` — emergency stop all actuators
11//! - `reflex.list` — list all reflex rules
12//! - `reflex.add` — add a new reflex rule
13//! - `reflex.evaluate` — evaluate reflex rules against current sensor readings
14
15#![forbid(unsafe_code)]
16
17use async_trait::async_trait;
18
19use serde_json::{Value, json};
20use std::sync::{Arc, Mutex};
21use wm_cognitive::reflex::safety::{SAFETY_MASK_ENV, SafetyBit, is_allowed};
22use wm_cognitive::{EventType, GanYingBus, ReflexDispatchTable};
23use wm_core::{Context, EffectRow, Gana, Resource, Tool, ToolStats};
24use wm_substrate::sensorimotor::{
25    ActuatorCommand, ActuatorKind, ReflexLoop, ReflexRule, SensorimotorBus,
26};
27
28// ── Safety gate ───────────────────────────────────────────────────────
29
30/// Refuse actuation unless the attached safety table allows
31/// [`SafetyBit::ActuatorControl`].
32///
33/// A missing table is fail-closed: tools built without the server's live
34/// reflex table cannot actuate. Callers that only read sensors are unaffected
35/// (a stopped clock cannot write; only the actuation path is gated).
36fn actuation_refusal(table: Option<&Arc<Mutex<ReflexDispatchTable>>>) -> Option<String> {
37    let Some(table) = table else {
38        return Some(
39            "actuation denied: no safety table attached (fail-closed) — \
40             the server wires the reflex table at init"
41                .to_string(),
42        );
43    };
44    let Ok(table) = table.lock() else {
45        return Some("actuation denied: safety table mutex poisoned".to_string());
46    };
47    if is_allowed(SafetyBit::ActuatorControl.mask(), table.safety_mask()) {
48        None
49    } else {
50        Some(format!(
51            "actuation denied by safety mask {:#010x} — actuator control requires the \
52             ActuatorControl bit (opt in with {SAFETY_MASK_ENV}=0x…)",
53            table.safety_mask()
54        ))
55    }
56}
57
58// ── Sensor Tools ──────────────────────────────────────────────────────
59
60/// `sensor.list` — list all registered sensors.
61pub struct SensorListTool {
62    bus: Arc<Mutex<SensorimotorBus>>,
63    stats: ToolStats,
64    effects: EffectRow,
65}
66
67impl SensorListTool {
68    pub fn new(bus: Arc<Mutex<SensorimotorBus>>) -> Self {
69        Self {
70            bus,
71            stats: ToolStats::default(),
72            effects: EffectRow::read_only(vec![Resource::Galaxy("substrate".into())]),
73        }
74    }
75}
76
77#[async_trait]
78impl Tool for SensorListTool {
79    fn name(&self) -> &str {
80        "sensor.list"
81    }
82    fn gana(&self) -> Gana {
83        Gana::Dipper
84    }
85    fn effects(&self) -> &EffectRow {
86        &self.effects
87    }
88    fn description(&self) -> &str {
89        "List all registered hardware sensors"
90    }
91    async fn call(&self, _ctx: &mut Context, _args: Value) -> wm_core::Result<Value> {
92        let Ok(bus) = self.bus.lock() else {
93            return Ok(json!({"status": "error", "message": "bus mutex poisoned"}));
94        };
95        Ok(json!({
96            "sensor_count": bus.sensor_count(),
97            "sensors": bus.sensor_ids(),
98            "readings_collected": bus.readings_collected(),
99        }))
100    }
101    fn stats(&self) -> &ToolStats {
102        &self.stats
103    }
104}
105
106/// `sensor.read` — read from a specific sensor by ID.
107pub struct SensorReadTool {
108    bus: Arc<Mutex<SensorimotorBus>>,
109    stats: ToolStats,
110    effects: EffectRow,
111}
112
113impl SensorReadTool {
114    pub fn new(bus: Arc<Mutex<SensorimotorBus>>) -> Self {
115        Self {
116            bus,
117            stats: ToolStats::default(),
118            effects: EffectRow::read_only(vec![Resource::Galaxy("substrate".into())]),
119        }
120    }
121}
122
123#[async_trait]
124impl Tool for SensorReadTool {
125    fn name(&self) -> &str {
126        "sensor.read"
127    }
128    fn gana(&self) -> Gana {
129        Gana::Dipper
130    }
131    fn effects(&self) -> &EffectRow {
132        &self.effects
133    }
134    fn description(&self) -> &str {
135        "Read current value from a specific sensor by ID (args: sensor_id)"
136    }
137    async fn call(&self, _ctx: &mut Context, args: Value) -> wm_core::Result<Value> {
138        let sensor_id = args.get("sensor_id").and_then(Value::as_str).unwrap_or("");
139        if sensor_id.is_empty() {
140            return Ok(json!({
141                "status": "error",
142                "message": "Missing required parameter: sensor_id",
143            }));
144        }
145
146        let Ok(bus) = self.bus.lock() else {
147            return Ok(json!({"status": "error", "message": "bus mutex poisoned"}));
148        };
149        match bus.read_sensor(sensor_id) {
150            Some(reading) => Ok(json!({
151                "sensor_id": reading.sensor_id,
152                "kind": reading.kind.as_str(),
153                "value": reading.value,
154                "extra": reading.extra,
155                "timestamp": reading.timestamp,
156                "confidence": reading.confidence,
157            })),
158            None => Ok(json!({
159                "status": "error",
160                "message": format!("Sensor '{sensor_id}' not found or unavailable"),
161            })),
162        }
163    }
164    fn stats(&self) -> &ToolStats {
165        &self.stats
166    }
167}
168
169/// `sensor.poll` — poll all sensors and return readings.
170pub struct SensorPollTool {
171    bus: Arc<Mutex<SensorimotorBus>>,
172    gan_ying: Option<Arc<Mutex<GanYingBus>>>,
173    stats: ToolStats,
174    effects: EffectRow,
175}
176
177impl SensorPollTool {
178    pub fn new(bus: Arc<Mutex<SensorimotorBus>>) -> Self {
179        Self {
180            bus,
181            gan_ying: None,
182            stats: ToolStats::default(),
183            effects: EffectRow::read_only(vec![Resource::Galaxy("substrate".into())]),
184        }
185    }
186
187    pub fn with_gan_ying(
188        bus: Arc<Mutex<SensorimotorBus>>,
189        gan_ying: Arc<Mutex<GanYingBus>>,
190    ) -> Self {
191        Self {
192            bus,
193            gan_ying: Some(gan_ying),
194            stats: ToolStats::default(),
195            effects: EffectRow::read_only(vec![Resource::Galaxy("substrate".into())]),
196        }
197    }
198}
199
200#[async_trait]
201impl Tool for SensorPollTool {
202    fn name(&self) -> &str {
203        "sensor.poll"
204    }
205    fn gana(&self) -> Gana {
206        Gana::Dipper
207    }
208    fn effects(&self) -> &EffectRow {
209        &self.effects
210    }
211    fn description(&self) -> &str {
212        "Poll all registered sensors and return current readings"
213    }
214    async fn call(&self, _ctx: &mut Context, _args: Value) -> wm_core::Result<Value> {
215        let Ok(mut bus) = self.bus.lock() else {
216            return Ok(json!({"status": "error", "message": "bus mutex poisoned"}));
217        };
218        let readings = bus.poll_all();
219        let readings_json: Vec<Value> = readings
220            .iter()
221            .map(|r| {
222                json!({
223                    "sensor_id": r.sensor_id,
224                    "kind": r.kind.as_str(),
225                    "value": r.value,
226                    "extra": r.extra,
227                    "timestamp": r.timestamp,
228                    "confidence": r.confidence,
229                })
230            })
231            .collect();
232
233        // Emit SensorFrameReceived to Gan Ying Bus if connected
234        if let Some(gan_ying) = &self.gan_ying {
235            if let Ok(mut bus) = gan_ying.lock() {
236                bus.emit(
237                    EventType::SensorFrameReceived,
238                    "sensorimotor",
239                    json!({
240                        "sensor_count": readings.len(),
241                        "sensors": readings.iter().map(|r| {
242                            json!({
243                                "id": r.sensor_id,
244                                "kind": r.kind.as_str(),
245                                "value": r.value,
246                            })
247                        }).collect::<Vec<_>>(),
248                    }),
249                );
250            }
251        }
252
253        Ok(json!({
254            "count": readings.len(),
255            "readings": readings_json,
256        }))
257    }
258    fn stats(&self) -> &ToolStats {
259        &self.stats
260    }
261}
262
263/// `sensor.history` — get recent sensor reading history.
264pub struct SensorHistoryTool {
265    bus: Arc<Mutex<SensorimotorBus>>,
266    stats: ToolStats,
267    effects: EffectRow,
268}
269
270impl SensorHistoryTool {
271    pub fn new(bus: Arc<Mutex<SensorimotorBus>>) -> Self {
272        Self {
273            bus,
274            stats: ToolStats::default(),
275            effects: EffectRow::read_only(vec![Resource::Galaxy("substrate".into())]),
276        }
277    }
278}
279
280#[async_trait]
281impl Tool for SensorHistoryTool {
282    fn name(&self) -> &str {
283        "sensor.history"
284    }
285    fn gana(&self) -> Gana {
286        Gana::Dipper
287    }
288    fn effects(&self) -> &EffectRow {
289        &self.effects
290    }
291    fn description(&self) -> &str {
292        "Get recent sensor reading history (optional args: limit)"
293    }
294    async fn call(&self, _ctx: &mut Context, args: Value) -> wm_core::Result<Value> {
295        let limit = args.get("limit").and_then(Value::as_u64).unwrap_or(50) as usize;
296
297        let Ok(bus) = self.bus.lock() else {
298            return Ok(json!({"status": "error", "message": "bus mutex poisoned"}));
299        };
300        let history = bus.history();
301        let limited: Vec<Value> = history
302            .iter()
303            .rev()
304            .take(limit)
305            .map(|r| {
306                json!({
307                    "sensor_id": r.sensor_id,
308                    "kind": r.kind.as_str(),
309                    "value": r.value,
310                    "timestamp": r.timestamp,
311                })
312            })
313            .collect();
314        Ok(json!({
315            "count": limited.len(),
316            "total_collected": bus.readings_collected(),
317            "readings": limited,
318        }))
319    }
320    fn stats(&self) -> &ToolStats {
321        &self.stats
322    }
323}
324
325// ── Actuator Tools ────────────────────────────────────────────────────
326
327/// `actuator.list` — list all registered actuators.
328pub struct ActuatorListTool {
329    bus: Arc<Mutex<SensorimotorBus>>,
330    stats: ToolStats,
331    effects: EffectRow,
332}
333
334impl ActuatorListTool {
335    pub fn new(bus: Arc<Mutex<SensorimotorBus>>) -> Self {
336        Self {
337            bus,
338            stats: ToolStats::default(),
339            effects: EffectRow::read_only(vec![Resource::Galaxy("substrate".into())]),
340        }
341    }
342}
343
344#[async_trait]
345impl Tool for ActuatorListTool {
346    fn name(&self) -> &str {
347        "actuator.list"
348    }
349    fn gana(&self) -> Gana {
350        Gana::Dipper
351    }
352    fn effects(&self) -> &EffectRow {
353        &self.effects
354    }
355    fn description(&self) -> &str {
356        "List all registered actuators"
357    }
358    async fn call(&self, _ctx: &mut Context, _args: Value) -> wm_core::Result<Value> {
359        let Ok(bus) = self.bus.lock() else {
360            return Ok(json!({"status": "error", "message": "bus mutex poisoned"}));
361        };
362        Ok(json!({
363            "actuator_count": bus.actuator_count(),
364            "actuators": bus.actuator_ids(),
365            "commands_sent": bus.commands_sent(),
366        }))
367    }
368    fn stats(&self) -> &ToolStats {
369        &self.stats
370    }
371}
372
373/// `actuator.command` — send a command to an actuator.
374pub struct ActuatorCommandTool {
375    bus: Arc<Mutex<SensorimotorBus>>,
376    gan_ying: Option<Arc<Mutex<GanYingBus>>>,
377    safety: Option<Arc<Mutex<ReflexDispatchTable>>>,
378    stats: ToolStats,
379    effects: EffectRow,
380}
381
382impl ActuatorCommandTool {
383    pub fn new(bus: Arc<Mutex<SensorimotorBus>>) -> Self {
384        Self {
385            bus,
386            gan_ying: None,
387            safety: None,
388            stats: ToolStats::default(),
389            effects: EffectRow {
390                writes: vec![Resource::Galaxy("substrate".into())],
391                ..Default::default()
392            },
393        }
394    }
395
396    pub fn with_gan_ying(
397        bus: Arc<Mutex<SensorimotorBus>>,
398        gan_ying: Arc<Mutex<GanYingBus>>,
399    ) -> Self {
400        Self {
401            bus,
402            gan_ying: Some(gan_ying),
403            safety: None,
404            stats: ToolStats::default(),
405            effects: EffectRow {
406                writes: vec![Resource::Galaxy("substrate".into())],
407                ..Default::default()
408            },
409        }
410    }
411
412    /// Attach the live reflex safety table. Without it, actuation is refused
413    /// (fail-closed).
414    #[must_use]
415    pub fn with_safety(mut self, table: Arc<Mutex<ReflexDispatchTable>>) -> Self {
416        self.safety = Some(table);
417        self
418    }
419}
420
421#[async_trait]
422impl Tool for ActuatorCommandTool {
423    fn name(&self) -> &str {
424        "actuator.command"
425    }
426    fn gana(&self) -> Gana {
427        Gana::Dipper
428    }
429    fn effects(&self) -> &EffectRow {
430        &self.effects
431    }
432    fn description(&self) -> &str {
433        "Send a command to an actuator (args: actuator_id, value, optional: kind, params). \
434         Safely gated: requires the ActuatorControl bit in the reflex safety mask."
435    }
436    async fn call(&self, _ctx: &mut Context, args: Value) -> wm_core::Result<Value> {
437        let actuator_id = args
438            .get("actuator_id")
439            .and_then(Value::as_str)
440            .unwrap_or("");
441        if actuator_id.is_empty() {
442            return Ok(json!({
443                "status": "error",
444                "message": "Missing required parameter: actuator_id",
445            }));
446        }
447
448        let value = args.get("value").and_then(Value::as_f64).unwrap_or(0.0);
449
450        let kind_str = args.get("kind").and_then(Value::as_str).unwrap_or("custom");
451        let kind = parse_actuator_kind(kind_str);
452
453        let params: Vec<f64> = args
454            .get("params")
455            .and_then(Value::as_array)
456            .map(|arr| arr.iter().filter_map(Value::as_f64).collect())
457            .unwrap_or_default();
458
459        let cmd = ActuatorCommand::new(actuator_id, kind, value).with_params(params);
460
461        if let Some(reason) = actuation_refusal(self.safety.as_ref()) {
462            return Ok(json!({
463                "status": "error",
464                "blocked_by": "safety-mask",
465                "message": reason,
466            }));
467        }
468
469        let Ok(mut bus) = self.bus.lock() else {
470            return Ok(json!({"status": "error", "message": "bus mutex poisoned"}));
471        };
472        match bus.send_command(&cmd) {
473            Ok(()) => {
474                // Emit ActuatorCommandSent to Gan Ying Bus if connected
475                if let Some(gan_ying) = &self.gan_ying {
476                    if let Ok(mut gy) = gan_ying.lock() {
477                        gy.emit(
478                            EventType::ActuatorCommandSent,
479                            "sensorimotor",
480                            json!({
481                                "actuator_id": actuator_id,
482                                "value": value,
483                                "kind": kind.as_str(),
484                            }),
485                        );
486                    }
487                }
488                Ok(json!({
489                    "status": "ok",
490                    "actuator_id": actuator_id,
491                    "value": value,
492                    "commands_sent": bus.commands_sent(),
493                }))
494            }
495            Err(e) => Ok(json!({
496                "status": "error",
497                "message": e,
498            })),
499        }
500    }
501    fn stats(&self) -> &ToolStats {
502        &self.stats
503    }
504}
505
506/// `actuator.estop` — emergency stop all actuators.
507pub struct ActuatorEStopTool {
508    bus: Arc<Mutex<SensorimotorBus>>,
509    gan_ying: Option<Arc<Mutex<GanYingBus>>>,
510    stats: ToolStats,
511    effects: EffectRow,
512}
513
514impl ActuatorEStopTool {
515    pub fn new(bus: Arc<Mutex<SensorimotorBus>>) -> Self {
516        Self {
517            bus,
518            gan_ying: None,
519            stats: ToolStats::default(),
520            effects: EffectRow {
521                writes: vec![Resource::Galaxy("substrate".into())],
522                ..Default::default()
523            },
524        }
525    }
526
527    pub fn with_gan_ying(
528        bus: Arc<Mutex<SensorimotorBus>>,
529        gan_ying: Arc<Mutex<GanYingBus>>,
530    ) -> Self {
531        Self {
532            bus,
533            gan_ying: Some(gan_ying),
534            stats: ToolStats::default(),
535            effects: EffectRow {
536                writes: vec![Resource::Galaxy("substrate".into())],
537                ..Default::default()
538            },
539        }
540    }
541}
542
543#[async_trait]
544impl Tool for ActuatorEStopTool {
545    fn name(&self) -> &str {
546        "actuator.estop"
547    }
548    fn gana(&self) -> Gana {
549        Gana::Dipper
550    }
551    fn effects(&self) -> &EffectRow {
552        &self.effects
553    }
554    fn description(&self) -> &str {
555        "Emergency stop all actuators"
556    }
557    async fn call(&self, _ctx: &mut Context, _args: Value) -> wm_core::Result<Value> {
558        let Ok(bus) = self.bus.lock() else {
559            return Ok(json!({"status": "error", "message": "bus mutex poisoned"}));
560        };
561        let errors = bus.e_stop_all();
562
563        // Emit ReflexEmergencyStop to Gan Ying Bus if connected
564        if let Some(gan_ying) = &self.gan_ying {
565            if let Ok(mut gy) = gan_ying.lock() {
566                gy.emit_with(
567                    EventType::ReflexEmergencyStop,
568                    "sensorimotor",
569                    json!({"errors": errors.len()}),
570                    0.9,
571                    true,
572                );
573            }
574        }
575
576        if errors.is_empty() {
577            Ok(json!({"status": "ok", "message": "All actuators stopped"}))
578        } else {
579            Ok(json!({
580                "status": "partial",
581                "errors": errors,
582            }))
583        }
584    }
585    fn stats(&self) -> &ToolStats {
586        &self.stats
587    }
588}
589
590// ── Reflex Tools ──────────────────────────────────────────────────────
591
592/// `reflex.list` — list all reflex rules.
593pub struct ReflexListTool {
594    reflex: Arc<Mutex<ReflexLoop>>,
595    stats: ToolStats,
596    effects: EffectRow,
597}
598
599impl ReflexListTool {
600    pub fn new(reflex: Arc<Mutex<ReflexLoop>>) -> Self {
601        Self {
602            reflex,
603            stats: ToolStats::default(),
604            effects: EffectRow::read_only(vec![Resource::Galaxy("substrate".into())]),
605        }
606    }
607}
608
609#[async_trait]
610impl Tool for ReflexListTool {
611    fn name(&self) -> &str {
612        "reflex.list"
613    }
614    fn gana(&self) -> Gana {
615        Gana::Dipper
616    }
617    fn effects(&self) -> &EffectRow {
618        &self.effects
619    }
620    fn description(&self) -> &str {
621        "List all registered reflex rules"
622    }
623    async fn call(&self, _ctx: &mut Context, _args: Value) -> wm_core::Result<Value> {
624        let Ok(reflex) = self.reflex.lock() else {
625            return Ok(json!({"status": "error", "message": "reflex mutex poisoned"}));
626        };
627        Ok(json!({
628            "rule_count": reflex.rule_count(),
629        }))
630    }
631    fn stats(&self) -> &ToolStats {
632        &self.stats
633    }
634}
635
636/// `reflex.add` — add a new reflex rule.
637pub struct ReflexAddTool {
638    reflex: Arc<Mutex<ReflexLoop>>,
639    stats: ToolStats,
640    effects: EffectRow,
641}
642
643impl ReflexAddTool {
644    pub fn new(reflex: Arc<Mutex<ReflexLoop>>) -> Self {
645        Self {
646            reflex,
647            stats: ToolStats::default(),
648            effects: EffectRow {
649                writes: vec![Resource::Galaxy("substrate".into())],
650                ..Default::default()
651            },
652        }
653    }
654}
655
656#[async_trait]
657impl Tool for ReflexAddTool {
658    fn name(&self) -> &str {
659        "reflex.add"
660    }
661    fn gana(&self) -> Gana {
662        Gana::Dipper
663    }
664    fn effects(&self) -> &EffectRow {
665        &self.effects
666    }
667    fn description(&self) -> &str {
668        "Add a reflex rule (args: sensor_id, actuator_id, actuator_kind, threshold, command_value, trigger_above, cooldown_secs)"
669    }
670    async fn call(&self, _ctx: &mut Context, args: Value) -> wm_core::Result<Value> {
671        let sensor_id = args.get("sensor_id").and_then(Value::as_str).unwrap_or("");
672        let actuator_id = args
673            .get("actuator_id")
674            .and_then(Value::as_str)
675            .unwrap_or("");
676        let actuator_kind = parse_actuator_kind(
677            args.get("actuator_kind")
678                .and_then(Value::as_str)
679                .unwrap_or("custom"),
680        );
681        let threshold = args.get("threshold").and_then(Value::as_f64).unwrap_or(1.0);
682        let command_value = args
683            .get("command_value")
684            .and_then(Value::as_f64)
685            .unwrap_or(0.0);
686        let trigger_above = args
687            .get("trigger_above")
688            .and_then(Value::as_bool)
689            .unwrap_or(true);
690        let cooldown_secs = args
691            .get("cooldown_secs")
692            .and_then(Value::as_f64)
693            .unwrap_or(1.0);
694
695        if sensor_id.is_empty() || actuator_id.is_empty() {
696            return Ok(json!({
697                "status": "error",
698                "message": "Missing required parameters: sensor_id and actuator_id",
699            }));
700        }
701
702        let rule = if trigger_above {
703            ReflexRule::above(
704                sensor_id,
705                actuator_id,
706                actuator_kind,
707                threshold,
708                command_value,
709                cooldown_secs,
710            )
711        } else {
712            ReflexRule::below(
713                sensor_id,
714                actuator_id,
715                actuator_kind,
716                threshold,
717                command_value,
718                cooldown_secs,
719            )
720        };
721
722        let Ok(mut reflex) = self.reflex.lock() else {
723            return Ok(json!({"status": "error", "message": "reflex mutex poisoned"}));
724        };
725        reflex.add_rule(rule);
726        Ok(json!({
727            "status": "ok",
728            "rule_count": reflex.rule_count(),
729        }))
730    }
731    fn stats(&self) -> &ToolStats {
732        &self.stats
733    }
734}
735
736/// `reflex.evaluate` — evaluate reflex rules against current sensor readings.
737pub struct ReflexEvaluateTool {
738    bus: Arc<Mutex<SensorimotorBus>>,
739    reflex: Arc<Mutex<ReflexLoop>>,
740    gan_ying: Option<Arc<Mutex<GanYingBus>>>,
741    safety: Option<Arc<Mutex<ReflexDispatchTable>>>,
742    stats: ToolStats,
743    effects: EffectRow,
744}
745
746impl ReflexEvaluateTool {
747    pub fn new(bus: Arc<Mutex<SensorimotorBus>>, reflex: Arc<Mutex<ReflexLoop>>) -> Self {
748        Self {
749            bus,
750            reflex,
751            gan_ying: None,
752            safety: None,
753            stats: ToolStats::default(),
754            effects: EffectRow {
755                writes: vec![Resource::Galaxy("substrate".into())],
756                ..Default::default()
757            },
758        }
759    }
760
761    pub fn with_gan_ying(
762        bus: Arc<Mutex<SensorimotorBus>>,
763        reflex: Arc<Mutex<ReflexLoop>>,
764        gan_ying: Arc<Mutex<GanYingBus>>,
765    ) -> Self {
766        Self {
767            bus,
768            reflex,
769            gan_ying: Some(gan_ying),
770            safety: None,
771            stats: ToolStats::default(),
772            effects: EffectRow {
773                writes: vec![Resource::Galaxy("substrate".into())],
774                ..Default::default()
775            },
776        }
777    }
778
779    /// Attach the live reflex safety table. Without it, triggered commands
780    /// are reported but never executed (fail-closed).
781    #[must_use]
782    pub fn with_safety(mut self, table: Arc<Mutex<ReflexDispatchTable>>) -> Self {
783        self.safety = Some(table);
784        self
785    }
786}
787
788#[async_trait]
789impl Tool for ReflexEvaluateTool {
790    fn name(&self) -> &str {
791        "reflex.evaluate"
792    }
793    fn gana(&self) -> Gana {
794        Gana::Dipper
795    }
796    fn effects(&self) -> &EffectRow {
797        &self.effects
798    }
799    fn description(&self) -> &str {
800        "Poll sensors, evaluate reflex rules, and send any triggered actuator commands \
801         (execution is gated by the ActuatorControl bit in the reflex safety mask)"
802    }
803    async fn call(&self, _ctx: &mut Context, _args: Value) -> wm_core::Result<Value> {
804        let readings = {
805            let Ok(mut bus) = self.bus.lock() else {
806                return Ok(json!({"status": "error", "message": "bus mutex poisoned"}));
807            };
808            bus.poll_all()
809        };
810
811        let commands = {
812            let Ok(mut reflex) = self.reflex.lock() else {
813                return Ok(json!({"status": "error", "message": "reflex mutex poisoned"}));
814            };
815            reflex.evaluate(&readings)
816        };
817
818        let mut executed = 0usize;
819        let mut blocked = 0usize;
820        let mut errors = Vec::new();
821
822        if !commands.is_empty() {
823            // Emit ReflexFired to Gan Ying Bus if connected
824            if let Some(gan_ying) = &self.gan_ying {
825                if let Ok(mut gy) = gan_ying.lock() {
826                    gy.emit_with(
827                        EventType::ReflexFired,
828                        "sensorimotor",
829                        json!({
830                            "sensors_polled": readings.len(),
831                            "commands_triggered": commands.len(),
832                        }),
833                        0.7,
834                        true,
835                    );
836                }
837            }
838
839            if let Some(reason) = actuation_refusal(self.safety.as_ref()) {
840                blocked = commands.len();
841                errors.push(reason);
842            } else {
843                let Ok(mut bus) = self.bus.lock() else {
844                    return Ok(json!({"status": "error", "message": "bus mutex poisoned"}));
845                };
846                for cmd in &commands {
847                    match bus.send_command(cmd) {
848                        Ok(()) => executed += 1,
849                        Err(e) => errors.push(e),
850                    }
851                }
852            }
853        }
854
855        Ok(json!({
856            "sensors_polled": readings.len(),
857            "commands_triggered": commands.len(),
858            "commands_executed": executed,
859            "commands_blocked": blocked,
860            "errors": errors,
861        }))
862    }
863    fn stats(&self) -> &ToolStats {
864        &self.stats
865    }
866}
867
868// ── Helpers ───────────────────────────────────────────────────────────
869
870/// Parse actuator kind from string.
871fn parse_actuator_kind(s: &str) -> ActuatorKind {
872    match s {
873        "motor" => ActuatorKind::Motor,
874        "relay" => ActuatorKind::Relay,
875        "display" => ActuatorKind::Display,
876        "speaker" => ActuatorKind::Speaker,
877        "valve" => ActuatorKind::Valve,
878        "thermal" => ActuatorKind::Thermal,
879        _ => ActuatorKind::Custom,
880    }
881}
882
883// ── Registration ──────────────────────────────────────────────────────
884
885/// Register all sensorimotor tools.
886///
887/// If `gan_ying` is provided, event-emitting tools will emit resonance events.
888/// If `safety` is provided, the actuation path is gated by its safety mask;
889/// without it, actuation is refused (fail-closed).
890pub fn register_sensorimotor(
891    registry: &wm_dispatch::ToolRegistry,
892    bus: Arc<Mutex<SensorimotorBus>>,
893    reflex: Arc<Mutex<ReflexLoop>>,
894    gan_ying: Option<&Arc<Mutex<GanYingBus>>>,
895    safety: Option<&Arc<Mutex<ReflexDispatchTable>>>,
896) -> wm_dispatch::ToolRegistry {
897    let poll = match gan_ying {
898        Some(gy) => SensorPollTool::with_gan_ying(bus.clone(), Arc::clone(gy)),
899        None => SensorPollTool::new(bus.clone()),
900    };
901    let cmd = match gan_ying {
902        Some(gy) => ActuatorCommandTool::with_gan_ying(bus.clone(), Arc::clone(gy)),
903        None => ActuatorCommandTool::new(bus.clone()),
904    };
905    let cmd = match safety {
906        Some(table) => cmd.with_safety(Arc::clone(table)),
907        None => cmd,
908    };
909    let estop = match &gan_ying {
910        Some(gy) => ActuatorEStopTool::with_gan_ying(bus.clone(), Arc::clone(gy)),
911        None => ActuatorEStopTool::new(bus.clone()),
912    };
913    let eval = match &gan_ying {
914        Some(gy) => ReflexEvaluateTool::with_gan_ying(bus.clone(), reflex.clone(), Arc::clone(gy)),
915        None => ReflexEvaluateTool::new(bus.clone(), reflex.clone()),
916    };
917    let eval = match safety {
918        Some(table) => eval.with_safety(Arc::clone(table)),
919        None => eval,
920    };
921    registry
922        .register(Arc::new(SensorListTool::new(bus.clone())))
923        .register(Arc::new(SensorReadTool::new(bus.clone())))
924        .register(Arc::new(poll))
925        .register(Arc::new(SensorHistoryTool::new(bus.clone())))
926        .register(Arc::new(ActuatorListTool::new(bus)))
927        .register(Arc::new(cmd))
928        .register(Arc::new(estop))
929        .register(Arc::new(ReflexListTool::new(reflex.clone())))
930        .register(Arc::new(ReflexAddTool::new(reflex)))
931        .register(Arc::new(eval))
932}
933
934// ── Tests ─────────────────────────────────────────────────────────────
935
936#[cfg(test)]
937mod tests {
938    use super::*;
939    use wm_cognitive::reflex::safety::SAFETY_DEFAULT;
940    use wm_substrate::sensorimotor::{SensorKind, StubActuator, StubSensor};
941
942    fn make_bus() -> Arc<Mutex<SensorimotorBus>> {
943        let mut bus = SensorimotorBus::new(64);
944        bus.register_sensor(Box::new(StubSensor::new(
945            "temp0",
946            SensorKind::Temperature,
947            55.0,
948        )));
949        bus.register_sensor(Box::new(StubSensor::new("load0", SensorKind::Custom, 0.3)));
950        Arc::new(Mutex::new(bus))
951    }
952
953    fn make_actuator_bus() -> Arc<Mutex<SensorimotorBus>> {
954        let mut bus = SensorimotorBus::new(64);
955        bus.register_sensor(Box::new(StubSensor::new(
956            "temp0",
957            SensorKind::Temperature,
958            55.0,
959        )));
960        bus.register_actuator(Box::new(StubActuator::new("fan0", ActuatorKind::Motor)));
961        Arc::new(Mutex::new(bus))
962    }
963
964    fn safety_table(mask: u32) -> Arc<Mutex<ReflexDispatchTable>> {
965        Arc::new(Mutex::new(ReflexDispatchTable::new(mask)))
966    }
967
968    fn make_reflex() -> Arc<Mutex<ReflexLoop>> {
969        Arc::new(Mutex::new(ReflexLoop::new()))
970    }
971
972    #[tokio::test]
973    async fn sensor_list_tool() {
974        let tool = SensorListTool::new(make_bus());
975        let mut ctx = Context::default();
976        let result = tool.call(&mut ctx, json!({})).await.unwrap();
977        assert_eq!(result["sensor_count"], 2);
978        assert!(
979            result["sensors"]
980                .as_array()
981                .unwrap()
982                .contains(&json!("temp0"))
983        );
984    }
985
986    #[tokio::test]
987    async fn sensor_read_tool() {
988        let tool = SensorReadTool::new(make_bus());
989        let mut ctx = Context::default();
990        let result = tool
991            .call(&mut ctx, json!({"sensor_id": "temp0"}))
992            .await
993            .unwrap();
994        assert_eq!(result["sensor_id"], "temp0");
995        assert!((result["value"].as_f64().unwrap() - 55.0).abs() < 0.01);
996    }
997
998    #[tokio::test]
999    async fn sensor_read_missing_id() {
1000        let tool = SensorReadTool::new(make_bus());
1001        let mut ctx = Context::default();
1002        let result = tool.call(&mut ctx, json!({})).await.unwrap();
1003        assert_eq!(result["status"], "error");
1004    }
1005
1006    #[tokio::test]
1007    async fn sensor_read_nonexistent() {
1008        let tool = SensorReadTool::new(make_bus());
1009        let mut ctx = Context::default();
1010        let result = tool
1011            .call(&mut ctx, json!({"sensor_id": "nonexistent"}))
1012            .await
1013            .unwrap();
1014        assert_eq!(result["status"], "error");
1015    }
1016
1017    #[tokio::test]
1018    async fn sensor_poll_tool() {
1019        let tool = SensorPollTool::new(make_bus());
1020        let mut ctx = Context::default();
1021        let result = tool.call(&mut ctx, json!({})).await.unwrap();
1022        assert_eq!(result["count"], 2);
1023        assert!(result["readings"].is_array());
1024    }
1025
1026    #[tokio::test]
1027    async fn sensor_history_tool() {
1028        let bus = make_bus();
1029        {
1030            let mut b = bus.lock().unwrap();
1031            let _ = b.poll_all();
1032            let _ = b.poll_all();
1033        }
1034        let tool = SensorHistoryTool::new(bus);
1035        let mut ctx = Context::default();
1036        let result = tool.call(&mut ctx, json!({"limit": 5})).await.unwrap();
1037        assert!(result["count"].as_u64().unwrap() > 0);
1038    }
1039
1040    #[tokio::test]
1041    async fn actuator_list_tool() {
1042        let tool = ActuatorListTool::new(make_bus());
1043        let mut ctx = Context::default();
1044        let result = tool.call(&mut ctx, json!({})).await.unwrap();
1045        assert_eq!(result["actuator_count"], 0);
1046    }
1047
1048    #[tokio::test]
1049    async fn actuator_command_missing_id() {
1050        let tool = ActuatorCommandTool::new(make_bus());
1051        let mut ctx = Context::default();
1052        let result = tool.call(&mut ctx, json!({})).await.unwrap();
1053        assert_eq!(result["status"], "error");
1054    }
1055
1056    #[tokio::test]
1057    async fn actuator_command_nonexistent() {
1058        let tool = ActuatorCommandTool::new(make_bus());
1059        let mut ctx = Context::default();
1060        let result = tool
1061            .call(
1062                &mut ctx,
1063                json!({"actuator_id": "nonexistent", "value": 0.5}),
1064            )
1065            .await
1066            .unwrap();
1067        assert_eq!(result["status"], "error");
1068    }
1069
1070    #[tokio::test]
1071    async fn actuator_estop_tool() {
1072        let tool = ActuatorEStopTool::new(make_bus());
1073        let mut ctx = Context::default();
1074        let result = tool.call(&mut ctx, json!({})).await.unwrap();
1075        assert_eq!(result["status"], "ok");
1076    }
1077
1078    #[tokio::test]
1079    async fn actuator_command_fails_closed_without_safety_table() {
1080        let tool = ActuatorCommandTool::new(make_actuator_bus());
1081        let mut ctx = Context::default();
1082        let result = tool
1083            .call(&mut ctx, json!({"actuator_id": "fan0", "value": 0.5}))
1084            .await
1085            .unwrap();
1086        assert_eq!(result["status"], "error");
1087        assert_eq!(result["blocked_by"], "safety-mask");
1088    }
1089
1090    #[tokio::test]
1091    async fn actuator_command_denied_by_default_mask() {
1092        let tool =
1093            ActuatorCommandTool::new(make_actuator_bus()).with_safety(safety_table(SAFETY_DEFAULT));
1094        let mut ctx = Context::default();
1095        let result = tool
1096            .call(&mut ctx, json!({"actuator_id": "fan0", "value": 0.5}))
1097            .await
1098            .unwrap();
1099        assert_eq!(result["status"], "error");
1100        assert_eq!(result["blocked_by"], "safety-mask");
1101    }
1102
1103    #[tokio::test]
1104    async fn actuator_command_allowed_with_actuator_bit() {
1105        let tool = ActuatorCommandTool::new(make_actuator_bus())
1106            .with_safety(safety_table(SafetyBit::ActuatorControl.mask()));
1107        let mut ctx = Context::default();
1108        let result = tool
1109            .call(&mut ctx, json!({"actuator_id": "fan0", "value": 0.5}))
1110            .await
1111            .unwrap();
1112        assert_eq!(result["status"], "ok");
1113        assert_eq!(result["actuator_id"], "fan0");
1114    }
1115
1116    #[tokio::test]
1117    async fn reflex_list_tool() {
1118        let tool = ReflexListTool::new(make_reflex());
1119        let mut ctx = Context::default();
1120        let result = tool.call(&mut ctx, json!({})).await.unwrap();
1121        assert_eq!(result["rule_count"], 0);
1122    }
1123
1124    #[tokio::test]
1125    async fn reflex_add_tool() {
1126        let reflex = make_reflex();
1127        let tool = ReflexAddTool::new(reflex);
1128        let mut ctx = Context::default();
1129        let result = tool
1130            .call(
1131                &mut ctx,
1132                json!({
1133                    "sensor_id": "temp0",
1134                    "actuator_id": "fan0",
1135                    "actuator_kind": "motor",
1136                    "threshold": 70.0,
1137                    "command_value": 1.0,
1138                    "trigger_above": true,
1139                    "cooldown_secs": 5.0,
1140                }),
1141            )
1142            .await
1143            .unwrap();
1144        assert_eq!(result["status"], "ok");
1145        assert_eq!(result["rule_count"], 1);
1146    }
1147
1148    #[tokio::test]
1149    async fn reflex_add_missing_params() {
1150        let tool = ReflexAddTool::new(make_reflex());
1151        let mut ctx = Context::default();
1152        let result = tool.call(&mut ctx, json!({})).await.unwrap();
1153        assert_eq!(result["status"], "error");
1154    }
1155
1156    #[tokio::test]
1157    async fn reflex_evaluate_tool() {
1158        let bus = make_bus();
1159        let reflex = make_reflex();
1160
1161        // Add a rule that triggers when temp0 > 50 (it reads 55.0)
1162        {
1163            let mut r = reflex.lock().unwrap();
1164            r.add_rule(ReflexRule::above(
1165                "temp0",
1166                "fan0",
1167                ActuatorKind::Motor,
1168                50.0,
1169                1.0,
1170                0.0,
1171            ));
1172        }
1173
1174        // No actuator registered, so command will fail
1175        let tool = ReflexEvaluateTool::new(bus, reflex);
1176        let mut ctx = Context::default();
1177        let result = tool.call(&mut ctx, json!({})).await.unwrap();
1178        assert_eq!(result["sensors_polled"], 2);
1179        // Command triggered but not executed (no actuator)
1180        assert!(result["commands_triggered"].as_u64().unwrap() > 0);
1181    }
1182
1183    #[tokio::test]
1184    async fn reflex_evaluate_blocks_commands_without_safety_table() {
1185        let bus = make_actuator_bus();
1186        let reflex = make_reflex();
1187        {
1188            let mut r = reflex.lock().unwrap();
1189            r.add_rule(ReflexRule::above(
1190                "temp0",
1191                "fan0",
1192                ActuatorKind::Motor,
1193                50.0,
1194                1.0,
1195                0.0,
1196            ));
1197        }
1198
1199        let tool = ReflexEvaluateTool::new(bus, reflex);
1200        let mut ctx = Context::default();
1201        let result = tool.call(&mut ctx, json!({})).await.unwrap();
1202        let triggered = result["commands_triggered"].as_u64().unwrap();
1203        assert!(triggered > 0);
1204        assert_eq!(result["commands_executed"], 0);
1205        assert_eq!(result["commands_blocked"].as_u64().unwrap(), triggered);
1206    }
1207
1208    #[tokio::test]
1209    async fn reflex_evaluate_executes_with_actuator_bit() {
1210        let bus = make_actuator_bus();
1211        let reflex = make_reflex();
1212        {
1213            let mut r = reflex.lock().unwrap();
1214            r.add_rule(ReflexRule::above(
1215                "temp0",
1216                "fan0",
1217                ActuatorKind::Motor,
1218                50.0,
1219                1.0,
1220                0.0,
1221            ));
1222        }
1223
1224        let tool = ReflexEvaluateTool::new(bus, reflex)
1225            .with_safety(safety_table(SafetyBit::ActuatorControl.mask()));
1226        let mut ctx = Context::default();
1227        let result = tool.call(&mut ctx, json!({})).await.unwrap();
1228        assert!(result["commands_triggered"].as_u64().unwrap() > 0);
1229        assert_eq!(result["commands_executed"], 1);
1230        assert_eq!(result["commands_blocked"], 0);
1231    }
1232
1233    #[tokio::test]
1234    async fn parse_actuator_kind_all_variants() {
1235        assert_eq!(parse_actuator_kind("motor"), ActuatorKind::Motor);
1236        assert_eq!(parse_actuator_kind("relay"), ActuatorKind::Relay);
1237        assert_eq!(parse_actuator_kind("display"), ActuatorKind::Display);
1238        assert_eq!(parse_actuator_kind("speaker"), ActuatorKind::Speaker);
1239        assert_eq!(parse_actuator_kind("valve"), ActuatorKind::Valve);
1240        assert_eq!(parse_actuator_kind("thermal"), ActuatorKind::Thermal);
1241        assert_eq!(parse_actuator_kind("unknown"), ActuatorKind::Custom);
1242    }
1243
1244    #[tokio::test]
1245    async fn register_sensorimotor_returns_registry() {
1246        let registry = wm_dispatch::ToolRegistry::new();
1247        let bus = make_bus();
1248        let reflex = make_reflex();
1249        let registered = register_sensorimotor(&registry, bus, reflex, None, None);
1250        assert!(registered.len() >= 10);
1251    }
1252
1253    #[tokio::test]
1254    async fn register_sensorimotor_with_safety_table_gates_actuation() {
1255        let registry = wm_dispatch::ToolRegistry::new();
1256        let bus = make_actuator_bus();
1257        let reflex = make_reflex();
1258        let table = safety_table(SAFETY_DEFAULT);
1259        let registered = register_sensorimotor(&registry, bus, reflex, None, Some(&table));
1260
1261        let tool = registered.get("actuator.command").expect("registered");
1262        let mut ctx = Context::default();
1263        let result = tool
1264            .call(&mut ctx, json!({"actuator_id": "fan0", "value": 1.0}))
1265            .await
1266            .unwrap();
1267        assert_eq!(result["blocked_by"], "safety-mask");
1268    }
1269}
wm_tools/expansion/sensorimotor_tools.rs

wm_tools/expansion/
sensorimotor_tools.rs
