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

wm_tools/expansion/
sensorimotor_tools.rs
