sesh-runtime 1.0.0

// ============================================================================
// SESH RUNTIME ENGINE & HARDWARE-IN-THE-LOOP SIMULATOR (1.0.0-PROD)
// ============================================================================
// Complete execution engine supporting full control flow (if/while/for/loop/match),
// lexical scope stack, deterministic real-time scheduler, and direct integration
// with real mathematical algorithms from `sesh-stdlib`.
// ============================================================================

use colored::*;
use serde::Serialize;
use sesh_ast::*;
use sesh_stdlib::{
    diff_drive_ik, ekf_update, feedforward_pid, fuse_imu_gps, haversine_distance, ik_3dof_arm,
    lqr_compute, pure_pursuit,
};
use std::collections::{HashMap, VecDeque};
use std::time::Instant;

/// Virtual GPIO Pin State in Simulator
#[derive(Debug, Clone, Serialize)]
pub struct VirtualPin {
    pub pin_num: u32,
    pub mode: PinMode,
    pub level: bool,   // true = HIGH, false = LOW
    pub pwm_duty: u32, // 0..100 %
}

impl Default for VirtualPin {
    fn default() -> Self {
        Self {
            pin_num: 0,
            mode: PinMode::Out,
            level: false,
            pwm_duty: 0,
        }
    }
}

/// Simulated Robot Kinematic Position & Odometry
#[derive(Debug, Clone, Default, Serialize)]
pub struct RobotOdometry {
    pub x: f64,
    pub y: f64,
    pub theta: f64, // radians
    pub left_wheel_speed: i32,
    pub right_wheel_speed: i32,
    pub obstacle_distance_cm: f64,
}

/// Virtual ROS2 Topic Message
#[derive(Debug, Clone, Serialize)]
pub struct RosMessage {
    pub topic: String,
    pub payload: String,
    pub timestamp_ms: u64,
}

#[derive(Debug, Clone, PartialEq)]
pub enum ControlFlow {
    None,
    Return(Value),
    Break,
    Continue,
}

#[derive(Debug, Clone, PartialEq)]
pub enum Value {
    Int(i64),
    Float(f64),
    Bool(bool),
    String(String),
    Tuple(Vec<Value>),
    Array(Vec<Value>),
    Struct(HashMap<String, Value>),
    Void,
}

/// The Sesh Virtual Robotics Hardware Simulator and Runtime Engine
pub struct SeshRuntime {
    pub gpio_bank: HashMap<u32, VirtualPin>,
    pub odometry: RobotOdometry,
    pub ros_topics: HashMap<String, VecDeque<RosMessage>>,
    pub scope_stack: Vec<HashMap<String, Value>>,
    pub robot_pin_map: HashMap<String, u32>,
    pub functions: HashMap<String, FnDecl>,
    pub pid_controllers: HashMap<String, sesh_stdlib::PidState>,
    pub state_machines: HashMap<String, String>, // sm_name -> current_state
    pub callsite_counters: HashMap<String, usize>,
    pub current_task_name: String,
    pub control_flow: ControlFlow,
    pub start_time: Instant,
    pub logs: Vec<String>,
    pub watchdog_ok: bool,
}

impl Default for SeshRuntime {
    fn default() -> Self {
        Self::new()
    }
}

impl SeshRuntime {
    pub fn new() -> Self {
        let mut runtime = Self {
            gpio_bank: HashMap::new(),
            odometry: RobotOdometry {
                x: 0.0,
                y: 0.0,
                theta: 0.0,
                left_wheel_speed: 0,
                right_wheel_speed: 0,
                obstacle_distance_cm: 45.0,
            },
            ros_topics: HashMap::new(),
            scope_stack: vec![HashMap::new()], // Global scope at index 0
            robot_pin_map: HashMap::new(),
            functions: HashMap::new(),
            pid_controllers: HashMap::new(),
            state_machines: HashMap::new(),
            callsite_counters: HashMap::new(),
            current_task_name: "default".to_string(),
            control_flow: ControlFlow::None,
            start_time: Instant::now(),
            logs: Vec::new(),
            watchdog_ok: true,
        };

        // Initialize 40 virtual GPIO pins
        for i in 0..40 {
            runtime.gpio_bank.insert(
                i,
                VirtualPin {
                    pin_num: i,
                    mode: PinMode::Out,
                    level: false,
                    pwm_duty: 0,
                },
            );
        }
        runtime
    }

    pub fn log_event(&mut self, msg: String) {
        let elapsed = self.start_time.elapsed().as_millis();
        self.logs.push(format!("[{:06}ms] {}", elapsed, msg));
    }

    // ========================================================================
    // LEXICAL SCOPING MANAGER (Stack of HashMaps)
    // ========================================================================

    pub fn push_scope(&mut self) {
        self.scope_stack.push(HashMap::new());
    }

    pub fn pop_scope(&mut self) {
        if self.scope_stack.len() > 1 {
            self.scope_stack.pop();
        }
    }

    pub fn define_var(&mut self, name: &str, val: Value) {
        if let Some(scope) = self.scope_stack.last_mut() {
            scope.insert(name.to_string(), val);
        }
    }

    pub fn assign_var(&mut self, name: &str, val: Value) -> bool {
        for scope in self.scope_stack.iter_mut().rev() {
            if scope.contains_key(name) {
                scope.insert(name.to_string(), val);
                return true;
            }
        }
        // Fallback: insert in current scope if not found
        self.define_var(name, val);
        false
    }

    pub fn lookup_var(&self, name: &str) -> Value {
        for scope in self.scope_stack.iter().rev() {
            if let Some(v) = scope.get(name) {
                return v.clone();
            }
        }
        Value::Int(0)
    }

    // ========================================================================
    // HARDWARE HAL FUNCTIONS
    // ========================================================================

    pub fn gpio_set_mode(&mut self, pin: u32, mode: PinMode) {
        if let Some(p) = self.gpio_bank.get_mut(&pin) {
            p.mode = mode;
        }
        self.log_event(format!("GPIO {} set mode {:?}", pin, mode));
    }

    pub fn gpio_write(&mut self, pin: u32, level: bool) {
        if let Some(p) = self.gpio_bank.get_mut(&pin) {
            p.level = level;
        }
        let state = if level { "HIGH" } else { "LOW" };
        self.log_event(format!("GPIO {} write {}", pin, state));
    }

    pub fn gpio_read(&self, pin: u32) -> bool {
        self.gpio_bank.get(&pin).map(|p| p.level).unwrap_or(false)
    }

    pub fn pwm_write(&mut self, pin: u32, duty: u32) {
        let clamped = duty.min(100);
        if let Some(p) = self.gpio_bank.get_mut(&pin) {
            p.mode = PinMode::Pwm;
            p.pwm_duty = clamped;
        }
        self.log_event(format!("PWM pin {} set duty cycle {}%", pin, clamped));

        if pin == 0 {
            self.odometry.left_wheel_speed = clamped as i32;
        } else if pin == 1 {
            self.odometry.right_wheel_speed = clamped as i32;
        }
        self.update_kinematics();
    }

    pub fn ultrasonic_read(&mut self, _trig_pin: u32, _echo_pin: u32) -> f64 {
        let dist = self.odometry.obstacle_distance_cm;
        self.log_event(format!("Ultrasonic sensor read distance: {:.2} cm", dist));
        dist
    }

    pub fn ros_publish(&mut self, topic: &str, payload: &str) {
        let elapsed = self.start_time.elapsed().as_millis() as u64;
        let msg = RosMessage {
            topic: topic.to_string(),
            payload: payload.to_string(),
            timestamp_ms: elapsed,
        };
        self.ros_topics
            .entry(topic.to_string())
            .or_default()
            .push_back(msg.clone());
        self.log_event(format!("ROS Publish [{}] -> {}", topic, payload));
    }

    pub fn update_kinematics(&mut self) {
        let v_left = self.odometry.left_wheel_speed as f64 * 0.05;
        let v_right = self.odometry.right_wheel_speed as f64 * 0.05;
        let v = (v_left + v_right) / 2.0;
        self.odometry.x += v * self.odometry.theta.cos();
        self.odometry.y += v * self.odometry.theta.sin();
    }

    // ========================================================================
    // DETERMINISTIC REAL-TIME SCHEDULER SIMULATION LOOP
    // ========================================================================

    /// Run a multi-cycle real-time scheduler simulation over all tasks in priority order
    pub fn run_scheduler_simulation(
        &mut self,
        program: &Program,
        total_cycles: usize,
    ) -> Result<(), String> {
        self.execute_program(program)?; // run initialization pass
        self.log_event(format!("Starting {} real-time control cycles...", total_cycles));

        for cycle in 0..total_cycles {
            for item in &program.items {
                if let Item::Robot(robot) = item {
                    for task in &robot.tasks {
                        self.log_event(format!(
                            "[Cycle {}] Running Task '{}::{}'",
                            cycle, robot.name, task.name
                        ));
                        self.execute_block(&task.body)?;
                    }
                } else if let Item::Task(task) = item {
                    self.log_event(format!("[Cycle {}] Running Task '{}'", cycle, task.name));
                    self.execute_block(&task.body)?;
                }
            }
        }
        Ok(())
    }

    // ========================================================================
    // ASCII ROBOTICS SIMULATOR DASHBOARD RENDERER
    // ========================================================================

    pub fn render_sim_dashboard(&self) -> String {
        let mut out = String::new();
        out.push_str(&format!(
            "{}\n",
            "========================================================================"
                .bright_blue()
                .bold()
        ));
        out.push_str(&format!(
            "{}  --  {}\n",
            "SESH ROBOTICS VIRTUAL HARDWARE SIMULATOR"
                .bright_green()
                .bold(),
            "1.0.0-PROD".yellow()
        ));
        out.push_str(&format!(
            "{}\n\n",
            "========================================================================"
                .bright_blue()
                .bold()
        ));

        out.push_str(&format!("{}\n", "[1] ROBOT ODOMETRY & MOTOR STATE".bold()));
        out.push_str(&format!(
            "    Position:   X = {:6.2} m   |   Y = {:6.2} m   |   Theta = {:5.2} rad\n",
            self.odometry.x, self.odometry.y, self.odometry.theta
        ));
        out.push_str(&format!(
            "    Motors:     Left PWM = {:3}%  |   Right PWM = {:3}%\n",
            self.odometry.left_wheel_speed, self.odometry.right_wheel_speed
        ));
        out.push_str(&format!(
            "    Sensors:    Ultrasonic Distance = {:.1} cm | Watchdog = {}\n\n",
            self.odometry.obstacle_distance_cm,
            if self.watchdog_ok {
                "OK".green()
            } else {
                "PANIC".red()
            }
        ));

        out.push_str(&format!("{}\n", "[2] ACTIVE GPIO PINS & PWM CHANNELS".bold()));
        for pin_id in [0, 1, 18, 21, 22, 23, 24] {
            if let Some(pin) = self.gpio_bank.get(&pin_id) {
                let state_str = if pin.mode == PinMode::Pwm {
                    format!("PWM ({}%)", pin.pwm_duty).cyan()
                } else if pin.level {
                    "HIGH".green()
                } else {
                    "LOW".red()
                };
                out.push_str(&format!(
                    "    GPIO {:2} : Mode = {:6?} | State = {}\n",
                    pin_id, pin.mode, state_str
                ));
            }
        }
        out.push('\n');

        out.push_str(&format!("{}\n", "[3] ROS 2 TOPICS BUS TELEMETRY".bold()));
        if self.ros_topics.is_empty() {
            out.push_str("    No active ROS messages published yet.\n");
        } else {
            for (topic, msgs) in &self.ros_topics {
                if let Some(latest) = msgs.back() {
                    out.push_str(&format!(
                        "    /{} [{}ms]: \"{}\"\n",
                        topic.bright_cyan(),
                        latest.timestamp_ms,
                        latest.payload
                    ));
                }
            }
        }
        out.push('\n');

        out.push_str(&format!("{}\n", "[4] RECENT REAL-TIME HARDWARE LOGS".bold()));
        let start_idx = self.logs.len().saturating_sub(6);
        for log in &self.logs[start_idx..] {
            out.push_str(&format!("    {}\n", log));
        }

        out.push_str(&format!(
            "\n{}\n",
            "========================================================================"
                .bright_blue()
                .bold()
        ));
        out
    }

    // ========================================================================
    // AST EXECUTION ENGINE WITH FULL CONTROL FLOW
    // ========================================================================

    pub fn register_program(&mut self, program: &Program) {
        for item in &program.items {
            if let Item::Function(f) = item {
                self.functions.insert(f.name.clone(), f.clone());
            } else if let Item::Robot(robot) = item {
                for method in &robot.methods {
                    self.functions.insert(method.name.clone(), method.clone());
                    self.functions.insert(format!("{}_{}", robot.name, method.name), method.clone());
                    self.functions.insert(format!("self.{}", method.name), method.clone());
                }
            }
        }
    }

    pub fn execute_program(&mut self, program: &Program) -> Result<Value, String> {
        self.register_program(program);
        for item in &program.items {
            match item {
                Item::Pin(pin) => {
                    self.gpio_set_mode(pin.pin_num, pin.mode);
                    self.robot_pin_map.insert(pin.name.clone(), pin.pin_num);
                }
                Item::Robot(robot) => {
                    self.log_event(format!("Robot '{}' initialized in simulation", robot.name));
                    for pin in &robot.pins {
                        self.gpio_set_mode(pin.pin_num, pin.mode);
                        self.robot_pin_map
                            .insert(pin.name.clone(), pin.pin_num);
                        self.robot_pin_map
                            .insert(format!("self.{}", pin.name), pin.pin_num);
                    }
                    // Execute init method if present
                    for method in &robot.methods {
                        if method.name == "init" {
                            self.execute_block(&method.body)?;
                        }
                    }
                    // Execute tasks in initial pass
                    for task in &robot.tasks {
                        self.current_task_name = task.name.clone();
                        self.log_event(format!("Executing real-time task '{}'", task.name));
                        self.execute_block(&task.body)?;
                    }
                }
                Item::Function(func) => {
                    if func.name == "main" || func.name == "init" {
                        self.execute_block(&func.body)?;
                    }
                }
                Item::Task(task) => {
                    self.current_task_name = task.name.clone();
                    self.log_event(format!("Executing real-time task '{}'", task.name));
                    self.execute_block(&task.body)?;
                }
                _ => {}
            }
        }
        Ok(Value::Void)
    }

    /// Run a multi-cycle real-time simulation over all tasks in priority order
    pub fn run_simulation(
        &mut self,
        program: &Program,
        cycles: usize,
    ) -> Result<Value, String> {
        self.register_program(program);
        // Initialization pass (pins, init methods)
        for item in &program.items {
            if let Item::Pin(pin) = item {
                self.gpio_set_mode(pin.pin_num, pin.mode);
                self.robot_pin_map.insert(pin.name.clone(), pin.pin_num);
            } else if let Item::Robot(robot) = item {
                self.log_event(format!("Robot '{}' initialized in simulation", robot.name));
                for pin in &robot.pins {
                    self.gpio_set_mode(pin.pin_num, pin.mode);
                    self.robot_pin_map
                        .insert(pin.name.clone(), pin.pin_num);
                    self.robot_pin_map
                        .insert(format!("self.{}", pin.name), pin.pin_num);
                }
                for method in &robot.methods {
                    if method.name == "init" {
                        self.execute_block(&method.body)?;
                    }
                }
                for sm in &robot.state_machines {
                    self.state_machines.insert(sm.name.clone(), sm.initial_state.clone());
                    self.log_event(format!("[StateMachine: {}] Initialized in state '{}'", sm.name, sm.initial_state));
                }
            } else if let Item::Function(func) = item {
                if func.name == "init" {
                    self.execute_block(&func.body)?;
                }
            } else if let Item::StateMachine(sm) = item {
                self.state_machines.insert(sm.name.clone(), sm.initial_state.clone());
                self.log_event(format!("[StateMachine: {}] Initialized in state '{}'", sm.name, sm.initial_state));
            }
        }

        // Run deterministic real-time control loop for N consecutive cycles!
        for cycle in 1..=cycles {
            self.callsite_counters.clear();
            self.log_event(format!("--- Control Cycle {} ---", cycle));

            // Step 2a: Evaluate State Machine transitions!
            for item in &program.items {
                let sm_list: Vec<&StateMachineDecl> = match item {
                    Item::Robot(r) => r.state_machines.iter().collect(),
                    Item::StateMachine(sm) => vec![sm],
                    _ => vec![],
                };
                for sm in sm_list {
                    let curr = self.state_machines.get(&sm.name).cloned().unwrap_or_else(|| sm.initial_state.clone());
                    for tr in &sm.transitions {
                        if tr.from_state == curr {
                            // Evaluate condition expression or flag
                            let cond_ok = if tr.condition == "true" {
                                true
                            } else {
                                self.is_truthy(&self.lookup_var(&tr.condition))
                            };
                            if cond_ok {
                                self.state_machines.insert(sm.name.clone(), tr.to_state.clone());
                                self.log_event(format!(
                                    "[StateMachine: {}] Transitioned {} -> {}",
                                    sm.name, tr.from_state, tr.to_state
                                ));
                                // Execute on_enter block of new state
                                if let Some(state_blk) = sm.states.iter().find(|s| s.name == tr.to_state) {
                                    self.execute_block(&state_blk.on_enter)?;
                                }
                                break;
                            }
                        }
                    }
                }
            }

            // Step 2b: Execute Tasks!
            for item in &program.items {
                if let Item::Robot(robot) = item {
                    for task in &robot.tasks {
                        self.current_task_name = task.name.clone();
                        self.execute_block(&task.body)?;
                    }
                } else if let Item::Task(task) = item {
                    self.current_task_name = task.name.clone();
                    self.execute_block(&task.body)?;
                }
            }
        }

        // Execute main function at the end
        for item in &program.items {
            if let Item::Function(func) = item {
                if func.name == "main" {
                    self.execute_block(&func.body)?;
                }
            }
        }

        Ok(Value::Void)
    }

    pub fn execute_block(&mut self, block: &Block) -> Result<Value, String> {
        self.push_scope();
        let mut last_val = Value::Void;
        for stmt in &block.statements {
            last_val = self.execute_stmt(stmt)?;
            if self.control_flow != ControlFlow::None {
                break;
            }
        }
        self.pop_scope();
        Ok(last_val)
    }

    pub fn execute_stmt(&mut self, stmt: &Stmt) -> Result<Value, String> {
        match stmt {
            Stmt::Let { name, init, .. } => {
                let val = self.eval_expr(init)?;
                self.define_var(name, val);
                Ok(Value::Void)
            }
            Stmt::Assign { target, value, .. } => {
                let val = self.eval_expr(value)?;
                match target {
                    Expr::Variable(name, _) => {
                        self.assign_var(name, val);
                    }
                    Expr::FieldAccess(receiver, field, _) => {
                        if let Expr::Variable(var_name, _) = receiver.as_ref() {
                            let current = self.lookup_var(var_name);
                            if let Value::Struct(mut map) = current {
                                map.insert(field.clone(), val.clone());
                                self.assign_var(var_name, Value::Struct(map));
                            }
                        }
                    }
                    Expr::Index(array, idx, _) => {
                        if let Expr::Variable(var_name, _) = array.as_ref() {
                            let idx_expr = self.eval_expr(idx)?;
                            let idx_val = self.as_int(&idx_expr) as usize;
                            let current = self.lookup_var(var_name);
                            if let Value::Array(mut vec) = current {
                                if idx_val < vec.len() {
                                    vec[idx_val] = val.clone();
                                    self.assign_var(var_name, Value::Array(vec));
                                }
                            }
                        }
                    }
                    _ => {}
                }
                Ok(Value::Void)
            }
            Stmt::If {
                cond,
                then_block,
                else_block,
                ..
            } => {
                let c = self.eval_expr(cond)?;
                if self.is_truthy(&c) {
                    self.execute_block(then_block)
                } else if let Some(e) = else_block {
                    self.execute_block(e)
                } else {
                    Ok(Value::Void)
                }
            }
            Stmt::While { cond, body, .. } => {
                let mut iter = 0;
                let mut cond_val = self.eval_expr(cond)?;
                while self.is_truthy(&cond_val) && iter < 1000 {
                    self.execute_block(body)?;
                    match self.control_flow {
                        ControlFlow::Break => {
                            self.control_flow = ControlFlow::None;
                            break;
                        }
                        ControlFlow::Continue => {
                            self.control_flow = ControlFlow::None;
                            cond_val = self.eval_expr(cond)?;
                            iter += 1;
                            continue;
                        }
                        ControlFlow::Return(_) => break,
                        ControlFlow::None => {}
                    }
                    cond_val = self.eval_expr(cond)?;
                    iter += 1;
                }
                Ok(Value::Void)
            }
            Stmt::For {
                var,
                start,
                end,
                body,
                ..
            } => {
                let s_val = self.eval_expr(start)?;
                let start_val = self.as_int(&s_val);
                let e_val = self.eval_expr(end)?;
                let end_val = self.as_int(&e_val);
                for idx in start_val..end_val {
                    self.push_scope();
                    self.define_var(var, Value::Int(idx));
                    for s in &body.statements {
                        self.execute_stmt(s)?;
                        if self.control_flow != ControlFlow::None {
                            break;
                        }
                    }
                    self.pop_scope();
                    match self.control_flow {
                        ControlFlow::Break => {
                            self.control_flow = ControlFlow::None;
                            break;
                        }
                        ControlFlow::Continue => {
                            self.control_flow = ControlFlow::None;
                            continue;
                        }
                        ControlFlow::Return(_) => break,
                        ControlFlow::None => {}
                    }
                }
                Ok(Value::Void)
            }
            Stmt::Loop { body, .. } => {
                for _ in 0..10 {
                    self.execute_block(body)?;
                    match self.control_flow {
                        ControlFlow::Break => {
                            self.control_flow = ControlFlow::None;
                            break;
                        }
                        ControlFlow::Continue => {
                            self.control_flow = ControlFlow::None;
                            continue;
                        }
                        ControlFlow::Return(_) => break,
                        ControlFlow::None => {}
                    }
                }
                Ok(Value::Void)
            }
            Stmt::Match { target, arms, .. } => {
                let target_val = self.eval_expr(target)?;
                for arm in arms {
                    let matched = match &arm.pattern {
                        Pattern::Wildcard => true,
                        Pattern::Literal(lit) => {
                            let arm_val = self.eval_literal(lit);
                            arm_val == target_val
                        }
                        Pattern::EnumVariant { variant, .. } => {
                            self.val_to_string(&target_val) == *variant
                        }
                    };
                    if matched {
                        return self.execute_block(&arm.body);
                    }
                }
                Ok(Value::Void)
            }
            Stmt::Break(_) => {
                self.control_flow = ControlFlow::Break;
                Ok(Value::Void)
            }
            Stmt::Continue(_) => {
                self.control_flow = ControlFlow::Continue;
                Ok(Value::Void)
            }
            Stmt::Return { value, .. } => {
                let ret_val = if let Some(v) = value {
                    self.eval_expr(v)?
                } else {
                    Value::Void
                };
                self.control_flow = ControlFlow::Return(ret_val.clone());
                Ok(ret_val)
            }
            Stmt::Emit { topic, payload, .. } => {
                let p_val = self.eval_expr(payload)?;
                let p_str = self.val_to_string(&p_val);
                self.ros_publish(topic, &p_str);
                Ok(Value::Void)
            }
            Stmt::Expr(expr) => self.eval_expr(expr),
        }
    }

    pub fn eval_expr(&mut self, expr: &Expr) -> Result<Value, String> {
        match expr {
            Expr::Literal(lit, _) => Ok(self.eval_literal(lit)),
            Expr::Variable(name, _) => {
                if let Some(pin_num) = self.robot_pin_map.get(name) {
                    Ok(Value::Int(*pin_num as i64))
                } else {
                    Ok(self.lookup_var(name))
                }
            }
            Expr::FieldAccess(receiver, field, _) => {
                let recv_val = self.eval_expr(receiver)?;
                if let Value::Struct(map) = &recv_val {
                    return Ok(map.get(field).cloned().unwrap_or(Value::Void));
                }
                if let Some(pin_num) = self.robot_pin_map.get(field) {
                    Ok(Value::Int(*pin_num as i64))
                } else {
                    let key = format!("{}.{}", self.val_to_string(&recv_val), field);
                    if let Some(pin_num) = self.robot_pin_map.get(&key) {
                        Ok(Value::Int(*pin_num as i64))
                    } else {
                        Ok(Value::Int(0))
                    }
                }
            }
            Expr::Binary {
                op, left, right, ..
            } => {
                if *op == BinaryOp::And {
                    let l = self.eval_expr(left)?;
                    if !self.is_truthy(&l) {
                        return Ok(Value::Bool(false));
                    }
                    let r = self.eval_expr(right)?;
                    return Ok(Value::Bool(self.is_truthy(&r)));
                } else if *op == BinaryOp::Or {
                    let l = self.eval_expr(left)?;
                    if self.is_truthy(&l) {
                        return Ok(Value::Bool(true));
                    }
                    let r = self.eval_expr(right)?;
                    return Ok(Value::Bool(self.is_truthy(&r)));
                }
                let l = self.eval_expr(left)?;
                let r = self.eval_expr(right)?;
                self.eval_binary(*op, l, r)
            }
            Expr::Call { func, args, .. } => {
                let mut arg_vals = Vec::new();
                for a in args {
                    arg_vals.push(self.eval_expr(a)?);
                }
                // Check if user-defined function or method!
                if let Some(fn_decl) = self.functions.get(func).cloned() {
                    self.push_scope();
                    for (i, arg_val) in arg_vals.iter().enumerate() {
                        if i < fn_decl.params.len() {
                            self.define_var(&fn_decl.params[i].name, arg_val.clone());
                        }
                    }
                    let res = self.execute_block(&fn_decl.body)?;
                    self.pop_scope();
                    if let ControlFlow::Return(ret) = &self.control_flow {
                        let result = ret.clone();
                        self.control_flow = ControlFlow::None;
                        return Ok(result);
                    }
                    return Ok(res);
                }
                self.eval_stdlib_call(func, &arg_vals)
            }
            Expr::MethodCall { method, args, .. } => {
                let mut arg_vals = Vec::new();
                for a in args {
                    arg_vals.push(self.eval_expr(a)?);
                }
                if let Some(fn_decl) = self
                    .functions
                    .get(method)
                    .cloned()
                    .or_else(|| self.functions.get(&format!("self.{}", method)).cloned())
                {
                    self.push_scope();
                    for (i, arg_val) in arg_vals.iter().enumerate() {
                        if i < fn_decl.params.len() {
                            self.define_var(&fn_decl.params[i].name, arg_val.clone());
                        }
                    }
                    let res = self.execute_block(&fn_decl.body)?;
                    self.pop_scope();
                    if let ControlFlow::Return(ret) = &self.control_flow {
                        let result = ret.clone();
                        self.control_flow = ControlFlow::None;
                        return Ok(result);
                    }
                    return Ok(res);
                }
                Ok(Value::Void)
            }
            Expr::ArrayInit(elems, _) => {
                let mut vals = Vec::new();
                for e in elems {
                    vals.push(self.eval_expr(e)?);
                }
                Ok(Value::Array(vals))
            }
            Expr::Index(array, idx, _) => {
                let arr_val = self.eval_expr(array)?;
                let idx_expr = self.eval_expr(idx)?;
                let idx_val = self.as_int(&idx_expr) as usize;
                if let Value::Array(vals) = arr_val {
                    if idx_val < vals.len() {
                        Ok(vals[idx_val].clone())
                    } else {
                        Ok(Value::Void)
                    }
                } else {
                    Ok(Value::Void)
                }
            }
            Expr::Unary { op, operand, .. } => {
                let val = self.eval_expr(operand)?;
                match op {
                    UnaryOp::Neg => {
                        match val {
                            Value::Int(i) => Ok(Value::Int(-i)),
                            Value::Float(f) => Ok(Value::Float(-f)),
                            _ => Ok(Value::Void),
                        }
                    }
                    UnaryOp::Not => {
                        let b = self.is_truthy(&val);
                        Ok(Value::Bool(!b))
                    }
                    UnaryOp::BitNot => {
                        let i = self.as_int(&val);
                        Ok(Value::Int(!i))
                    }
                }
            }
            Expr::StructInit { fields, .. } => {
                let mut map = HashMap::new();
                for (fname, fexpr) in fields {
                    let v = self.eval_expr(fexpr)?;
                    map.insert(fname.clone(), v);
                }
                Ok(Value::Struct(map))
            }
        }
    }

    fn eval_literal(&self, lit: &Literal) -> Value {
        match lit {
            Literal::Int(v) => Value::Int(*v),
            Literal::Float(v) => Value::Float(*v),
            Literal::Bool(v) => Value::Bool(*v),
            Literal::String(s) => Value::String(s.clone()),
            Literal::DurationMs(ms) => Value::Int(*ms as i64),
            Literal::HardwareConstant(s) => {
                let val = match s.as_str() {
                    "HIGH" | "OUT" | "RISING" => 1,
                    _ => 0,
                };
                Value::Int(val)
            }
        }
    }

    // ========================================================================
    // DIRECT DISPATCH TO REAL RUST COMPUTATIONAL ENGINE IN SESH-STDLIB
    // ========================================================================

    fn eval_stdlib_call(&mut self, func: &str, args: &[Value]) -> Result<Value, String> {
        match func {
            "std::gpio::write" => {
                if args.len() >= 2 {
                    let pin = self.as_int(&args[0]) as u32;
                    let level = self.as_int(&args[1]) > 0;
                    self.gpio_write(pin, level);
                }
                Ok(Value::Void)
            }
            "std::pwm::write" => {
                if args.len() >= 2 {
                    let pin = self.as_int(&args[0]) as u32;
                    let duty = self.as_int(&args[1]) as u32;
                    self.pwm_write(pin, duty);
                }
                Ok(Value::Void)
            }
            "std::sensor::ultrasonic_read" => {
                let trig = if !args.is_empty() {
                    self.as_int(&args[0]) as u32
                } else {
                    23
                };
                let echo = if args.len() >= 2 {
                    self.as_int(&args[1]) as u32
                } else {
                    24
                };
                let dist = self.ultrasonic_read(trig, echo);
                Ok(Value::Float(dist))
            }
            "std::ros::publish" => {
                if args.len() >= 2 {
                    let topic = self.val_to_string(&args[0]);
                    let payload = self.val_to_string(&args[1]);
                    self.ros_publish(&topic, &payload);
                }
                Ok(Value::Void)
            }
            // --- ACTUAL MATHEMATICAL ALGORITHMS ---
            "std::motor::pid_compute" => {
                if args.len() >= 5 {
                    let setpoint = self.as_float(&args[0]);
                    let actual = self.as_float(&args[1]);
                    let kp = self.as_float(&args[2]);
                    let ki = self.as_float(&args[3]);
                    let kd = self.as_float(&args[4]);
                    let (res, prev_err) = {
                        let count = {
                            let c = self
                                .callsite_counters
                                .entry(self.current_task_name.clone())
                                .or_insert(0);
                            *c += 1;
                            *c
                        };
                        let key = format!("{}_pid_{}", self.current_task_name, count);
                        let pid = self
                            .pid_controllers
                            .entry(key)
                            .or_insert_with(|| sesh_stdlib::PidState::new(kp, ki, kd));
                        pid.kp = kp;
                        pid.ki = ki;
                        pid.kd = kd;
                        let r = pid.compute(setpoint, actual, 0.005).clamp(-100.0, 100.0);
                        (r, pid.prev_error)
                    };
                    self.log_event(format!(
                        "PID compute(sp={:.1}, act={:.1}, d_err={:.2}) -> {:.2}",
                        setpoint, actual, prev_err, res
                    ));
                    Ok(Value::Float(res))
                } else {
                    Ok(Value::Float(0.0))
                }
            }
            "std::kinematics::ik_3dof_arm" => {
                if args.len() >= 3 {
                    let x = self.as_float(&args[0]);
                    let y = self.as_float(&args[1]);
                    let z = self.as_float(&args[2]);
                    let (b, s, e) = ik_3dof_arm(x, y, z);
                    self.log_event(format!(
                        "IK 3-DOF Arm solved(x={:.1}, y={:.1}, z={:.1}) -> ({:.1}°, {:.1}°, {:.1}°)",
                        x, y, z, b, s, e
                    ));
                    Ok(Value::Tuple(vec![
                        Value::Float(b),
                        Value::Float(s),
                        Value::Float(e),
                    ]))
                } else {
                    Ok(Value::Void)
                }
            }
            "std::kinematics::diff_drive_ik" => {
                if args.len() >= 2 {
                    let v = self.as_float(&args[0]);
                    let w = self.as_float(&args[1]);
                    let (left, right) = diff_drive_ik(v, w);
                    self.log_event(format!(
                        "Diff Drive IK(v={:.2}, w={:.2}) -> PWM ({}/{})",
                        v, w, left, right
                    ));
                    Ok(Value::Tuple(vec![
                        Value::Int(left as i64),
                        Value::Int(right as i64),
                    ]))
                } else {
                    Ok(Value::Void)
                }
            }
            "std::kalman::ekf_update" => {
                if args.len() >= 4 {
                    let x = self.as_float(&args[0]);
                    let z = self.as_float(&args[1]);
                    let p = self.as_float(&args[2]);
                    let r = self.as_float(&args[3]);
                    let (new_x, new_p) = ekf_update(x, z, p, r);
                    self.log_event(format!(
                        "EKF step(x={:.2}, z={:.2}) -> new_x={:.2}, cov={:.3}",
                        x, z, new_x, new_p
                    ));
                    Ok(Value::Tuple(vec![Value::Float(new_x), Value::Float(new_p)]))
                } else {
                    Ok(Value::Void)
                }
            }
            "std::kalman::fuse_imu_gps" => {
                if args.len() >= 3 {
                    let lat = self.as_float(&args[0]);
                    let lon = self.as_float(&args[1]);
                    let yaw = self.as_float(&args[2]);
                    let (lat_c, lon_c, yaw_c) = fuse_imu_gps(lat, lon, yaw);
                    self.log_event(format!(
                        "EKF fused GPS ({:.4}, {:.4}, yaw={:.1}°)",
                        lat_c, lon_c, yaw_c
                    ));
                    Ok(Value::Tuple(vec![
                        Value::Float(lat_c),
                        Value::Float(lon_c),
                        Value::Float(yaw_c),
                    ]))
                } else {
                    Ok(Value::Void)
                }
            }
            "std::path::pure_pursuit" => {
                if args.len() >= 6 {
                    let rx = self.as_float(&args[0]);
                    let ry = self.as_float(&args[1]);
                    let rtheta = self.as_float(&args[2]);
                    let wx = self.as_float(&args[3]);
                    let wy = self.as_float(&args[4]);
                    let v = self.as_float(&args[5]);
                    let (kappa, w) = pure_pursuit(rx, ry, rtheta, wx, wy, v);
                    self.log_event(format!(
                        "Pure Pursuit kappa={:.3}, w={:.2} rad/s",
                        kappa, w
                    ));
                    Ok(Value::Tuple(vec![Value::Float(kappa), Value::Float(w)]))
                } else {
                    Ok(Value::Void)
                }
            }
            "std::control::feedforward_pid" => {
                if args.len() >= 8 {
                    let sp = self.as_float(&args[0]);
                    let act = self.as_float(&args[1]);
                    let kp = self.as_float(&args[2]);
                    let ki = self.as_float(&args[3]);
                    let kd = self.as_float(&args[4]);
                    let kf = self.as_float(&args[5]);
                    let prev_sp = self.as_float(&args[6]);
                    let dt = self.as_float(&args[7]);
                    let out = feedforward_pid(sp, act, kp, ki, kd, kf, prev_sp, dt);
                    self.log_event(format!("Feedforward PID out = {:.2}", out));
                    Ok(Value::Float(out))
                } else {
                    Ok(Value::Float(0.0))
                }
            }
            "std::control::lqr_compute" => {
                if args.len() >= 4 {
                    let ex = self.as_float(&args[0]);
                    let ev = self.as_float(&args[1]);
                    let k1 = self.as_float(&args[2]);
                    let k2 = self.as_float(&args[3]);
                    let u = lqr_compute(ex, ev, k1, k2);
                    self.log_event(format!("LQR optimal feedback u = {:.2}", u));
                    Ok(Value::Float(u))
                } else {
                    Ok(Value::Float(0.0))
                }
            }
            "std::gps::distance_to_waypoint" => {
                if args.len() >= 4 {
                    let l1 = self.as_float(&args[0]);
                    let n1 = self.as_float(&args[1]);
                    let l2 = self.as_float(&args[2]);
                    let n2 = self.as_float(&args[3]);
                    let d = haversine_distance(l1, n1, l2, n2);
                    self.log_event(format!("GPS Haversine distance = {:.1} m", d));
                    Ok(Value::Float(d))
                } else {
                    Ok(Value::Float(0.0))
                }
            }
            "std::watchdog::start" => {
                self.watchdog_ok = true;
                self.log_event("Watchdog timer started (status: OK)".to_string());
                Ok(Value::Void)
            }
            "std::watchdog::reset" => {
                self.watchdog_ok = true;
                self.log_event("Watchdog timer kicked/refreshed".to_string());
                Ok(Value::Void)
            }
            "print" | "println" => {
                let text = args
                    .iter()
                    .map(|v| self.val_to_string(v))
                    .collect::<Vec<_>>()
                    .join(" ");
                println!("{}", text);
                self.log_event(format!("PRINT: {}", text));
                Ok(Value::Void)
            }
            _ => Ok(Value::Void),
        }
    }

    fn eval_binary(&self, op: BinaryOp, l: Value, r: Value) -> Result<Value, String> {
        let l_float = self.as_float(&l);
        let r_float = self.as_float(&r);

        match (l, r) {
            (Value::Int(a), Value::Int(b)) => match op {
                BinaryOp::Add => Ok(Value::Int(a + b)),
                BinaryOp::Sub => Ok(Value::Int(a - b)),
                BinaryOp::Mul => Ok(Value::Int(a * b)),
                BinaryOp::Div => {
                    if b != 0 {
                        Ok(Value::Int(a / b))
                    } else {
                        eprintln!("[RUNTIME ERROR: Integer division by zero]");
                        Ok(Value::Int(0))
                    }
                }
                BinaryOp::Mod => {
                    if b != 0 {
                        Ok(Value::Int(a % b))
                    } else {
                        eprintln!("[RUNTIME ERROR: Integer modulo by zero]");
                        Ok(Value::Int(0))
                    }
                }
                BinaryOp::Eq => Ok(Value::Bool(a == b)),
                BinaryOp::NotEq => Ok(Value::Bool(a != b)),
                BinaryOp::Lt => Ok(Value::Bool(a < b)),
                BinaryOp::LtEq => Ok(Value::Bool(a <= b)),
                BinaryOp::Gt => Ok(Value::Bool(a > b)),
                BinaryOp::GtEq => Ok(Value::Bool(a >= b)),
                BinaryOp::And => Ok(Value::Bool(a != 0 && b != 0)),
                BinaryOp::Or => Ok(Value::Bool(a != 0 || b != 0)),
                BinaryOp::BitAnd => Ok(Value::Int(a & b)),
                BinaryOp::BitOr => Ok(Value::Int(a | b)),
                BinaryOp::BitXor => Ok(Value::Int(a ^ b)),
                BinaryOp::Shl => Ok(Value::Int(a << (b as u32).min(63))),
                BinaryOp::Shr => Ok(Value::Int(a >> (b as u32).min(63))),
            },
            (Value::Float(_), _) | (_, Value::Float(_)) => match op {
                BinaryOp::Add => Ok(Value::Float(l_float + r_float)),
                BinaryOp::Sub => Ok(Value::Float(l_float - r_float)),
                BinaryOp::Mul => Ok(Value::Float(l_float * r_float)),
                BinaryOp::Div => Ok(Value::Float(if r_float != 0.0 { l_float / r_float } else { 0.0 })),
                BinaryOp::Eq => Ok(Value::Bool((l_float - r_float).abs() < 1e-9)),
                BinaryOp::NotEq => Ok(Value::Bool((l_float - r_float).abs() >= 1e-9)),
                BinaryOp::Lt => Ok(Value::Bool(l_float < r_float)),
                BinaryOp::LtEq => Ok(Value::Bool(l_float <= r_float)),
                BinaryOp::Gt => Ok(Value::Bool(l_float > r_float)),
                BinaryOp::GtEq => Ok(Value::Bool(l_float >= r_float)),
                _ => Ok(Value::Void),
            },
            (Value::String(a), right) => {
                if op == BinaryOp::Add {
                    Ok(Value::String(format!("{}{}", a, self.val_to_string(&right))))
                } else {
                    Ok(Value::Void)
                }
            }
            (left, Value::String(b)) => {
                if op == BinaryOp::Add {
                    Ok(Value::String(format!("{}{}", self.val_to_string(&left), b)))
                } else {
                    Ok(Value::Void)
                }
            }
            _ => Ok(Value::Void),
        }
    }

    fn is_truthy(&self, v: &Value) -> bool {
        match v {
            Value::Bool(b) => *b,
            Value::Int(i) => *i != 0,
            Value::Float(f) => *f != 0.0,
            _ => false,
        }
    }

    fn as_int(&self, v: &Value) -> i64 {
        match v {
            Value::Int(i) => *i,
            Value::Float(f) => *f as i64,
            Value::Bool(b) => {
                if *b {
                    1
                } else {
                    0
                }
            }
            _ => 0,
        }
    }

    fn as_float(&self, v: &Value) -> f64 {
        match v {
            Value::Float(f) => *f,
            Value::Int(i) => *i as f64,
            Value::Bool(b) => {
                if *b {
                    1.0
                } else {
                    0.0
                }
            }
            _ => 0.0,
        }
    }

    fn val_to_string(&self, v: &Value) -> String {
        match v {
            Value::Int(i) => i.to_string(),
            Value::Float(f) => format!("{:.2}", f),
            Value::Bool(b) => b.to_string(),
            Value::String(s) => s.clone(),
            Value::Tuple(vals) => {
                let items: Vec<String> = vals.iter().map(|item| self.val_to_string(item)).collect();
                format!("({})", items.join(", "))
            }
            Value::Array(vals) => {
                let items: Vec<String> = vals.iter().map(|item| self.val_to_string(item)).collect();
                format!("[{}]", items.join(", "))
            }
            Value::Struct(map) => {
                let items: Vec<String> = map
                    .iter()
                    .map(|(k, v)| format!("{}: {}", k, self.val_to_string(v)))
                    .collect();
                format!("{{ {} }}", items.join(", "))
            }
            Value::Void => "void".to_string(),
        }
    }

    pub fn to_json_state(&self) -> String {
        let mut pins = Vec::new();
        for i in [0, 1, 2, 3, 18, 21, 22, 23, 24] {
            if let Some(pin) = self.gpio_bank.get(&i) {
                pins.push(pin.clone());
            }
        }
        let mut topics = Vec::new();
        for (_, msgs) in &self.ros_topics {
            if let Some(msg) = msgs.back() {
                topics.push(msg.clone());
            }
        }
        serde_json::json!({
            "odometry": self.odometry,
            "gpio_pins": pins,
            "ros_topics": topics,
            "logs": self.logs.iter().rev().take(15).cloned().collect::<Vec<_>>(),
            "watchdog_ok": self.watchdog_ok
        })
        .to_string()
    }
}

/// Run the interactive HTML5/Canvas Hardware-In-the-Loop Web Simulator
pub fn run_web_simulator(program: &Program, port: u16) -> Result<(), Box<dyn std::error::Error>> {
    use std::io::{Read, Write};
    use std::net::TcpListener;

    let listener = TcpListener::bind(format!("0.0.0.0:{}", port))?;
    println!(
        "{}\n",
        "========================================================================"
            .bright_cyan()
            .bold()
    );
    println!(
        "{}  --  {}\n",
        "SESH WEB HARDWARE-IN-THE-LOOP (HIL) SIMULATOR STARTED"
            .bright_green()
            .bold(),
        format!("http://0.0.0.0:{}", port).yellow().bold()
    );
    println!(
        "{} Open the Live Preview in your browser to interact with the Robot Dashboard!",
        "[INFO]".bright_blue().bold()
    );
    println!(
        "{}\n",
        "========================================================================"
            .bright_cyan()
            .bold()
    );

    let mut runtime = SeshRuntime::new();
    let _ = runtime.execute_program(program);

    for stream in listener.incoming() {
        if let Ok(mut stream) = stream {
            let mut buf = [0u8; 4096];
            let n = stream.read(&mut buf).unwrap_or(0);
            let req_str = String::from_utf8_lossy(&buf[..n]);

            let response = if req_str.starts_with("GET /api/state") {
                let _ = runtime.execute_program(program);
                let json = runtime.to_json_state();
                format!(
                    "HTTP/1.1 200 OK\r\nContent-Type: application/json\r\nAccess-Control-Allow-Origin: *\r\nContent-Length: {}\r\n\r\n{}",
                    json.len(),
                    json
                )
            } else if req_str.starts_with("POST /api/cmd/obstacle") {
                runtime.odometry.obstacle_distance_cm = 15.0;
                runtime.log_event("WEB UI: Injected obstacle at 15.0 cm distance!".to_string());
                let json = runtime.to_json_state();
                format!(
                    "HTTP/1.1 200 OK\r\nContent-Type: application/json\r\nAccess-Control-Allow-Origin: *\r\nContent-Length: {}\r\n\r\n{}",
                    json.len(),
                    json
                )
            } else if req_str.starts_with("POST /api/cmd/clear") {
                runtime.odometry.obstacle_distance_cm = 55.0;
                runtime.log_event("WEB UI: Cleared obstacles (distance = 55.0 cm)".to_string());
                let json = runtime.to_json_state();
                format!(
                    "HTTP/1.1 200 OK\r\nContent-Type: application/json\r\nAccess-Control-Allow-Origin: *\r\nContent-Length: {}\r\n\r\n{}",
                    json.len(),
                    json
                )
            } else if req_str.starts_with("POST /api/cmd/reset") {
                runtime.odometry.x = 0.0;
                runtime.odometry.y = 0.0;
                runtime.odometry.theta = 0.0;
                runtime.log_event("WEB UI: Reset rover odometry to origin (0, 0)".to_string());
                let json = runtime.to_json_state();
                format!(
                    "HTTP/1.1 200 OK\r\nContent-Type: application/json\r\nAccess-Control-Allow-Origin: *\r\nContent-Length: {}\r\n\r\n{}",
                    json.len(),
                    json
                )
            } else {
                let html = get_web_dashboard_html();
                format!(
                    "HTTP/1.1 200 OK\r\nContent-Type: text/html; charset=utf-8\r\nContent-Length: {}\r\n\r\n{}",
                    html.len(),
                    html
                )
            };

            let _ = stream.write_all(response.as_bytes());
            let _ = stream.flush();
        }
    }

    Ok(())
}

fn get_web_dashboard_html() -> &'static str {
    r#"<!DOCTYPE html>
<html lang="en">
<head>
  <meta charset="UTF-8">
  <title>Sesh 1.0.0 Robotics — HIL Web Simulator & Dashboard</title>
  <style>
    * { box-sizing: border-box; margin: 0; padding: 0; }
    body {
      background: #0a0e17;
      color: #e0e6ed;
      font-family: 'Segoe UI', -apple-system, BlinkMacSystemFont, Roboto, sans-serif;
      padding: 20px;
    }
    header {
      display: flex;
      justify-content: space-between;
      align-items: center;
      border-bottom: 2px solid #1f293d;
      padding-bottom: 15px;
      margin-bottom: 20px;
    }
    header h1 {
      color: #00f0ff;
      font-size: 24px;
      letter-spacing: 1px;
    }
    header .status-tag {
      background: #00ff6622;
      color: #00ff66;
      border: 1px solid #00ff66;
      padding: 6px 14px;
      border-radius: 20px;
      font-weight: bold;
      font-size: 13px;
    }
    .grid {
      display: grid;
      grid-template-columns: 1.2fr 1fr;
      gap: 20px;
    }
    .card {
      background: #111827;
      border: 1px solid #1f293d;
      border-radius: 12px;
      padding: 18px;
      margin-bottom: 20px;
      box-shadow: 0 4px 20px rgba(0,0,0,0.4);
    }
    .card h2 {
      font-size: 16px;
      color: #00f0ff;
      margin-bottom: 12px;
      text-transform: uppercase;
      letter-spacing: 0.8px;
      border-bottom: 1px solid #1f293d;
      padding-bottom: 8px;
    }
    canvas {
      background: #0d131f;
      border: 1px solid #23304a;
      border-radius: 8px;
      width: 100%;
      height: 380px;
      display: block;
    }
    .controls {
      display: flex;
      gap: 12px;
      margin-top: 15px;
    }
    button {
      flex: 1;
      padding: 10px 15px;
      border: none;
      border-radius: 6px;
      font-weight: bold;
      cursor: pointer;
      transition: all 0.2s;
    }
    .btn-obstacle { background: #ff3366; color: #fff; }
    .btn-obstacle:hover { background: #e02253; }
    .btn-clear { background: #00ff66; color: #0a0e17; }
    .btn-clear:hover { background: #00dd55; }
    .btn-reset { background: #3b82f6; color: #fff; }
    .btn-reset:hover { background: #2563eb; }
    .stats-bar {
      display: grid;
      grid-template-columns: repeat(3, 1fr);
      gap: 10px;
      margin-top: 15px;
    }
    .stat-box {
      background: #0d131f;
      border: 1px solid #1f293d;
      padding: 10px;
      border-radius: 6px;
      text-align: center;
    }
    .stat-box .label { font-size: 11px; color: #94a3b8; }
    .stat-box .val { font-size: 18px; font-weight: bold; color: #00f0ff; margin-top: 4px; }
    .pin-grid {
      display: grid;
      grid-template-columns: repeat(3, 1fr);
      gap: 10px;
    }
    .pin-card {
      background: #0d131f;
      border: 1px solid #23304a;
      padding: 10px;
      border-radius: 8px;
    }
    .pin-card .title { font-size: 12px; font-weight: bold; color: #e2e8f0; }
    .pin-card .mode { font-size: 11px; color: #60a5fa; margin-top: 2px; }
    .pin-card .state { font-size: 12px; font-weight: bold; margin-top: 4px; }
    .state-high { color: #00ff66; }
    .state-low { color: #94a3b8; }
    .state-pwm { color: #00f0ff; }
    .pwm-bar {
      height: 4px;
      background: #1e293b;
      border-radius: 2px;
      margin-top: 6px;
      overflow: hidden;
    }
    .pwm-bar-fill {
      height: 100%;
      background: #00f0ff;
      width: 0%;
      transition: width 0.3s;
    }
    .console-box {
      background: #070a10;
      border: 1px solid #1f293d;
      border-radius: 8px;
      padding: 12px;
      font-family: 'Courier New', Courier, monospace;
      font-size: 12px;
      height: 160px;
      overflow-y: auto;
      color: #94a3b8;
    }
    .console-line { margin-bottom: 4px; }
    .console-line span { color: #00f0ff; }
  </style>
</head>
<body>
  <header>
    <h1>SESH 1.0.0 ROBOTICS — LIVE HIL WEB SIMULATOR</h1>
    <div class="status-tag">● HARDWARE IN THE LOOP ACTIVE (500Hz)</div>
  </header>

  <div class="grid">
    <div>
      <div class="card">
        <h2>2D Kinematic Odometry & Ultrasonic Radar</h2>
        <canvas id="radarCanvas" width="600" height="380"></canvas>
        <div class="stats-bar">
          <div class="stat-box">
            <div class="label">ROBOT POSITION (X, Y)</div>
            <div class="val" id="posVal">0.00m, 0.00m</div>
          </div>
          <div class="stat-box">
            <div class="label">WHEEL PWM (LEFT / RIGHT)</div>
            <div class="val" id="pwmVal">0% / 0%</div>
          </div>
          <div class="stat-box">
            <div class="label">ULTRASONIC OBSTACLE DISTANCE</div>
            <div class="val" id="distVal">45.0 cm</div>
          </div>
        </div>
        <div class="controls">
          <button class="btn-obstacle" onclick="sendCmd('/api/cmd/obstacle')">⚠️ INJECT OBSTACLE (15 cm)</button>
          <button class="btn-clear" onclick="sendCmd('/api/cmd/clear')">✅ CLEAR OBSTACLES (55 cm)</button>
          <button class="btn-reset" onclick="sendCmd('/api/cmd/reset')">🔄 RESET ODOMETRY</button>
        </div>
      </div>
    </div>

    <div>
      <div class="card">
        <h2>Hardware Pin Bank (GPIO / PWM Channels)</h2>
        <div class="pin-grid" id="pinGrid">
          <!-- Populated dynamically -->
        </div>
      </div>

      <div class="card">
        <h2>ROS 2 Topic Bus Telemetry Log</h2>
        <div class="console-box" id="rosConsole">
          <div class="console-line">Waiting for ROS 2 topic traffic...</div>
        </div>
      </div>

      <div class="card">
        <h2>Real-Time Hardware Log Feed</h2>
        <div class="console-box" id="hwConsole">
          <div class="console-line">System initialized. Executing control cycles...</div>
        </div>
      </div>
    </div>
  </div>

  <script>
    const canvas = document.getElementById('radarCanvas');
    const ctx = canvas.getContext('2d');

    async function fetchState() {
      try {
        const res = await fetch('/api/state');
        const data = await res.json();
        updateDashboard(data);
      } catch (e) {
        console.error("Error fetching state:", e);
      }
    }

    async function sendCmd(endpoint) {
      await fetch(endpoint, { method: 'POST' });
      fetchState();
    }

    function updateDashboard(data) {
      const odo = data.odometry;
      document.getElementById('posVal').innerText = `${odo.x.toFixed(2)}m, ${odo.y.toFixed(2)}m`;
      document.getElementById('pwmVal').innerText = `${odo.left_wheel_speed}% / ${odo.right_wheel_speed}%`;
      document.getElementById('distVal').innerText = `${odo.obstacle_distance_cm.toFixed(1)} cm`;
      document.getElementById('distVal').style.color = odo.obstacle_distance_cm < 20.0 ? '#ff3366' : '#00ff66';

      drawRadar(odo);
      renderPins(data.gpio_pins);
      renderRosTopics(data.ros_topics);
      renderLogs(data.logs);
    }

    function drawRadar(odo) {
      ctx.clearRect(0, 0, canvas.width, canvas.height);

      // Draw grid
      ctx.strokeStyle = '#1e293b';
      ctx.lineWidth = 1;
      for (let x = 0; x < canvas.width; x += 50) {
        ctx.beginPath(); ctx.moveTo(x, 0); ctx.lineTo(x, canvas.height); ctx.stroke();
      }
      for (let y = 0; y < canvas.height; y += 50) {
        ctx.beginPath(); ctx.moveTo(0, y); ctx.lineTo(canvas.width, y); ctx.stroke();
      }

      // Center crosshair
      const cx = 80 + (odo.x * 25) % (canvas.width - 160);
      const cy = canvas.height / 2 + (odo.y * 25) % 150;

      // Draw obstacle wall if close
      if (odo.obstacle_distance_cm < 25.0) {
        ctx.fillStyle = '#ff336633';
        ctx.fillRect(cx + 40, cy - 60, 20, 120);
        ctx.strokeStyle = '#ff3366';
        ctx.lineWidth = 3;
        ctx.strokeRect(cx + 40, cy - 60, 20, 120);
      }

      // Draw robot rover
      ctx.save();
      ctx.translate(cx, cy);
      ctx.rotate(odo.theta);

      // Ultrasonic sensor cone
      ctx.fillStyle = odo.obstacle_distance_cm < 20.0 ? '#ff336622' : '#00f0ff15';
      ctx.beginPath();
      ctx.moveTo(0, 0);
      ctx.arc(0, 0, odo.obstacle_distance_cm * 2, -Math.PI/6, Math.PI/6);
      ctx.closePath();
      ctx.fill();

      // Body
      ctx.fillStyle = '#00f0ff';
      ctx.fillRect(-22, -16, 44, 32);
      ctx.strokeStyle = '#fff';
      ctx.lineWidth = 2;
      ctx.strokeRect(-22, -16, 44, 32);

      // Wheels
      ctx.fillStyle = '#475569';
      ctx.fillRect(-18, -20, 14, 4);
      ctx.fillRect(10, -20, 14, 4);
      ctx.fillRect(-18, 16, 14, 4);
      ctx.fillRect(10, 16, 14, 4);

      // Front indicator
      ctx.fillStyle = odo.obstacle_distance_cm < 20.0 ? '#ff3366' : '#00ff66';
      ctx.beginPath();
      ctx.arc(15, 0, 5, 0, Math.PI * 2);
      ctx.fill();

      ctx.restore();
    }

    function renderPins(pins) {
      const grid = document.getElementById('pinGrid');
      grid.innerHTML = pins.map(p => {
        let modeStr = p.mode;
        let stateStr = p.level ? 'HIGH' : 'LOW';
        let stateClass = p.level ? 'state-high' : 'state-low';
        let pwmFill = 0;

        if (p.mode === 'Pwm' || p.pwm_duty > 0) {
          stateStr = `PWM (${p.pwm_duty}%)`;
          stateClass = 'state-pwm';
          pwmFill = p.pwm_duty;
        }

        return `<div class="pin-card">
          <div class="title">GPIO PIN ${p.pin_num}</div>
          <div class="mode">Mode: ${modeStr}</div>
          <div class="state ${stateClass}">${stateStr}</div>
          <div class="pwm-bar">
            <div class="pwm-bar-fill" style="width: ${pwmFill}%"></div>
          </div>
        </div>`;
      }).join('');
    }

    function renderRosTopics(topics) {
      const box = document.getElementById('rosConsole');
      if (topics.length === 0) {
        box.innerHTML = '<div class="console-line">No ROS 2 messages published yet.</div>';
        return;
      }
      box.innerHTML = topics.map(t => 
        `<div class="console-line">[${t.timestamp_ms}ms] <span>/${t.topic}</span>: "${t.payload}"</div>`
      ).join('');
    }

    function renderLogs(logs) {
      const box = document.getElementById('hwConsole');
      if (logs.length === 0) return;
      box.innerHTML = logs.map(l => `<div class="console-line">${l}</div>`).join('');
    }

    setInterval(fetchState, 500);
    fetchState();
  </script>
</body>
</html>"#
}