gloamy-robot 0.1.0

//! Safety monitor and movement-gating utilities.
//!
//! This module is designed to stay independent from the LLM control loop.
//! Movement requests may originate from an agent, but approval, rate limiting,
//! obstacle tracking, and emergency-stop behavior live here.
//!
//! Safety order of operations:
//!
//! 1. pre-move checks
//! 2. live sensor updates
//! 3. reactive blocking and recovery
//! 4. watchdog notifications
//! 5. hardware emergency-stop override

use crate::config::{RobotConfig, SafetyConfig};
use crate::traits::ToolResult;
use anyhow::Result;
use std::sync::atomic::{AtomicBool, AtomicU64, Ordering};
use std::sync::Arc;
use std::time::{Duration, Instant};
use tokio::sync::{broadcast, RwLock};

/// Safety events broadcast to all listeners
#[derive(Debug, Clone)]
pub enum SafetyEvent {
    /// Obstacle detected, movement blocked
    ObstacleDetected { distance: f64, angle: u16 },
    /// Emergency stop triggered
    EmergencyStop { reason: String },
    /// Watchdog timeout - no activity
    WatchdogTimeout,
    /// Movement approved
    MovementApproved,
    /// Movement denied with reason
    MovementDenied { reason: String },
    /// Bump sensor triggered
    BumpDetected { sensor: String },
    /// System recovered, ready to move again
    Recovered,
}

/// Shared real-time safety state.
pub struct SafetyState {
    /// Is it safe to move?
    pub can_move: AtomicBool,
    /// Emergency stop active?
    pub estop_active: AtomicBool,
    /// Last movement command timestamp (ms since epoch)
    pub last_command_ms: AtomicU64,
    /// Current minimum distance to obstacle
    pub min_obstacle_distance: RwLock<f64>,
    /// Reason movement is blocked (if any)
    pub block_reason: RwLock<Option<String>>,
    /// Speed multiplier based on proximity (0.0 - 1.0)
    pub speed_limit: RwLock<f64>,
}

impl Default for SafetyState {
    fn default() -> Self {
        Self {
            can_move: AtomicBool::new(true),
            estop_active: AtomicBool::new(false),
            last_command_ms: AtomicU64::new(0),
            min_obstacle_distance: RwLock::new(999.0),
            block_reason: RwLock::new(None),
            speed_limit: RwLock::new(1.0),
        }
    }
}

/// Background safety monitor that tracks movement eligibility.
pub struct SafetyMonitor {
    config: SafetyConfig,
    state: Arc<SafetyState>,
    event_tx: broadcast::Sender<SafetyEvent>,
    shutdown: AtomicBool,
}

impl SafetyMonitor {
    pub fn new(config: SafetyConfig) -> (Self, broadcast::Receiver<SafetyEvent>) {
        let (event_tx, event_rx) = broadcast::channel(64);
        let monitor = Self {
            config,
            state: Arc::new(SafetyState::default()),
            event_tx,
            shutdown: AtomicBool::new(false),
        };
        (monitor, event_rx)
    }

    pub fn state(&self) -> Arc<SafetyState> {
        self.state.clone()
    }

    pub fn subscribe(&self) -> broadcast::Receiver<SafetyEvent> {
        self.event_tx.subscribe()
    }

    /// Check if movement is currently allowed
    pub async fn can_move(&self) -> bool {
        if self.state.estop_active.load(Ordering::SeqCst) {
            return false;
        }
        self.state.can_move.load(Ordering::SeqCst)
    }

    /// Get current speed limit multiplier (0.0 - 1.0)
    pub async fn speed_limit(&self) -> f64 {
        *self.state.speed_limit.read().await
    }

    /// Request permission to move - returns allowed speed multiplier or error
    pub async fn request_movement(&self, direction: &str, distance: f64) -> Result<f64, String> {
        // Check E-stop
        if self.state.estop_active.load(Ordering::SeqCst) {
            return Err("Emergency stop active".to_string());
        }

        // Check general movement permission
        if !self.state.can_move.load(Ordering::SeqCst) {
            let reason = self.state.block_reason.read().await;
            return Err(reason
                .clone()
                .unwrap_or_else(|| "Movement blocked".to_string()));
        }

        // Check obstacle distance in movement direction
        let min_dist = *self.state.min_obstacle_distance.read().await;
        if min_dist < self.config.min_obstacle_distance {
            let msg = format!(
                "Obstacle too close: {:.2}m (min: {:.2}m)",
                min_dist, self.config.min_obstacle_distance
            );
            let _ = self.event_tx.send(SafetyEvent::MovementDenied {
                reason: msg.clone(),
            });
            return Err(msg);
        }

        // Check if requested distance would hit obstacle
        if distance > min_dist - self.config.min_obstacle_distance {
            let safe_distance = (min_dist - self.config.min_obstacle_distance).max(0.0);
            if safe_distance < 0.1 {
                return Err(format!(
                    "Cannot move {}: obstacle at {:.2}m",
                    direction, min_dist
                ));
            }
            // Allow reduced distance
            tracing::warn!(
                "Reducing {} distance from {:.2}m to {:.2}m due to obstacle",
                direction,
                distance,
                safe_distance
            );
        }

        // Update last command time
        let now_ms = std::time::SystemTime::now()
            .duration_since(std::time::UNIX_EPOCH)
            .unwrap()
            .as_millis() as u64;
        self.state.last_command_ms.store(now_ms, Ordering::SeqCst);

        // Calculate speed limit based on proximity
        let speed_mult = self.calculate_speed_limit(min_dist).await;

        let _ = self.event_tx.send(SafetyEvent::MovementApproved);
        Ok(speed_mult)
    }

    /// Calculate safe speed based on obstacle proximity
    async fn calculate_speed_limit(&self, obstacle_distance: f64) -> f64 {
        let min_dist = self.config.min_obstacle_distance;
        let slow_zone = (min_dist * self.config.slow_zone_multiplier).max(min_dist);
        let approach_cap = self.config.approach_speed_limit.clamp(0.0, 1.0);

        let limit = if obstacle_distance >= slow_zone {
            1.0 // Full speed
        } else if obstacle_distance <= min_dist {
            0.0 // Stop
        } else {
            // Linear interpolation between stop and the configured approach cap.
            let ratio = (obstacle_distance - min_dist) / (slow_zone - min_dist);
            ratio * approach_cap
        };

        *self.state.speed_limit.write().await = limit;
        limit
    }

    /// Trigger emergency stop
    pub async fn emergency_stop(&self, reason: &str) {
        tracing::error!("EMERGENCY STOP: {}", reason);
        self.state.estop_active.store(true, Ordering::SeqCst);
        self.state.can_move.store(false, Ordering::SeqCst);
        *self.state.block_reason.write().await = Some(reason.to_string());

        let _ = self.event_tx.send(SafetyEvent::EmergencyStop {
            reason: reason.to_string(),
        });
    }

    /// Reset emergency stop (requires explicit action)
    pub async fn reset_estop(&self) {
        tracing::info!("E-STOP RESET");
        self.state.estop_active.store(false, Ordering::SeqCst);
        self.state.can_move.store(true, Ordering::SeqCst);
        *self.state.block_reason.write().await = None;

        let _ = self.event_tx.send(SafetyEvent::Recovered);
    }

    /// Update obstacle distance (call from sensor loop)
    pub async fn update_obstacle_distance(&self, distance: f64, angle: u16) {
        // Update minimum distance tracking
        {
            let mut min_dist = self.state.min_obstacle_distance.write().await;
            // Always update to current reading (not just if closer)
            *min_dist = distance;
        }

        // Recalculate speed limit based on new distance
        self.calculate_speed_limit(distance).await;

        // Check if too close
        if distance < self.config.min_obstacle_distance {
            self.state.can_move.store(false, Ordering::SeqCst);
            *self.state.block_reason.write().await =
                Some(format!("Obstacle at {:.2}m ({}°)", distance, angle));

            let _ = self
                .event_tx
                .send(SafetyEvent::ObstacleDetected { distance, angle });
        } else if !self.state.estop_active.load(Ordering::SeqCst) {
            // Clear block if obstacle moved away and no E-stop
            self.state.can_move.store(true, Ordering::SeqCst);
            *self.state.block_reason.write().await = None;
        }
    }

    /// Report bump sensor triggered
    pub async fn bump_detected(&self, sensor: &str) {
        tracing::warn!("BUMP DETECTED: {}", sensor);

        // Immediate stop
        self.state.can_move.store(false, Ordering::SeqCst);
        *self.state.block_reason.write().await = Some(format!("Bump: {}", sensor));

        let _ = self.event_tx.send(SafetyEvent::BumpDetected {
            sensor: sensor.to_string(),
        });

        // Auto-recover after brief pause (robot should back up)
        tokio::spawn({
            let state = self.state.clone();
            let event_tx = self.event_tx.clone();
            async move {
                tokio::time::sleep(Duration::from_secs(2)).await;
                if !state.estop_active.load(Ordering::SeqCst) {
                    state.can_move.store(true, Ordering::SeqCst);
                    *state.block_reason.write().await = None;
                    let _ = event_tx.send(SafetyEvent::Recovered);
                }
            }
        });
    }

    /// Shutdown the monitor
    pub fn shutdown(&self) {
        self.shutdown.store(true, Ordering::SeqCst);
    }

    /// Run the safety monitor loop (call in background task)
    pub async fn run(&self, mut sensor_rx: tokio::sync::mpsc::Receiver<SensorReading>) {
        let watchdog_timeout = Duration::from_secs(self.config.max_drive_duration);
        let mut last_sensor_update = Instant::now();

        while !self.shutdown.load(Ordering::SeqCst) {
            tokio::select! {
                // Process sensor readings
                Some(reading) = sensor_rx.recv() => {
                    last_sensor_update = Instant::now();
                    match reading {
                        SensorReading::Lidar { distance, angle } => {
                            self.update_obstacle_distance(distance, angle).await;
                        }
                        SensorReading::Bump { sensor } => {
                            self.bump_detected(&sensor).await;
                        }
                        SensorReading::Estop { pressed } => {
                            if pressed {
                                self.emergency_stop("Hardware E-stop pressed").await;
                            }
                        }
                    }
                }

                // Watchdog check every second
                _ = tokio::time::sleep(Duration::from_secs(1)) => {
                    // Check for sensor timeout
                    if last_sensor_update.elapsed()
                        > Duration::from_secs(self.config.sensor_timeout_secs)
                    {
                        tracing::warn!("Sensor data stale - blocking movement");
                        self.state.can_move.store(false, Ordering::SeqCst);
                        *self.state.block_reason.write().await =
                            Some("Sensor data stale".to_string());
                    }

                    // Check watchdog (auto-stop if no commands)
                    let last_cmd_ms = self.state.last_command_ms.load(Ordering::SeqCst);
                    if last_cmd_ms > 0 {
                        let now_ms = std::time::SystemTime::now()
                            .duration_since(std::time::UNIX_EPOCH)
                            .unwrap()
                            .as_millis() as u64;

                        let elapsed = Duration::from_millis(now_ms - last_cmd_ms);
                        if elapsed > watchdog_timeout {
                            tracing::info!("Watchdog timeout - no commands for {:?}", elapsed);
                            let _ = self.event_tx.send(SafetyEvent::WatchdogTimeout);
                            // Don't block movement, just notify
                        }
                    }
                }
            }
        }
    }
}

/// Sensor readings fed into the safety monitor loop.
#[derive(Debug, Clone)]
pub enum SensorReading {
    Lidar { distance: f64, angle: u16 },
    Bump { sensor: String },
    Estop { pressed: bool },
}

/// Drive wrapper that enforces movement approval through [`SafetyMonitor`].
pub struct SafeDrive {
    inner_drive: Arc<dyn crate::traits::Tool>,
    safety: Arc<SafetyMonitor>,
}

impl SafeDrive {
    pub fn new(drive: Arc<dyn crate::traits::Tool>, safety: Arc<SafetyMonitor>) -> Self {
        Self {
            inner_drive: drive,
            safety,
        }
    }
}

#[async_trait::async_trait]
impl crate::traits::Tool for SafeDrive {
    fn name(&self) -> &str {
        "drive"
    }

    fn description(&self) -> &str {
        "Move the robot (with safety limits enforced)"
    }

    fn parameters_schema(&self) -> serde_json::Value {
        self.inner_drive.parameters_schema()
    }

    async fn execute(&self, args: serde_json::Value) -> Result<ToolResult> {
        // ToolResult imported at top of file

        let action = args["action"].as_str().unwrap_or("unknown");
        let distance = args["distance"].as_f64().unwrap_or(0.5);

        // Always allow stop
        if action == "stop" {
            return self.inner_drive.execute(args).await;
        }

        // Request permission from safety system
        match self.safety.request_movement(action, distance).await {
            Ok(speed_mult) => {
                // Modify speed in args
                let mut modified_args = args.clone();
                let original_speed = args["speed"].as_f64().unwrap_or(0.5);
                modified_args["speed"] = serde_json::json!(original_speed * speed_mult);

                if speed_mult < 1.0 {
                    tracing::info!(
                        "Safety: Reducing speed to {:.0}% due to obstacle proximity",
                        speed_mult * 100.0
                    );
                }

                self.inner_drive.execute(modified_args).await
            }
            Err(reason) => Ok(ToolResult {
                success: false,
                output: String::new(),
                error: Some(format!("Safety blocked movement: {}", reason)),
            }),
        }
    }
}

/// Pre-flight safety check before any operation
pub async fn preflight_check(config: &RobotConfig) -> Result<Vec<String>> {
    let mut warnings = Vec::new();

    // Check safety config
    if config.safety.min_obstacle_distance < 0.1 {
        warnings.push("WARNING: min_obstacle_distance < 0.1m is dangerously low".to_string());
    }

    if config.safety.max_drive_duration > 60 {
        warnings.push("WARNING: max_drive_duration > 60s may allow runaway".to_string());
    }

    if config.drive.max_speed > 1.0 {
        warnings.push("WARNING: max_speed > 1.0 m/s is very fast for indoor use".to_string());
    }

    if config.safety.estop_pin.is_none() {
        warnings.push(
            "WARNING: No E-stop pin configured. Recommend wiring a hardware stop button."
                .to_string(),
        );
    }

    // Check for sensor availability
    if config.sensors.lidar_type == "mock" {
        warnings.push("NOTICE: LIDAR in mock mode - no real obstacle detection".to_string());
    }

    Ok(warnings)
}

#[cfg(test)]
mod tests {
    use super::*;

    #[tokio::test]
    async fn safety_state_defaults() {
        let state = SafetyState::default();
        assert!(state.can_move.load(Ordering::SeqCst));
        assert!(!state.estop_active.load(Ordering::SeqCst));
    }

    #[tokio::test]
    async fn safety_monitor_blocks_on_obstacle() {
        let config = SafetyConfig::default();

        let (monitor, _rx) = SafetyMonitor::new(config);

        // Initially can move
        assert!(monitor.can_move().await);

        // Report close obstacle
        monitor.update_obstacle_distance(0.2, 0).await;

        // Now blocked
        assert!(!monitor.can_move().await);
    }

    #[tokio::test]
    async fn safety_monitor_estop() {
        let config = SafetyConfig::default();
        let (monitor, mut rx) = SafetyMonitor::new(config);

        monitor.emergency_stop("test").await;

        assert!(!monitor.can_move().await);
        assert!(monitor.state.estop_active.load(Ordering::SeqCst));

        // Check event was sent
        let event = rx.try_recv().unwrap();
        matches!(event, SafetyEvent::EmergencyStop { .. });
    }

    #[tokio::test]
    async fn speed_limit_calculation() {
        let config = SafetyConfig {
            min_obstacle_distance: 0.3,
            slow_zone_multiplier: 3.0,
            approach_speed_limit: 0.3,
            ..Default::default()
        };
        let (monitor, _rx) = SafetyMonitor::new(config);

        // Far obstacle = full speed
        let speed = monitor.calculate_speed_limit(2.0).await;
        assert!((speed - 1.0).abs() < 0.01);

        // Close obstacle = reduced speed
        let speed = monitor.calculate_speed_limit(0.5).await;
        assert!(speed < 1.0);
        assert!(speed > 0.0);
        assert!(speed <= 0.3);

        // At minimum = stop
        let speed = monitor.calculate_speed_limit(0.3).await;
        assert!((speed - 0.0).abs() < 0.01);
    }

    #[tokio::test]
    async fn request_movement_blocked() {
        let config = SafetyConfig {
            min_obstacle_distance: 0.3,
            ..Default::default()
        };
        let (monitor, _rx) = SafetyMonitor::new(config);

        // Set obstacle too close
        monitor.update_obstacle_distance(0.2, 0).await;

        // Movement should be denied
        let result = monitor.request_movement("forward", 1.0).await;
        assert!(result.is_err());
    }

    impl Default for SafetyConfig {
        fn default() -> Self {
            Self {
                min_obstacle_distance: 0.3,
                slow_zone_multiplier: 3.0,
                approach_speed_limit: 0.3,
                max_drive_duration: 30,
                estop_pin: Some(4),
                bump_sensor_pins: vec![5, 6],
                bump_reverse_distance: 0.15,
                confirm_movement: false,
                predict_collisions: true,
                sensor_timeout_secs: 5,
                blind_mode_speed_limit: 0.2,
            }
        }
    }
}