sesh_runtime/
lib.rs

1// ============================================================================
2// SESH RUNTIME ENGINE & HARDWARE-IN-THE-LOOP SIMULATOR (1.0.0-PROD)
3// ============================================================================
4// Complete execution engine supporting full control flow (if/while/for/loop/match),
5// lexical scope stack, deterministic real-time scheduler, and direct integration
6// with real mathematical algorithms from `sesh-stdlib`.
7// ============================================================================
8
9use colored::*;
10use serde::Serialize;
11use sesh_ast::*;
12use sesh_stdlib::{
13    diff_drive_ik, ekf_update, feedforward_pid, fuse_imu_gps, haversine_distance, ik_3dof_arm,
14    lqr_compute, pure_pursuit,
15};
16use std::collections::{HashMap, VecDeque};
17use std::time::Instant;
18
19/// Virtual GPIO Pin State in Simulator
20#[derive(Debug, Clone, Serialize)]
21pub struct VirtualPin {
22    pub pin_num: u32,
23    pub mode: PinMode,
24    pub level: bool,   // true = HIGH, false = LOW
25    pub pwm_duty: u32, // 0..100 %
26}
27
28impl Default for VirtualPin {
29    fn default() -> Self {
30        Self {
31            pin_num: 0,
32            mode: PinMode::Out,
33            level: false,
34            pwm_duty: 0,
35        }
36    }
37}
38
39/// Simulated Robot Kinematic Position & Odometry
40#[derive(Debug, Clone, Default, Serialize)]
41pub struct RobotOdometry {
42    pub x: f64,
43    pub y: f64,
44    pub theta: f64, // radians
45    pub left_wheel_speed: i32,
46    pub right_wheel_speed: i32,
47    pub obstacle_distance_cm: f64,
48}
49
50/// Virtual ROS2 Topic Message
51#[derive(Debug, Clone, Serialize)]
52pub struct RosMessage {
53    pub topic: String,
54    pub payload: String,
55    pub timestamp_ms: u64,
56}
57
58#[derive(Debug, Clone, PartialEq)]
59pub enum ControlFlow {
60    None,
61    Return(Value),
62    Break,
63    Continue,
64}
65
66#[derive(Debug, Clone, PartialEq)]
67pub enum Value {
68    Int(i64),
69    Float(f64),
70    Bool(bool),
71    String(String),
72    Tuple(Vec<Value>),
73    Array(Vec<Value>),
74    Struct(HashMap<String, Value>),
75    Void,
76}
77
78/// The Sesh Virtual Robotics Hardware Simulator and Runtime Engine
79pub struct SeshRuntime {
80    pub gpio_bank: HashMap<u32, VirtualPin>,
81    pub odometry: RobotOdometry,
82    pub ros_topics: HashMap<String, VecDeque<RosMessage>>,
83    pub scope_stack: Vec<HashMap<String, Value>>,
84    pub robot_pin_map: HashMap<String, u32>,
85    pub functions: HashMap<String, FnDecl>,
86    pub pid_controllers: HashMap<String, sesh_stdlib::PidState>,
87    pub state_machines: HashMap<String, String>, // sm_name -> current_state
88    pub callsite_counters: HashMap<String, usize>,
89    pub current_task_name: String,
90    pub control_flow: ControlFlow,
91    pub start_time: Instant,
92    pub logs: Vec<String>,
93    pub watchdog_ok: bool,
94}
95
96impl Default for SeshRuntime {
97    fn default() -> Self {
98        Self::new()
99    }
100}
101
102impl SeshRuntime {
103    pub fn new() -> Self {
104        let mut runtime = Self {
105            gpio_bank: HashMap::new(),
106            odometry: RobotOdometry {
107                x: 0.0,
108                y: 0.0,
109                theta: 0.0,
110                left_wheel_speed: 0,
111                right_wheel_speed: 0,
112                obstacle_distance_cm: 45.0,
113            },
114            ros_topics: HashMap::new(),
115            scope_stack: vec![HashMap::new()], // Global scope at index 0
116            robot_pin_map: HashMap::new(),
117            functions: HashMap::new(),
118            pid_controllers: HashMap::new(),
119            state_machines: HashMap::new(),
120            callsite_counters: HashMap::new(),
121            current_task_name: "default".to_string(),
122            control_flow: ControlFlow::None,
123            start_time: Instant::now(),
124            logs: Vec::new(),
125            watchdog_ok: true,
126        };
127
128        // Initialize 40 virtual GPIO pins
129        for i in 0..40 {
130            runtime.gpio_bank.insert(
131                i,
132                VirtualPin {
133                    pin_num: i,
134                    mode: PinMode::Out,
135                    level: false,
136                    pwm_duty: 0,
137                },
138            );
139        }
140        runtime
141    }
142
143    pub fn log_event(&mut self, msg: String) {
144        let elapsed = self.start_time.elapsed().as_millis();
145        self.logs.push(format!("[{:06}ms] {}", elapsed, msg));
146    }
147
148    // ========================================================================
149    // LEXICAL SCOPING MANAGER (Stack of HashMaps)
150    // ========================================================================
151
152    pub fn push_scope(&mut self) {
153        self.scope_stack.push(HashMap::new());
154    }
155
156    pub fn pop_scope(&mut self) {
157        if self.scope_stack.len() > 1 {
158            self.scope_stack.pop();
159        }
160    }
161
162    pub fn define_var(&mut self, name: &str, val: Value) {
163        if let Some(scope) = self.scope_stack.last_mut() {
164            scope.insert(name.to_string(), val);
165        }
166    }
167
168    pub fn assign_var(&mut self, name: &str, val: Value) -> bool {
169        for scope in self.scope_stack.iter_mut().rev() {
170            if scope.contains_key(name) {
171                scope.insert(name.to_string(), val);
172                return true;
173            }
174        }
175        // Fallback: insert in current scope if not found
176        self.define_var(name, val);
177        false
178    }
179
180    pub fn lookup_var(&self, name: &str) -> Value {
181        for scope in self.scope_stack.iter().rev() {
182            if let Some(v) = scope.get(name) {
183                return v.clone();
184            }
185        }
186        Value::Int(0)
187    }
188
189    // ========================================================================
190    // HARDWARE HAL FUNCTIONS
191    // ========================================================================
192
193    pub fn gpio_set_mode(&mut self, pin: u32, mode: PinMode) {
194        if let Some(p) = self.gpio_bank.get_mut(&pin) {
195            p.mode = mode;
196        }
197        self.log_event(format!("GPIO {} set mode {:?}", pin, mode));
198    }
199
200    pub fn gpio_write(&mut self, pin: u32, level: bool) {
201        if let Some(p) = self.gpio_bank.get_mut(&pin) {
202            p.level = level;
203        }
204        let state = if level { "HIGH" } else { "LOW" };
205        self.log_event(format!("GPIO {} write {}", pin, state));
206    }
207
208    pub fn gpio_read(&self, pin: u32) -> bool {
209        self.gpio_bank.get(&pin).map(|p| p.level).unwrap_or(false)
210    }
211
212    pub fn pwm_write(&mut self, pin: u32, duty: u32) {
213        let clamped = duty.min(100);
214        if let Some(p) = self.gpio_bank.get_mut(&pin) {
215            p.mode = PinMode::Pwm;
216            p.pwm_duty = clamped;
217        }
218        self.log_event(format!("PWM pin {} set duty cycle {}%", pin, clamped));
219
220        if pin == 0 {
221            self.odometry.left_wheel_speed = clamped as i32;
222        } else if pin == 1 {
223            self.odometry.right_wheel_speed = clamped as i32;
224        }
225        self.update_kinematics();
226    }
227
228    pub fn ultrasonic_read(&mut self, _trig_pin: u32, _echo_pin: u32) -> f64 {
229        let dist = self.odometry.obstacle_distance_cm;
230        self.log_event(format!("Ultrasonic sensor read distance: {:.2} cm", dist));
231        dist
232    }
233
234    pub fn ros_publish(&mut self, topic: &str, payload: &str) {
235        let elapsed = self.start_time.elapsed().as_millis() as u64;
236        let msg = RosMessage {
237            topic: topic.to_string(),
238            payload: payload.to_string(),
239            timestamp_ms: elapsed,
240        };
241        self.ros_topics
242            .entry(topic.to_string())
243            .or_default()
244            .push_back(msg.clone());
245        self.log_event(format!("ROS Publish [{}] -> {}", topic, payload));
246    }
247
248    pub fn update_kinematics(&mut self) {
249        let v_left = self.odometry.left_wheel_speed as f64 * 0.05;
250        let v_right = self.odometry.right_wheel_speed as f64 * 0.05;
251        let v = (v_left + v_right) / 2.0;
252        self.odometry.x += v * self.odometry.theta.cos();
253        self.odometry.y += v * self.odometry.theta.sin();
254    }
255
256    // ========================================================================
257    // DETERMINISTIC REAL-TIME SCHEDULER SIMULATION LOOP
258    // ========================================================================
259
260    /// Run a multi-cycle real-time scheduler simulation over all tasks in priority order
261    pub fn run_scheduler_simulation(
262        &mut self,
263        program: &Program,
264        total_cycles: usize,
265    ) -> Result<(), String> {
266        self.execute_program(program)?; // run initialization pass
267        self.log_event(format!("Starting {} real-time control cycles...", total_cycles));
268
269        for cycle in 0..total_cycles {
270            for item in &program.items {
271                if let Item::Robot(robot) = item {
272                    for task in &robot.tasks {
273                        self.log_event(format!(
274                            "[Cycle {}] Running Task '{}::{}'",
275                            cycle, robot.name, task.name
276                        ));
277                        self.execute_block(&task.body)?;
278                    }
279                } else if let Item::Task(task) = item {
280                    self.log_event(format!("[Cycle {}] Running Task '{}'", cycle, task.name));
281                    self.execute_block(&task.body)?;
282                }
283            }
284        }
285        Ok(())
286    }
287
288    // ========================================================================
289    // ASCII ROBOTICS SIMULATOR DASHBOARD RENDERER
290    // ========================================================================
291
292    pub fn render_sim_dashboard(&self) -> String {
293        let mut out = String::new();
294        out.push_str(&format!(
295            "{}\n",
296            "========================================================================"
297                .bright_blue()
298                .bold()
299        ));
300        out.push_str(&format!(
301            "{}  --  {}\n",
302            "SESH ROBOTICS VIRTUAL HARDWARE SIMULATOR"
303                .bright_green()
304                .bold(),
305            "1.0.0-PROD".yellow()
306        ));
307        out.push_str(&format!(
308            "{}\n\n",
309            "========================================================================"
310                .bright_blue()
311                .bold()
312        ));
313
314        out.push_str(&format!("{}\n", "[1] ROBOT ODOMETRY & MOTOR STATE".bold()));
315        out.push_str(&format!(
316            "    Position:   X = {:6.2} m   |   Y = {:6.2} m   |   Theta = {:5.2} rad\n",
317            self.odometry.x, self.odometry.y, self.odometry.theta
318        ));
319        out.push_str(&format!(
320            "    Motors:     Left PWM = {:3}%  |   Right PWM = {:3}%\n",
321            self.odometry.left_wheel_speed, self.odometry.right_wheel_speed
322        ));
323        out.push_str(&format!(
324            "    Sensors:    Ultrasonic Distance = {:.1} cm | Watchdog = {}\n\n",
325            self.odometry.obstacle_distance_cm,
326            if self.watchdog_ok {
327                "OK".green()
328            } else {
329                "PANIC".red()
330            }
331        ));
332
333        out.push_str(&format!("{}\n", "[2] ACTIVE GPIO PINS & PWM CHANNELS".bold()));
334        for pin_id in [0, 1, 18, 21, 22, 23, 24] {
335            if let Some(pin) = self.gpio_bank.get(&pin_id) {
336                let state_str = if pin.mode == PinMode::Pwm {
337                    format!("PWM ({}%)", pin.pwm_duty).cyan()
338                } else if pin.level {
339                    "HIGH".green()
340                } else {
341                    "LOW".red()
342                };
343                out.push_str(&format!(
344                    "    GPIO {:2} : Mode = {:6?} | State = {}\n",
345                    pin_id, pin.mode, state_str
346                ));
347            }
348        }
349        out.push('\n');
350
351        out.push_str(&format!("{}\n", "[3] ROS 2 TOPICS BUS TELEMETRY".bold()));
352        if self.ros_topics.is_empty() {
353            out.push_str("    No active ROS messages published yet.\n");
354        } else {
355            for (topic, msgs) in &self.ros_topics {
356                if let Some(latest) = msgs.back() {
357                    out.push_str(&format!(
358                        "    /{} [{}ms]: \"{}\"\n",
359                        topic.bright_cyan(),
360                        latest.timestamp_ms,
361                        latest.payload
362                    ));
363                }
364            }
365        }
366        out.push('\n');
367
368        out.push_str(&format!("{}\n", "[4] RECENT REAL-TIME HARDWARE LOGS".bold()));
369        let start_idx = self.logs.len().saturating_sub(6);
370        for log in &self.logs[start_idx..] {
371            out.push_str(&format!("    {}\n", log));
372        }
373
374        out.push_str(&format!(
375            "\n{}\n",
376            "========================================================================"
377                .bright_blue()
378                .bold()
379        ));
380        out
381    }
382
383    // ========================================================================
384    // AST EXECUTION ENGINE WITH FULL CONTROL FLOW
385    // ========================================================================
386
387    pub fn register_program(&mut self, program: &Program) {
388        for item in &program.items {
389            if let Item::Function(f) = item {
390                self.functions.insert(f.name.clone(), f.clone());
391            } else if let Item::Robot(robot) = item {
392                for method in &robot.methods {
393                    self.functions.insert(method.name.clone(), method.clone());
394                    self.functions.insert(format!("{}_{}", robot.name, method.name), method.clone());
395                    self.functions.insert(format!("self.{}", method.name), method.clone());
396                }
397            }
398        }
399    }
400
401    pub fn execute_program(&mut self, program: &Program) -> Result<Value, String> {
402        self.register_program(program);
403        for item in &program.items {
404            match item {
405                Item::Pin(pin) => {
406                    self.gpio_set_mode(pin.pin_num, pin.mode);
407                    self.robot_pin_map.insert(pin.name.clone(), pin.pin_num);
408                }
409                Item::Robot(robot) => {
410                    self.log_event(format!("Robot '{}' initialized in simulation", robot.name));
411                    for pin in &robot.pins {
412                        self.gpio_set_mode(pin.pin_num, pin.mode);
413                        self.robot_pin_map
414                            .insert(pin.name.clone(), pin.pin_num);
415                        self.robot_pin_map
416                            .insert(format!("self.{}", pin.name), pin.pin_num);
417                    }
418                    // Execute init method if present
419                    for method in &robot.methods {
420                        if method.name == "init" {
421                            self.execute_block(&method.body)?;
422                        }
423                    }
424                    // Execute tasks in initial pass
425                    for task in &robot.tasks {
426                        self.current_task_name = task.name.clone();
427                        self.log_event(format!("Executing real-time task '{}'", task.name));
428                        self.execute_block(&task.body)?;
429                    }
430                }
431                Item::Function(func) => {
432                    if func.name == "main" || func.name == "init" {
433                        self.execute_block(&func.body)?;
434                    }
435                }
436                Item::Task(task) => {
437                    self.current_task_name = task.name.clone();
438                    self.log_event(format!("Executing real-time task '{}'", task.name));
439                    self.execute_block(&task.body)?;
440                }
441                _ => {}
442            }
443        }
444        Ok(Value::Void)
445    }
446
447    /// Run a multi-cycle real-time simulation over all tasks in priority order
448    pub fn run_simulation(
449        &mut self,
450        program: &Program,
451        cycles: usize,
452    ) -> Result<Value, String> {
453        self.register_program(program);
454        // Initialization pass (pins, init methods)
455        for item in &program.items {
456            if let Item::Pin(pin) = item {
457                self.gpio_set_mode(pin.pin_num, pin.mode);
458                self.robot_pin_map.insert(pin.name.clone(), pin.pin_num);
459            } else if let Item::Robot(robot) = item {
460                self.log_event(format!("Robot '{}' initialized in simulation", robot.name));
461                for pin in &robot.pins {
462                    self.gpio_set_mode(pin.pin_num, pin.mode);
463                    self.robot_pin_map
464                        .insert(pin.name.clone(), pin.pin_num);
465                    self.robot_pin_map
466                        .insert(format!("self.{}", pin.name), pin.pin_num);
467                }
468                for method in &robot.methods {
469                    if method.name == "init" {
470                        self.execute_block(&method.body)?;
471                    }
472                }
473                for sm in &robot.state_machines {
474                    self.state_machines.insert(sm.name.clone(), sm.initial_state.clone());
475                    self.log_event(format!("[StateMachine: {}] Initialized in state '{}'", sm.name, sm.initial_state));
476                }
477            } else if let Item::Function(func) = item {
478                if func.name == "init" {
479                    self.execute_block(&func.body)?;
480                }
481            } else if let Item::StateMachine(sm) = item {
482                self.state_machines.insert(sm.name.clone(), sm.initial_state.clone());
483                self.log_event(format!("[StateMachine: {}] Initialized in state '{}'", sm.name, sm.initial_state));
484            }
485        }
486
487        // Run deterministic real-time control loop for N consecutive cycles!
488        for cycle in 1..=cycles {
489            self.callsite_counters.clear();
490            self.log_event(format!("--- Control Cycle {} ---", cycle));
491
492            // Step 2a: Evaluate State Machine transitions!
493            for item in &program.items {
494                let sm_list: Vec<&StateMachineDecl> = match item {
495                    Item::Robot(r) => r.state_machines.iter().collect(),
496                    Item::StateMachine(sm) => vec![sm],
497                    _ => vec![],
498                };
499                for sm in sm_list {
500                    let curr = self.state_machines.get(&sm.name).cloned().unwrap_or_else(|| sm.initial_state.clone());
501                    for tr in &sm.transitions {
502                        if tr.from_state == curr {
503                            // Evaluate condition expression or flag
504                            let cond_ok = if tr.condition == "true" {
505                                true
506                            } else {
507                                self.is_truthy(&self.lookup_var(&tr.condition))
508                            };
509                            if cond_ok {
510                                self.state_machines.insert(sm.name.clone(), tr.to_state.clone());
511                                self.log_event(format!(
512                                    "[StateMachine: {}] Transitioned {} -> {}",
513                                    sm.name, tr.from_state, tr.to_state
514                                ));
515                                // Execute on_enter block of new state
516                                if let Some(state_blk) = sm.states.iter().find(|s| s.name == tr.to_state) {
517                                    self.execute_block(&state_blk.on_enter)?;
518                                }
519                                break;
520                            }
521                        }
522                    }
523                }
524            }
525
526            // Step 2b: Execute Tasks!
527            for item in &program.items {
528                if let Item::Robot(robot) = item {
529                    for task in &robot.tasks {
530                        self.current_task_name = task.name.clone();
531                        self.execute_block(&task.body)?;
532                    }
533                } else if let Item::Task(task) = item {
534                    self.current_task_name = task.name.clone();
535                    self.execute_block(&task.body)?;
536                }
537            }
538        }
539
540        // Execute main function at the end
541        for item in &program.items {
542            if let Item::Function(func) = item {
543                if func.name == "main" {
544                    self.execute_block(&func.body)?;
545                }
546            }
547        }
548
549        Ok(Value::Void)
550    }
551
552    pub fn execute_block(&mut self, block: &Block) -> Result<Value, String> {
553        self.push_scope();
554        let mut last_val = Value::Void;
555        for stmt in &block.statements {
556            last_val = self.execute_stmt(stmt)?;
557            if self.control_flow != ControlFlow::None {
558                break;
559            }
560        }
561        self.pop_scope();
562        Ok(last_val)
563    }
564
565    pub fn execute_stmt(&mut self, stmt: &Stmt) -> Result<Value, String> {
566        match stmt {
567            Stmt::Let { name, init, .. } => {
568                let val = self.eval_expr(init)?;
569                self.define_var(name, val);
570                Ok(Value::Void)
571            }
572            Stmt::Assign { target, value, .. } => {
573                let val = self.eval_expr(value)?;
574                match target {
575                    Expr::Variable(name, _) => {
576                        self.assign_var(name, val);
577                    }
578                    Expr::FieldAccess(receiver, field, _) => {
579                        if let Expr::Variable(var_name, _) = receiver.as_ref() {
580                            let current = self.lookup_var(var_name);
581                            if let Value::Struct(mut map) = current {
582                                map.insert(field.clone(), val.clone());
583                                self.assign_var(var_name, Value::Struct(map));
584                            }
585                        }
586                    }
587                    Expr::Index(array, idx, _) => {
588                        if let Expr::Variable(var_name, _) = array.as_ref() {
589                            let idx_expr = self.eval_expr(idx)?;
590                            let idx_val = self.as_int(&idx_expr) as usize;
591                            let current = self.lookup_var(var_name);
592                            if let Value::Array(mut vec) = current {
593                                if idx_val < vec.len() {
594                                    vec[idx_val] = val.clone();
595                                    self.assign_var(var_name, Value::Array(vec));
596                                }
597                            }
598                        }
599                    }
600                    _ => {}
601                }
602                Ok(Value::Void)
603            }
604            Stmt::If {
605                cond,
606                then_block,
607                else_block,
608                ..
609            } => {
610                let c = self.eval_expr(cond)?;
611                if self.is_truthy(&c) {
612                    self.execute_block(then_block)
613                } else if let Some(e) = else_block {
614                    self.execute_block(e)
615                } else {
616                    Ok(Value::Void)
617                }
618            }
619            Stmt::While { cond, body, .. } => {
620                let mut iter = 0;
621                let mut cond_val = self.eval_expr(cond)?;
622                while self.is_truthy(&cond_val) && iter < 1000 {
623                    self.execute_block(body)?;
624                    match self.control_flow {
625                        ControlFlow::Break => {
626                            self.control_flow = ControlFlow::None;
627                            break;
628                        }
629                        ControlFlow::Continue => {
630                            self.control_flow = ControlFlow::None;
631                            cond_val = self.eval_expr(cond)?;
632                            iter += 1;
633                            continue;
634                        }
635                        ControlFlow::Return(_) => break,
636                        ControlFlow::None => {}
637                    }
638                    cond_val = self.eval_expr(cond)?;
639                    iter += 1;
640                }
641                Ok(Value::Void)
642            }
643            Stmt::For {
644                var,
645                start,
646                end,
647                body,
648                ..
649            } => {
650                let s_val = self.eval_expr(start)?;
651                let start_val = self.as_int(&s_val);
652                let e_val = self.eval_expr(end)?;
653                let end_val = self.as_int(&e_val);
654                for idx in start_val..end_val {
655                    self.push_scope();
656                    self.define_var(var, Value::Int(idx));
657                    for s in &body.statements {
658                        self.execute_stmt(s)?;
659                        if self.control_flow != ControlFlow::None {
660                            break;
661                        }
662                    }
663                    self.pop_scope();
664                    match self.control_flow {
665                        ControlFlow::Break => {
666                            self.control_flow = ControlFlow::None;
667                            break;
668                        }
669                        ControlFlow::Continue => {
670                            self.control_flow = ControlFlow::None;
671                            continue;
672                        }
673                        ControlFlow::Return(_) => break,
674                        ControlFlow::None => {}
675                    }
676                }
677                Ok(Value::Void)
678            }
679            Stmt::Loop { body, .. } => {
680                for _ in 0..10 {
681                    self.execute_block(body)?;
682                    match self.control_flow {
683                        ControlFlow::Break => {
684                            self.control_flow = ControlFlow::None;
685                            break;
686                        }
687                        ControlFlow::Continue => {
688                            self.control_flow = ControlFlow::None;
689                            continue;
690                        }
691                        ControlFlow::Return(_) => break,
692                        ControlFlow::None => {}
693                    }
694                }
695                Ok(Value::Void)
696            }
697            Stmt::Match { target, arms, .. } => {
698                let target_val = self.eval_expr(target)?;
699                for arm in arms {
700                    let matched = match &arm.pattern {
701                        Pattern::Wildcard => true,
702                        Pattern::Literal(lit) => {
703                            let arm_val = self.eval_literal(lit);
704                            arm_val == target_val
705                        }
706                        Pattern::EnumVariant { variant, .. } => {
707                            self.val_to_string(&target_val) == *variant
708                        }
709                    };
710                    if matched {
711                        return self.execute_block(&arm.body);
712                    }
713                }
714                Ok(Value::Void)
715            }
716            Stmt::Break(_) => {
717                self.control_flow = ControlFlow::Break;
718                Ok(Value::Void)
719            }
720            Stmt::Continue(_) => {
721                self.control_flow = ControlFlow::Continue;
722                Ok(Value::Void)
723            }
724            Stmt::Return { value, .. } => {
725                let ret_val = if let Some(v) = value {
726                    self.eval_expr(v)?
727                } else {
728                    Value::Void
729                };
730                self.control_flow = ControlFlow::Return(ret_val.clone());
731                Ok(ret_val)
732            }
733            Stmt::Emit { topic, payload, .. } => {
734                let p_val = self.eval_expr(payload)?;
735                let p_str = self.val_to_string(&p_val);
736                self.ros_publish(topic, &p_str);
737                Ok(Value::Void)
738            }
739            Stmt::Expr(expr) => self.eval_expr(expr),
740        }
741    }
742
743    pub fn eval_expr(&mut self, expr: &Expr) -> Result<Value, String> {
744        match expr {
745            Expr::Literal(lit, _) => Ok(self.eval_literal(lit)),
746            Expr::Variable(name, _) => {
747                if let Some(pin_num) = self.robot_pin_map.get(name) {
748                    Ok(Value::Int(*pin_num as i64))
749                } else {
750                    Ok(self.lookup_var(name))
751                }
752            }
753            Expr::FieldAccess(receiver, field, _) => {
754                let recv_val = self.eval_expr(receiver)?;
755                if let Value::Struct(map) = &recv_val {
756                    return Ok(map.get(field).cloned().unwrap_or(Value::Void));
757                }
758                if let Some(pin_num) = self.robot_pin_map.get(field) {
759                    Ok(Value::Int(*pin_num as i64))
760                } else {
761                    let key = format!("{}.{}", self.val_to_string(&recv_val), field);
762                    if let Some(pin_num) = self.robot_pin_map.get(&key) {
763                        Ok(Value::Int(*pin_num as i64))
764                    } else {
765                        Ok(Value::Int(0))
766                    }
767                }
768            }
769            Expr::Binary {
770                op, left, right, ..
771            } => {
772                if *op == BinaryOp::And {
773                    let l = self.eval_expr(left)?;
774                    if !self.is_truthy(&l) {
775                        return Ok(Value::Bool(false));
776                    }
777                    let r = self.eval_expr(right)?;
778                    return Ok(Value::Bool(self.is_truthy(&r)));
779                } else if *op == BinaryOp::Or {
780                    let l = self.eval_expr(left)?;
781                    if self.is_truthy(&l) {
782                        return Ok(Value::Bool(true));
783                    }
784                    let r = self.eval_expr(right)?;
785                    return Ok(Value::Bool(self.is_truthy(&r)));
786                }
787                let l = self.eval_expr(left)?;
788                let r = self.eval_expr(right)?;
789                self.eval_binary(*op, l, r)
790            }
791            Expr::Call { func, args, .. } => {
792                let mut arg_vals = Vec::new();
793                for a in args {
794                    arg_vals.push(self.eval_expr(a)?);
795                }
796                // Check if user-defined function or method!
797                if let Some(fn_decl) = self.functions.get(func).cloned() {
798                    self.push_scope();
799                    for (i, arg_val) in arg_vals.iter().enumerate() {
800                        if i < fn_decl.params.len() {
801                            self.define_var(&fn_decl.params[i].name, arg_val.clone());
802                        }
803                    }
804                    let res = self.execute_block(&fn_decl.body)?;
805                    self.pop_scope();
806                    if let ControlFlow::Return(ret) = &self.control_flow {
807                        let result = ret.clone();
808                        self.control_flow = ControlFlow::None;
809                        return Ok(result);
810                    }
811                    return Ok(res);
812                }
813                self.eval_stdlib_call(func, &arg_vals)
814            }
815            Expr::MethodCall { method, args, .. } => {
816                let mut arg_vals = Vec::new();
817                for a in args {
818                    arg_vals.push(self.eval_expr(a)?);
819                }
820                if let Some(fn_decl) = self
821                    .functions
822                    .get(method)
823                    .cloned()
824                    .or_else(|| self.functions.get(&format!("self.{}", method)).cloned())
825                {
826                    self.push_scope();
827                    for (i, arg_val) in arg_vals.iter().enumerate() {
828                        if i < fn_decl.params.len() {
829                            self.define_var(&fn_decl.params[i].name, arg_val.clone());
830                        }
831                    }
832                    let res = self.execute_block(&fn_decl.body)?;
833                    self.pop_scope();
834                    if let ControlFlow::Return(ret) = &self.control_flow {
835                        let result = ret.clone();
836                        self.control_flow = ControlFlow::None;
837                        return Ok(result);
838                    }
839                    return Ok(res);
840                }
841                Ok(Value::Void)
842            }
843            Expr::ArrayInit(elems, _) => {
844                let mut vals = Vec::new();
845                for e in elems {
846                    vals.push(self.eval_expr(e)?);
847                }
848                Ok(Value::Array(vals))
849            }
850            Expr::Index(array, idx, _) => {
851                let arr_val = self.eval_expr(array)?;
852                let idx_expr = self.eval_expr(idx)?;
853                let idx_val = self.as_int(&idx_expr) as usize;
854                if let Value::Array(vals) = arr_val {
855                    if idx_val < vals.len() {
856                        Ok(vals[idx_val].clone())
857                    } else {
858                        Ok(Value::Void)
859                    }
860                } else {
861                    Ok(Value::Void)
862                }
863            }
864            Expr::Unary { op, operand, .. } => {
865                let val = self.eval_expr(operand)?;
866                match op {
867                    UnaryOp::Neg => {
868                        match val {
869                            Value::Int(i) => Ok(Value::Int(-i)),
870                            Value::Float(f) => Ok(Value::Float(-f)),
871                            _ => Ok(Value::Void),
872                        }
873                    }
874                    UnaryOp::Not => {
875                        let b = self.is_truthy(&val);
876                        Ok(Value::Bool(!b))
877                    }
878                    UnaryOp::BitNot => {
879                        let i = self.as_int(&val);
880                        Ok(Value::Int(!i))
881                    }
882                }
883            }
884            Expr::StructInit { fields, .. } => {
885                let mut map = HashMap::new();
886                for (fname, fexpr) in fields {
887                    let v = self.eval_expr(fexpr)?;
888                    map.insert(fname.clone(), v);
889                }
890                Ok(Value::Struct(map))
891            }
892        }
893    }
894
895    fn eval_literal(&self, lit: &Literal) -> Value {
896        match lit {
897            Literal::Int(v) => Value::Int(*v),
898            Literal::Float(v) => Value::Float(*v),
899            Literal::Bool(v) => Value::Bool(*v),
900            Literal::String(s) => Value::String(s.clone()),
901            Literal::DurationMs(ms) => Value::Int(*ms as i64),
902            Literal::HardwareConstant(s) => {
903                let val = match s.as_str() {
904                    "HIGH" | "OUT" | "RISING" => 1,
905                    _ => 0,
906                };
907                Value::Int(val)
908            }
909        }
910    }
911
912    // ========================================================================
913    // DIRECT DISPATCH TO REAL RUST COMPUTATIONAL ENGINE IN SESH-STDLIB
914    // ========================================================================
915
916    fn eval_stdlib_call(&mut self, func: &str, args: &[Value]) -> Result<Value, String> {
917        match func {
918            "std::gpio::write" => {
919                if args.len() >= 2 {
920                    let pin = self.as_int(&args[0]) as u32;
921                    let level = self.as_int(&args[1]) > 0;
922                    self.gpio_write(pin, level);
923                }
924                Ok(Value::Void)
925            }
926            "std::pwm::write" => {
927                if args.len() >= 2 {
928                    let pin = self.as_int(&args[0]) as u32;
929                    let duty = self.as_int(&args[1]) as u32;
930                    self.pwm_write(pin, duty);
931                }
932                Ok(Value::Void)
933            }
934            "std::sensor::ultrasonic_read" => {
935                let trig = if !args.is_empty() {
936                    self.as_int(&args[0]) as u32
937                } else {
938                    23
939                };
940                let echo = if args.len() >= 2 {
941                    self.as_int(&args[1]) as u32
942                } else {
943                    24
944                };
945                let dist = self.ultrasonic_read(trig, echo);
946                Ok(Value::Float(dist))
947            }
948            "std::ros::publish" => {
949                if args.len() >= 2 {
950                    let topic = self.val_to_string(&args[0]);
951                    let payload = self.val_to_string(&args[1]);
952                    self.ros_publish(&topic, &payload);
953                }
954                Ok(Value::Void)
955            }
956            // --- ACTUAL MATHEMATICAL ALGORITHMS ---
957            "std::motor::pid_compute" => {
958                if args.len() >= 5 {
959                    let setpoint = self.as_float(&args[0]);
960                    let actual = self.as_float(&args[1]);
961                    let kp = self.as_float(&args[2]);
962                    let ki = self.as_float(&args[3]);
963                    let kd = self.as_float(&args[4]);
964                    let (res, prev_err) = {
965                        let count = {
966                            let c = self
967                                .callsite_counters
968                                .entry(self.current_task_name.clone())
969                                .or_insert(0);
970                            *c += 1;
971                            *c
972                        };
973                        let key = format!("{}_pid_{}", self.current_task_name, count);
974                        let pid = self
975                            .pid_controllers
976                            .entry(key)
977                            .or_insert_with(|| sesh_stdlib::PidState::new(kp, ki, kd));
978                        pid.kp = kp;
979                        pid.ki = ki;
980                        pid.kd = kd;
981                        let r = pid.compute(setpoint, actual, 0.005).clamp(-100.0, 100.0);
982                        (r, pid.prev_error)
983                    };
984                    self.log_event(format!(
985                        "PID compute(sp={:.1}, act={:.1}, d_err={:.2}) -> {:.2}",
986                        setpoint, actual, prev_err, res
987                    ));
988                    Ok(Value::Float(res))
989                } else {
990                    Ok(Value::Float(0.0))
991                }
992            }
993            "std::kinematics::ik_3dof_arm" => {
994                if args.len() >= 3 {
995                    let x = self.as_float(&args[0]);
996                    let y = self.as_float(&args[1]);
997                    let z = self.as_float(&args[2]);
998                    let (b, s, e) = ik_3dof_arm(x, y, z);
999                    self.log_event(format!(
1000                        "IK 3-DOF Arm solved(x={:.1}, y={:.1}, z={:.1}) -> ({:.1}°, {:.1}°, {:.1}°)",
1001                        x, y, z, b, s, e
1002                    ));
1003                    Ok(Value::Tuple(vec![
1004                        Value::Float(b),
1005                        Value::Float(s),
1006                        Value::Float(e),
1007                    ]))
1008                } else {
1009                    Ok(Value::Void)
1010                }
1011            }
1012            "std::kinematics::diff_drive_ik" => {
1013                if args.len() >= 2 {
1014                    let v = self.as_float(&args[0]);
1015                    let w = self.as_float(&args[1]);
1016                    let (left, right) = diff_drive_ik(v, w);
1017                    self.log_event(format!(
1018                        "Diff Drive IK(v={:.2}, w={:.2}) -> PWM ({}/{})",
1019                        v, w, left, right
1020                    ));
1021                    Ok(Value::Tuple(vec![
1022                        Value::Int(left as i64),
1023                        Value::Int(right as i64),
1024                    ]))
1025                } else {
1026                    Ok(Value::Void)
1027                }
1028            }
1029            "std::kalman::ekf_update" => {
1030                if args.len() >= 4 {
1031                    let x = self.as_float(&args[0]);
1032                    let z = self.as_float(&args[1]);
1033                    let p = self.as_float(&args[2]);
1034                    let r = self.as_float(&args[3]);
1035                    let (new_x, new_p) = ekf_update(x, z, p, r);
1036                    self.log_event(format!(
1037                        "EKF step(x={:.2}, z={:.2}) -> new_x={:.2}, cov={:.3}",
1038                        x, z, new_x, new_p
1039                    ));
1040                    Ok(Value::Tuple(vec![Value::Float(new_x), Value::Float(new_p)]))
1041                } else {
1042                    Ok(Value::Void)
1043                }
1044            }
1045            "std::kalman::fuse_imu_gps" => {
1046                if args.len() >= 3 {
1047                    let lat = self.as_float(&args[0]);
1048                    let lon = self.as_float(&args[1]);
1049                    let yaw = self.as_float(&args[2]);
1050                    let (lat_c, lon_c, yaw_c) = fuse_imu_gps(lat, lon, yaw);
1051                    self.log_event(format!(
1052                        "EKF fused GPS ({:.4}, {:.4}, yaw={:.1}°)",
1053                        lat_c, lon_c, yaw_c
1054                    ));
1055                    Ok(Value::Tuple(vec![
1056                        Value::Float(lat_c),
1057                        Value::Float(lon_c),
1058                        Value::Float(yaw_c),
1059                    ]))
1060                } else {
1061                    Ok(Value::Void)
1062                }
1063            }
1064            "std::path::pure_pursuit" => {
1065                if args.len() >= 6 {
1066                    let rx = self.as_float(&args[0]);
1067                    let ry = self.as_float(&args[1]);
1068                    let rtheta = self.as_float(&args[2]);
1069                    let wx = self.as_float(&args[3]);
1070                    let wy = self.as_float(&args[4]);
1071                    let v = self.as_float(&args[5]);
1072                    let (kappa, w) = pure_pursuit(rx, ry, rtheta, wx, wy, v);
1073                    self.log_event(format!(
1074                        "Pure Pursuit kappa={:.3}, w={:.2} rad/s",
1075                        kappa, w
1076                    ));
1077                    Ok(Value::Tuple(vec![Value::Float(kappa), Value::Float(w)]))
1078                } else {
1079                    Ok(Value::Void)
1080                }
1081            }
1082            "std::control::feedforward_pid" => {
1083                if args.len() >= 8 {
1084                    let sp = self.as_float(&args[0]);
1085                    let act = self.as_float(&args[1]);
1086                    let kp = self.as_float(&args[2]);
1087                    let ki = self.as_float(&args[3]);
1088                    let kd = self.as_float(&args[4]);
1089                    let kf = self.as_float(&args[5]);
1090                    let prev_sp = self.as_float(&args[6]);
1091                    let dt = self.as_float(&args[7]);
1092                    let out = feedforward_pid(sp, act, kp, ki, kd, kf, prev_sp, dt);
1093                    self.log_event(format!("Feedforward PID out = {:.2}", out));
1094                    Ok(Value::Float(out))
1095                } else {
1096                    Ok(Value::Float(0.0))
1097                }
1098            }
1099            "std::control::lqr_compute" => {
1100                if args.len() >= 4 {
1101                    let ex = self.as_float(&args[0]);
1102                    let ev = self.as_float(&args[1]);
1103                    let k1 = self.as_float(&args[2]);
1104                    let k2 = self.as_float(&args[3]);
1105                    let u = lqr_compute(ex, ev, k1, k2);
1106                    self.log_event(format!("LQR optimal feedback u = {:.2}", u));
1107                    Ok(Value::Float(u))
1108                } else {
1109                    Ok(Value::Float(0.0))
1110                }
1111            }
1112            "std::gps::distance_to_waypoint" => {
1113                if args.len() >= 4 {
1114                    let l1 = self.as_float(&args[0]);
1115                    let n1 = self.as_float(&args[1]);
1116                    let l2 = self.as_float(&args[2]);
1117                    let n2 = self.as_float(&args[3]);
1118                    let d = haversine_distance(l1, n1, l2, n2);
1119                    self.log_event(format!("GPS Haversine distance = {:.1} m", d));
1120                    Ok(Value::Float(d))
1121                } else {
1122                    Ok(Value::Float(0.0))
1123                }
1124            }
1125            "std::watchdog::start" => {
1126                self.watchdog_ok = true;
1127                self.log_event("Watchdog timer started (status: OK)".to_string());
1128                Ok(Value::Void)
1129            }
1130            "std::watchdog::reset" => {
1131                self.watchdog_ok = true;
1132                self.log_event("Watchdog timer kicked/refreshed".to_string());
1133                Ok(Value::Void)
1134            }
1135            "print" | "println" => {
1136                let text = args
1137                    .iter()
1138                    .map(|v| self.val_to_string(v))
1139                    .collect::<Vec<_>>()
1140                    .join(" ");
1141                println!("{}", text);
1142                self.log_event(format!("PRINT: {}", text));
1143                Ok(Value::Void)
1144            }
1145            _ => Ok(Value::Void),
1146        }
1147    }
1148
1149    fn eval_binary(&self, op: BinaryOp, l: Value, r: Value) -> Result<Value, String> {
1150        let l_float = self.as_float(&l);
1151        let r_float = self.as_float(&r);
1152
1153        match (l, r) {
1154            (Value::Int(a), Value::Int(b)) => match op {
1155                BinaryOp::Add => Ok(Value::Int(a + b)),
1156                BinaryOp::Sub => Ok(Value::Int(a - b)),
1157                BinaryOp::Mul => Ok(Value::Int(a * b)),
1158                BinaryOp::Div => {
1159                    if b != 0 {
1160                        Ok(Value::Int(a / b))
1161                    } else {
1162                        eprintln!("[RUNTIME ERROR: Integer division by zero]");
1163                        Ok(Value::Int(0))
1164                    }
1165                }
1166                BinaryOp::Mod => {
1167                    if b != 0 {
1168                        Ok(Value::Int(a % b))
1169                    } else {
1170                        eprintln!("[RUNTIME ERROR: Integer modulo by zero]");
1171                        Ok(Value::Int(0))
1172                    }
1173                }
1174                BinaryOp::Eq => Ok(Value::Bool(a == b)),
1175                BinaryOp::NotEq => Ok(Value::Bool(a != b)),
1176                BinaryOp::Lt => Ok(Value::Bool(a < b)),
1177                BinaryOp::LtEq => Ok(Value::Bool(a <= b)),
1178                BinaryOp::Gt => Ok(Value::Bool(a > b)),
1179                BinaryOp::GtEq => Ok(Value::Bool(a >= b)),
1180                BinaryOp::And => Ok(Value::Bool(a != 0 && b != 0)),
1181                BinaryOp::Or => Ok(Value::Bool(a != 0 || b != 0)),
1182                BinaryOp::BitAnd => Ok(Value::Int(a & b)),
1183                BinaryOp::BitOr => Ok(Value::Int(a | b)),
1184                BinaryOp::BitXor => Ok(Value::Int(a ^ b)),
1185                BinaryOp::Shl => Ok(Value::Int(a << (b as u32).min(63))),
1186                BinaryOp::Shr => Ok(Value::Int(a >> (b as u32).min(63))),
1187            },
1188            (Value::Float(_), _) | (_, Value::Float(_)) => match op {
1189                BinaryOp::Add => Ok(Value::Float(l_float + r_float)),
1190                BinaryOp::Sub => Ok(Value::Float(l_float - r_float)),
1191                BinaryOp::Mul => Ok(Value::Float(l_float * r_float)),
1192                BinaryOp::Div => Ok(Value::Float(if r_float != 0.0 { l_float / r_float } else { 0.0 })),
1193                BinaryOp::Eq => Ok(Value::Bool((l_float - r_float).abs() < 1e-9)),
1194                BinaryOp::NotEq => Ok(Value::Bool((l_float - r_float).abs() >= 1e-9)),
1195                BinaryOp::Lt => Ok(Value::Bool(l_float < r_float)),
1196                BinaryOp::LtEq => Ok(Value::Bool(l_float <= r_float)),
1197                BinaryOp::Gt => Ok(Value::Bool(l_float > r_float)),
1198                BinaryOp::GtEq => Ok(Value::Bool(l_float >= r_float)),
1199                _ => Ok(Value::Void),
1200            },
1201            (Value::String(a), right) => {
1202                if op == BinaryOp::Add {
1203                    Ok(Value::String(format!("{}{}", a, self.val_to_string(&right))))
1204                } else {
1205                    Ok(Value::Void)
1206                }
1207            }
1208            (left, Value::String(b)) => {
1209                if op == BinaryOp::Add {
1210                    Ok(Value::String(format!("{}{}", self.val_to_string(&left), b)))
1211                } else {
1212                    Ok(Value::Void)
1213                }
1214            }
1215            _ => Ok(Value::Void),
1216        }
1217    }
1218
1219    fn is_truthy(&self, v: &Value) -> bool {
1220        match v {
1221            Value::Bool(b) => *b,
1222            Value::Int(i) => *i != 0,
1223            Value::Float(f) => *f != 0.0,
1224            _ => false,
1225        }
1226    }
1227
1228    fn as_int(&self, v: &Value) -> i64 {
1229        match v {
1230            Value::Int(i) => *i,
1231            Value::Float(f) => *f as i64,
1232            Value::Bool(b) => {
1233                if *b {
1234                    1
1235                } else {
1236                    0
1237                }
1238            }
1239            _ => 0,
1240        }
1241    }
1242
1243    fn as_float(&self, v: &Value) -> f64 {
1244        match v {
1245            Value::Float(f) => *f,
1246            Value::Int(i) => *i as f64,
1247            Value::Bool(b) => {
1248                if *b {
1249                    1.0
1250                } else {
1251                    0.0
1252                }
1253            }
1254            _ => 0.0,
1255        }
1256    }
1257
1258    fn val_to_string(&self, v: &Value) -> String {
1259        match v {
1260            Value::Int(i) => i.to_string(),
1261            Value::Float(f) => format!("{:.2}", f),
1262            Value::Bool(b) => b.to_string(),
1263            Value::String(s) => s.clone(),
1264            Value::Tuple(vals) => {
1265                let items: Vec<String> = vals.iter().map(|item| self.val_to_string(item)).collect();
1266                format!("({})", items.join(", "))
1267            }
1268            Value::Array(vals) => {
1269                let items: Vec<String> = vals.iter().map(|item| self.val_to_string(item)).collect();
1270                format!("[{}]", items.join(", "))
1271            }
1272            Value::Struct(map) => {
1273                let items: Vec<String> = map
1274                    .iter()
1275                    .map(|(k, v)| format!("{}: {}", k, self.val_to_string(v)))
1276                    .collect();
1277                format!("{{ {} }}", items.join(", "))
1278            }
1279            Value::Void => "void".to_string(),
1280        }
1281    }
1282
1283    pub fn to_json_state(&self) -> String {
1284        let mut pins = Vec::new();
1285        for i in [0, 1, 2, 3, 18, 21, 22, 23, 24] {
1286            if let Some(pin) = self.gpio_bank.get(&i) {
1287                pins.push(pin.clone());
1288            }
1289        }
1290        let mut topics = Vec::new();
1291        for (_, msgs) in &self.ros_topics {
1292            if let Some(msg) = msgs.back() {
1293                topics.push(msg.clone());
1294            }
1295        }
1296        serde_json::json!({
1297            "odometry": self.odometry,
1298            "gpio_pins": pins,
1299            "ros_topics": topics,
1300            "logs": self.logs.iter().rev().take(15).cloned().collect::<Vec<_>>(),
1301            "watchdog_ok": self.watchdog_ok
1302        })
1303        .to_string()
1304    }
1305}
1306
1307/// Run the interactive HTML5/Canvas Hardware-In-the-Loop Web Simulator
1308pub fn run_web_simulator(program: &Program, port: u16) -> Result<(), Box<dyn std::error::Error>> {
1309    use std::io::{Read, Write};
1310    use std::net::TcpListener;
1311
1312    let listener = TcpListener::bind(format!("0.0.0.0:{}", port))?;
1313    println!(
1314        "{}\n",
1315        "========================================================================"
1316            .bright_cyan()
1317            .bold()
1318    );
1319    println!(
1320        "{}  --  {}\n",
1321        "SESH WEB HARDWARE-IN-THE-LOOP (HIL) SIMULATOR STARTED"
1322            .bright_green()
1323            .bold(),
1324        format!("http://0.0.0.0:{}", port).yellow().bold()
1325    );
1326    println!(
1327        "{} Open the Live Preview in your browser to interact with the Robot Dashboard!",
1328        "[INFO]".bright_blue().bold()
1329    );
1330    println!(
1331        "{}\n",
1332        "========================================================================"
1333            .bright_cyan()
1334            .bold()
1335    );
1336
1337    let mut runtime = SeshRuntime::new();
1338    let _ = runtime.execute_program(program);
1339
1340    for stream in listener.incoming() {
1341        if let Ok(mut stream) = stream {
1342            let mut buf = [0u8; 4096];
1343            let n = stream.read(&mut buf).unwrap_or(0);
1344            let req_str = String::from_utf8_lossy(&buf[..n]);
1345
1346            let response = if req_str.starts_with("GET /api/state") {
1347                let _ = runtime.execute_program(program);
1348                let json = runtime.to_json_state();
1349                format!(
1350                    "HTTP/1.1 200 OK\r\nContent-Type: application/json\r\nAccess-Control-Allow-Origin: *\r\nContent-Length: {}\r\n\r\n{}",
1351                    json.len(),
1352                    json
1353                )
1354            } else if req_str.starts_with("POST /api/cmd/obstacle") {
1355                runtime.odometry.obstacle_distance_cm = 15.0;
1356                runtime.log_event("WEB UI: Injected obstacle at 15.0 cm distance!".to_string());
1357                let json = runtime.to_json_state();
1358                format!(
1359                    "HTTP/1.1 200 OK\r\nContent-Type: application/json\r\nAccess-Control-Allow-Origin: *\r\nContent-Length: {}\r\n\r\n{}",
1360                    json.len(),
1361                    json
1362                )
1363            } else if req_str.starts_with("POST /api/cmd/clear") {
1364                runtime.odometry.obstacle_distance_cm = 55.0;
1365                runtime.log_event("WEB UI: Cleared obstacles (distance = 55.0 cm)".to_string());
1366                let json = runtime.to_json_state();
1367                format!(
1368                    "HTTP/1.1 200 OK\r\nContent-Type: application/json\r\nAccess-Control-Allow-Origin: *\r\nContent-Length: {}\r\n\r\n{}",
1369                    json.len(),
1370                    json
1371                )
1372            } else if req_str.starts_with("POST /api/cmd/reset") {
1373                runtime.odometry.x = 0.0;
1374                runtime.odometry.y = 0.0;
1375                runtime.odometry.theta = 0.0;
1376                runtime.log_event("WEB UI: Reset rover odometry to origin (0, 0)".to_string());
1377                let json = runtime.to_json_state();
1378                format!(
1379                    "HTTP/1.1 200 OK\r\nContent-Type: application/json\r\nAccess-Control-Allow-Origin: *\r\nContent-Length: {}\r\n\r\n{}",
1380                    json.len(),
1381                    json
1382                )
1383            } else {
1384                let html = get_web_dashboard_html();
1385                format!(
1386                    "HTTP/1.1 200 OK\r\nContent-Type: text/html; charset=utf-8\r\nContent-Length: {}\r\n\r\n{}",
1387                    html.len(),
1388                    html
1389                )
1390            };
1391
1392            let _ = stream.write_all(response.as_bytes());
1393            let _ = stream.flush();
1394        }
1395    }
1396
1397    Ok(())
1398}
1399
1400fn get_web_dashboard_html() -> &'static str {
1401    r#"<!DOCTYPE html>
1402<html lang="en">
1403<head>
1404  <meta charset="UTF-8">
1405  <title>Sesh 1.0.0 Robotics — HIL Web Simulator & Dashboard</title>
1406  <style>
1407    * { box-sizing: border-box; margin: 0; padding: 0; }
1408    body {
1409      background: #0a0e17;
1410      color: #e0e6ed;
1411      font-family: 'Segoe UI', -apple-system, BlinkMacSystemFont, Roboto, sans-serif;
1412      padding: 20px;
1413    }
1414    header {
1415      display: flex;
1416      justify-content: space-between;
1417      align-items: center;
1418      border-bottom: 2px solid #1f293d;
1419      padding-bottom: 15px;
1420      margin-bottom: 20px;
1421    }
1422    header h1 {
1423      color: #00f0ff;
1424      font-size: 24px;
1425      letter-spacing: 1px;
1426    }
1427    header .status-tag {
1428      background: #00ff6622;
1429      color: #00ff66;
1430      border: 1px solid #00ff66;
1431      padding: 6px 14px;
1432      border-radius: 20px;
1433      font-weight: bold;
1434      font-size: 13px;
1435    }
1436    .grid {
1437      display: grid;
1438      grid-template-columns: 1.2fr 1fr;
1439      gap: 20px;
1440    }
1441    .card {
1442      background: #111827;
1443      border: 1px solid #1f293d;
1444      border-radius: 12px;
1445      padding: 18px;
1446      margin-bottom: 20px;
1447      box-shadow: 0 4px 20px rgba(0,0,0,0.4);
1448    }
1449    .card h2 {
1450      font-size: 16px;
1451      color: #00f0ff;
1452      margin-bottom: 12px;
1453      text-transform: uppercase;
1454      letter-spacing: 0.8px;
1455      border-bottom: 1px solid #1f293d;
1456      padding-bottom: 8px;
1457    }
1458    canvas {
1459      background: #0d131f;
1460      border: 1px solid #23304a;
1461      border-radius: 8px;
1462      width: 100%;
1463      height: 380px;
1464      display: block;
1465    }
1466    .controls {
1467      display: flex;
1468      gap: 12px;
1469      margin-top: 15px;
1470    }
1471    button {
1472      flex: 1;
1473      padding: 10px 15px;
1474      border: none;
1475      border-radius: 6px;
1476      font-weight: bold;
1477      cursor: pointer;
1478      transition: all 0.2s;
1479    }
1480    .btn-obstacle { background: #ff3366; color: #fff; }
1481    .btn-obstacle:hover { background: #e02253; }
1482    .btn-clear { background: #00ff66; color: #0a0e17; }
1483    .btn-clear:hover { background: #00dd55; }
1484    .btn-reset { background: #3b82f6; color: #fff; }
1485    .btn-reset:hover { background: #2563eb; }
1486    .stats-bar {
1487      display: grid;
1488      grid-template-columns: repeat(3, 1fr);
1489      gap: 10px;
1490      margin-top: 15px;
1491    }
1492    .stat-box {
1493      background: #0d131f;
1494      border: 1px solid #1f293d;
1495      padding: 10px;
1496      border-radius: 6px;
1497      text-align: center;
1498    }
1499    .stat-box .label { font-size: 11px; color: #94a3b8; }
1500    .stat-box .val { font-size: 18px; font-weight: bold; color: #00f0ff; margin-top: 4px; }
1501    .pin-grid {
1502      display: grid;
1503      grid-template-columns: repeat(3, 1fr);
1504      gap: 10px;
1505    }
1506    .pin-card {
1507      background: #0d131f;
1508      border: 1px solid #23304a;
1509      padding: 10px;
1510      border-radius: 8px;
1511    }
1512    .pin-card .title { font-size: 12px; font-weight: bold; color: #e2e8f0; }
1513    .pin-card .mode { font-size: 11px; color: #60a5fa; margin-top: 2px; }
1514    .pin-card .state { font-size: 12px; font-weight: bold; margin-top: 4px; }
1515    .state-high { color: #00ff66; }
1516    .state-low { color: #94a3b8; }
1517    .state-pwm { color: #00f0ff; }
1518    .pwm-bar {
1519      height: 4px;
1520      background: #1e293b;
1521      border-radius: 2px;
1522      margin-top: 6px;
1523      overflow: hidden;
1524    }
1525    .pwm-bar-fill {
1526      height: 100%;
1527      background: #00f0ff;
1528      width: 0%;
1529      transition: width 0.3s;
1530    }
1531    .console-box {
1532      background: #070a10;
1533      border: 1px solid #1f293d;
1534      border-radius: 8px;
1535      padding: 12px;
1536      font-family: 'Courier New', Courier, monospace;
1537      font-size: 12px;
1538      height: 160px;
1539      overflow-y: auto;
1540      color: #94a3b8;
1541    }
1542    .console-line { margin-bottom: 4px; }
1543    .console-line span { color: #00f0ff; }
1544  </style>
1545</head>
1546<body>
1547  <header>
1548    <h1>SESH 1.0.0 ROBOTICS — LIVE HIL WEB SIMULATOR</h1>
1549    <div class="status-tag">● HARDWARE IN THE LOOP ACTIVE (500Hz)</div>
1550  </header>
1551
1552  <div class="grid">
1553    <div>
1554      <div class="card">
1555        <h2>2D Kinematic Odometry & Ultrasonic Radar</h2>
1556        <canvas id="radarCanvas" width="600" height="380"></canvas>
1557        <div class="stats-bar">
1558          <div class="stat-box">
1559            <div class="label">ROBOT POSITION (X, Y)</div>
1560            <div class="val" id="posVal">0.00m, 0.00m</div>
1561          </div>
1562          <div class="stat-box">
1563            <div class="label">WHEEL PWM (LEFT / RIGHT)</div>
1564            <div class="val" id="pwmVal">0% / 0%</div>
1565          </div>
1566          <div class="stat-box">
1567            <div class="label">ULTRASONIC OBSTACLE DISTANCE</div>
1568            <div class="val" id="distVal">45.0 cm</div>
1569          </div>
1570        </div>
1571        <div class="controls">
1572          <button class="btn-obstacle" onclick="sendCmd('/api/cmd/obstacle')">⚠️ INJECT OBSTACLE (15 cm)</button>
1573          <button class="btn-clear" onclick="sendCmd('/api/cmd/clear')">✅ CLEAR OBSTACLES (55 cm)</button>
1574          <button class="btn-reset" onclick="sendCmd('/api/cmd/reset')">🔄 RESET ODOMETRY</button>
1575        </div>
1576      </div>
1577    </div>
1578
1579    <div>
1580      <div class="card">
1581        <h2>Hardware Pin Bank (GPIO / PWM Channels)</h2>
1582        <div class="pin-grid" id="pinGrid">
1583          <!-- Populated dynamically -->
1584        </div>
1585      </div>
1586
1587      <div class="card">
1588        <h2>ROS 2 Topic Bus Telemetry Log</h2>
1589        <div class="console-box" id="rosConsole">
1590          <div class="console-line">Waiting for ROS 2 topic traffic...</div>
1591        </div>
1592      </div>
1593
1594      <div class="card">
1595        <h2>Real-Time Hardware Log Feed</h2>
1596        <div class="console-box" id="hwConsole">
1597          <div class="console-line">System initialized. Executing control cycles...</div>
1598        </div>
1599      </div>
1600    </div>
1601  </div>
1602
1603  <script>
1604    const canvas = document.getElementById('radarCanvas');
1605    const ctx = canvas.getContext('2d');
1606
1607    async function fetchState() {
1608      try {
1609        const res = await fetch('/api/state');
1610        const data = await res.json();
1611        updateDashboard(data);
1612      } catch (e) {
1613        console.error("Error fetching state:", e);
1614      }
1615    }
1616
1617    async function sendCmd(endpoint) {
1618      await fetch(endpoint, { method: 'POST' });
1619      fetchState();
1620    }
1621
1622    function updateDashboard(data) {
1623      const odo = data.odometry;
1624      document.getElementById('posVal').innerText = `${odo.x.toFixed(2)}m, ${odo.y.toFixed(2)}m`;
1625      document.getElementById('pwmVal').innerText = `${odo.left_wheel_speed}% / ${odo.right_wheel_speed}%`;
1626      document.getElementById('distVal').innerText = `${odo.obstacle_distance_cm.toFixed(1)} cm`;
1627      document.getElementById('distVal').style.color = odo.obstacle_distance_cm < 20.0 ? '#ff3366' : '#00ff66';
1628
1629      drawRadar(odo);
1630      renderPins(data.gpio_pins);
1631      renderRosTopics(data.ros_topics);
1632      renderLogs(data.logs);
1633    }
1634
1635    function drawRadar(odo) {
1636      ctx.clearRect(0, 0, canvas.width, canvas.height);
1637
1638      // Draw grid
1639      ctx.strokeStyle = '#1e293b';
1640      ctx.lineWidth = 1;
1641      for (let x = 0; x < canvas.width; x += 50) {
1642        ctx.beginPath(); ctx.moveTo(x, 0); ctx.lineTo(x, canvas.height); ctx.stroke();
1643      }
1644      for (let y = 0; y < canvas.height; y += 50) {
1645        ctx.beginPath(); ctx.moveTo(0, y); ctx.lineTo(canvas.width, y); ctx.stroke();
1646      }
1647
1648      // Center crosshair
1649      const cx = 80 + (odo.x * 25) % (canvas.width - 160);
1650      const cy = canvas.height / 2 + (odo.y * 25) % 150;
1651
1652      // Draw obstacle wall if close
1653      if (odo.obstacle_distance_cm < 25.0) {
1654        ctx.fillStyle = '#ff336633';
1655        ctx.fillRect(cx + 40, cy - 60, 20, 120);
1656        ctx.strokeStyle = '#ff3366';
1657        ctx.lineWidth = 3;
1658        ctx.strokeRect(cx + 40, cy - 60, 20, 120);
1659      }
1660
1661      // Draw robot rover
1662      ctx.save();
1663      ctx.translate(cx, cy);
1664      ctx.rotate(odo.theta);
1665
1666      // Ultrasonic sensor cone
1667      ctx.fillStyle = odo.obstacle_distance_cm < 20.0 ? '#ff336622' : '#00f0ff15';
1668      ctx.beginPath();
1669      ctx.moveTo(0, 0);
1670      ctx.arc(0, 0, odo.obstacle_distance_cm * 2, -Math.PI/6, Math.PI/6);
1671      ctx.closePath();
1672      ctx.fill();
1673
1674      // Body
1675      ctx.fillStyle = '#00f0ff';
1676      ctx.fillRect(-22, -16, 44, 32);
1677      ctx.strokeStyle = '#fff';
1678      ctx.lineWidth = 2;
1679      ctx.strokeRect(-22, -16, 44, 32);
1680
1681      // Wheels
1682      ctx.fillStyle = '#475569';
1683      ctx.fillRect(-18, -20, 14, 4);
1684      ctx.fillRect(10, -20, 14, 4);
1685      ctx.fillRect(-18, 16, 14, 4);
1686      ctx.fillRect(10, 16, 14, 4);
1687
1688      // Front indicator
1689      ctx.fillStyle = odo.obstacle_distance_cm < 20.0 ? '#ff3366' : '#00ff66';
1690      ctx.beginPath();
1691      ctx.arc(15, 0, 5, 0, Math.PI * 2);
1692      ctx.fill();
1693
1694      ctx.restore();
1695    }
1696
1697    function renderPins(pins) {
1698      const grid = document.getElementById('pinGrid');
1699      grid.innerHTML = pins.map(p => {
1700        let modeStr = p.mode;
1701        let stateStr = p.level ? 'HIGH' : 'LOW';
1702        let stateClass = p.level ? 'state-high' : 'state-low';
1703        let pwmFill = 0;
1704
1705        if (p.mode === 'Pwm' || p.pwm_duty > 0) {
1706          stateStr = `PWM (${p.pwm_duty}%)`;
1707          stateClass = 'state-pwm';
1708          pwmFill = p.pwm_duty;
1709        }
1710
1711        return `<div class="pin-card">
1712          <div class="title">GPIO PIN ${p.pin_num}</div>
1713          <div class="mode">Mode: ${modeStr}</div>
1714          <div class="state ${stateClass}">${stateStr}</div>
1715          <div class="pwm-bar">
1716            <div class="pwm-bar-fill" style="width: ${pwmFill}%"></div>
1717          </div>
1718        </div>`;
1719      }).join('');
1720    }
1721
1722    function renderRosTopics(topics) {
1723      const box = document.getElementById('rosConsole');
1724      if (topics.length === 0) {
1725        box.innerHTML = '<div class="console-line">No ROS 2 messages published yet.</div>';
1726        return;
1727      }
1728      box.innerHTML = topics.map(t => 
1729        `<div class="console-line">[${t.timestamp_ms}ms] <span>/${t.topic}</span>: "${t.payload}"</div>`
1730      ).join('');
1731    }
1732
1733    function renderLogs(logs) {
1734      const box = document.getElementById('hwConsole');
1735      if (logs.length === 0) return;
1736      box.innerHTML = logs.map(l => `<div class="console-line">${l}</div>`).join('');
1737    }
1738
1739    setInterval(fetchState, 500);
1740    fetchState();
1741  </script>
1742</body>
1743</html>"#
1744}
sesh_runtime/lib.rs

sesh_runtime/
lib.rs
