mecha10-cli 0.6.3

//! Entering teleop control mode for dev mode

use super::registry::NodeRegistry;
use crate::dev::termination::ScreenOutcome;
use crate::types::ProjectConfig;
use crate::ui::{LogBuffer, TeleopUi};
use anyhow::Result;
use serde::Deserialize;
use std::path::Path;
use std::sync::atomic::AtomicBool;
use std::sync::Arc;

/// Teleop node's velocity/physics limits, as read from its own `TeleopConfig`
/// (`configs/nodes/@mecha10/teleop/config.json` or equivalent). Mirrors
/// `TeleopConfig::max_linear_vel`/`max_angular_vel`/`acceleration` in
/// `packages/nodes/teleop/src/config.rs` - kept as a standalone struct here
/// (rather than depending on the `mecha10-nodes-teleop` crate) since the CLI
/// only needs these fields to compute per-keypress command targets and to
/// forward an optional physics override to the simulator subprocess, not
/// the full node config. Named `Limits` rather than `VelocityLimits` since it
/// now also carries `acceleration`, which is an optional override rather than
/// a limit with its own compiled-in default.
#[derive(Debug, Deserialize)]
pub(super) struct TeleopPhysicsLimits {
    #[serde(default = "default_max_linear_vel")]
    pub(super) max_linear_vel: f32,
    #[serde(default = "default_max_angular_vel")]
    pub(super) max_angular_vel: f32,
    /// Mirrors `TeleopConfig::acceleration` - `None` (the natural absent-value default) means no
    /// override is configured, in which case the simulator subprocess falls back to `model.json`.
    #[serde(default)]
    pub(super) acceleration: Option<f32>,
}

/// Matches `TeleopConfig`'s own `default_max_linear`.
fn default_max_linear_vel() -> f32 {
    10.0
}

/// Matches `TeleopConfig`'s own `default_max_angular`.
fn default_max_angular_vel() -> f32 {
    5.0
}

impl Default for TeleopPhysicsLimits {
    fn default() -> Self {
        Self {
            max_linear_vel: default_max_linear_vel(),
            max_angular_vel: default_max_angular_vel(),
            acceleration: None,
        }
    }
}

/// Node configs are optionally wrapped per-environment (`{"dev": {...}, "production": {...}}`);
/// see `handlers/dev/ops/models.rs::EnvironmentNodeConfig` for the same convention.
#[derive(Deserialize)]
struct EnvironmentAwareTeleopConfig {
    dev: Option<TeleopPhysicsLimits>,
    production: Option<TeleopPhysicsLimits>,
}

/// Resolve the teleop node's configured velocity limits from its project config file.
///
/// Two consumers rely on this:
/// - `enter_teleop_mode` below, so the CLI's per-keypress teleop UI commands the same limits the
///   teleop node itself will enforce.
/// - `ops::spawn`'s `maybe_inject_teleop_locomotion_env` (LAB-1977), which forwards a *real*
///   (non-fallback) result on into the `simulator` node's subprocess env as
///   `MECHA10_LOCOMOTION_MAX_LINEAR_SPEED`/`MECHA10_LOCOMOTION_MAX_ANGULAR_SPEED` - see that
///   function's docs for why the fallback-vs-real distinction matters there. That's why this
///   returns `Option` rather than always falling back internally: `None` here means "no project
///   teleop config was actually found and parsed", which callers other than `enter_teleop_mode`
///   may need to treat differently than "found a config, here are its limits".
///
/// `None` when no config file is found or it fails to parse - this must never be fatal for
/// `enter_teleop_mode`'s own use (see that call site's `unwrap_or_default()`), since teleop
/// should still be usable with defaults.
pub(super) fn resolve_teleop_velocity_limits() -> Option<TeleopPhysicsLimits> {
    let candidate_paths = crate::paths::config::node_config_candidates("teleop");

    let profile = std::env::var("MECHA10_ENVIRONMENT").unwrap_or_else(|_| "dev".to_string());

    for path in &candidate_paths {
        let path = Path::new(path);
        if !path.exists() {
            continue;
        }

        let content = match std::fs::read_to_string(path) {
            Ok(c) => c,
            Err(_) => continue,
        };

        // Try environment-aware format first (the template default)...
        if let Ok(env_config) = serde_json::from_str::<EnvironmentAwareTeleopConfig>(&content) {
            let chosen = match profile.as_str() {
                "production" | "prod" => env_config.production,
                _ => env_config.dev,
            };
            if let Some(limits) = chosen {
                return Some(limits);
            }
        }

        // ...fall back to a flat (non-environment-scoped) config.
        if let Ok(limits) = serde_json::from_str::<TeleopPhysicsLimits>(&content) {
            return Some(limits);
        }
    }

    None
}

/// Enter teleop mode
///
/// # Arguments
///
/// * `config` - Project configuration
/// * `registry` - Node PID/failure registry
/// * `redis_url` - Redis connection URL
/// * `running` - Shared running flag for clean shutdown
/// * `log_buffer` - Log buffer for TUI logs
/// * `spawn_failure` - If the caller just ran a lifecycle mode change that was
///   supposed to start teleop (e.g. pressing `s` to enter simulation mode,
///   which reuses this UI once simulator + teleop are up) and teleop was
///   among the nodes that failed to spawn, the `(node_name, error)` pair
///   for that failure. `None` means either no mode change just ran, or it
///   ran and teleop wasn't one of the nodes it attempted to start — in
///   both cases a plain "not running" is the right message, not a
///   spawn-failure one.
///
/// # Returns
///
/// The `ScreenOutcome` the teleop screen exited with (`ScreenOutcome::ReturnToParent` if the
/// screen never actually ran — teleop not configured/not running — since there's nothing to
/// unwind beyond showing the message above). The caller knows whether it reached this via `'s'`
/// (simulation hand-off) or `'t'` (standalone teleop) and interprets `ReturnToParent`
/// accordingly — see `handle_simulation_key`/`handle_teleop_key` in
/// `ops/interactive/keys.rs`.
pub async fn enter_teleop_mode(
    config: &ProjectConfig,
    registry: &NodeRegistry,
    redis_url: &str,
    running: &Arc<AtomicBool>,
    log_buffer: &LogBuffer,
    spawn_failure: Option<&(String, String)>,
) -> Result<ScreenOutcome> {
    let has_teleop = config.nodes.contains("teleop");
    // `registry` is keyed by the full scoped node identifier (e.g.
    // `@mecha10/teleop`), not the bare short name — strip any `@scope/`
    // prefix before comparing. See `super::strip_node_scope`.
    let teleop_running = registry
        .pids()
        .keys()
        .any(|name| super::strip_node_scope(name) == "teleop");

    if !has_teleop {
        println!("\n⚠️  Teleop node not found in project");
        println!();
        println!("To add teleop to your project:");
        println!("  mecha10 add teleop");
        println!();
        println!("The teleop coordination node manages multiple");
        println!("control sources with priority arbitration.");
        println!();
        // The 't' handler re-enters the alternate-screen TUI and redraws
        // immediately after this returns, which wipes whatever was just
        // printed to the normal screen buffer before the user can read it
        // (pressing 't' otherwise looks like it silently did nothing). Push
        // to the persistent log_buffer too so the warning survives in the
        // TUI's "System Logs" panel, matching how handle_simulation_key's
        // "no simulation mode configured" case already does this.
        log_buffer.push("TELEOP: Teleop node not found in project (run `mecha10 add teleop`)".to_string());
        return Ok(ScreenOutcome::ReturnToParent);
    } else if !teleop_running {
        if let Some((_, err)) = spawn_failure {
            // teleop IS configured, and the mode change we just ran DID try
            // to start it — it just failed. Telling the user to add it to
            // dev nodes here would be actively wrong, since it's already
            // there; show the actual spawn error instead.
            println!("\n⚠️  Teleop node failed to start");
            println!();
            println!("It's configured in mecha10.json and mecha10 just tried to start it");
            println!("as part of this mode change, but the spawn failed:");
            println!();
            println!("  {}", err);
            println!();
            println!("Run 'mecha10 dev teleop' directly to see the full startup output.");
            println!();
            log_buffer.push(format!("TELEOP: teleop failed to start during mode change: {}", err));
        } else {
            println!("\n⚠️  Teleop node not running");
            println!();
            println!("Start the teleop node by adding it to dev nodes:");
            println!("  mecha10 dev teleop");
            println!();
            log_buffer.push("TELEOP: Teleop node not running (run `mecha10 dev teleop`)".to_string());
        }
        return Ok(ScreenOutcome::ReturnToParent);
    }

    println!("\n🎮 Entering teleop control mode...\n");

    // Create teleop TUI with dashboard URL from project config, and velocity limits
    // resolved from the teleop node's own config so the UI's per-keypress targets match
    // what the teleop node is actually configured to accept (see
    // `resolve_teleop_velocity_limits`).
    let dashboard_url = config.environments.dashboard_url();
    let velocity_limits = resolve_teleop_velocity_limits().unwrap_or_default();
    let mut teleop_ui = TeleopUi::new(
        redis_url.to_string(),
        log_buffer.clone(),
        dashboard_url,
        velocity_limits.max_linear_vel,
        velocity_limits.max_angular_vel,
    );

    match teleop_ui.run_with_interrupt(running.clone()).await {
        Ok(outcome) => {
            println!("\n✅ Exited teleop mode\n");
            Ok(outcome)
        }
        Err(e) => {
            eprintln!("\n❌ Teleop UI error: {}\n", e);
            Ok(ScreenOutcome::ReturnToParent)
        }
    }
}

#[cfg(test)]
mod tests {
    use super::*;
    use crate::types::project::{NodesConfig, ProjectConfig, RobotConfig};
    use std::collections::HashMap as StdHashMap;
    use std::sync::atomic::AtomicBool;

    fn project_config_with_nodes(node_names: &[&str]) -> ProjectConfig {
        let mut nodes = StdHashMap::new();
        for name in node_names {
            // Framework node identifiers are namespaced (`@mecha10/<name>`);
            // `NodesConfig::contains` matches on the parsed short name.
            nodes.insert(format!("@mecha10/{}", name), "0.1.0".to_string());
        }

        ProjectConfig {
            name: "test".to_string(),
            version: "0.1.0".to_string(),
            robot: RobotConfig {
                id: "test".to_string(),
                platform: None,
                description: None,
            },
            lifecycle: None,
            simulation: None,
            nodes: NodesConfig(nodes),
            targets: None,
            behaviors: None,
            services: Default::default(),
            docker: Default::default(),
            environments: Default::default(),
        }
    }

    #[tokio::test]
    async fn not_configured_reports_add_teleop_regardless_of_spawn_failure() {
        let config = project_config_with_nodes(&["camera"]);
        let registry = NodeRegistry::new();
        let log_buffer = LogBuffer::new(100);
        let running = Arc::new(AtomicBool::new(true));

        enter_teleop_mode(&config, &registry, "redis://localhost", &running, &log_buffer, None)
            .await
            .unwrap();

        let logs = log_buffer.get_logs();
        assert!(
            logs.iter().any(|l| l.contains("not found in project")),
            "expected a 'not found in project' log entry, got: {logs:?}"
        );
    }

    #[tokio::test]
    async fn configured_but_not_running_without_spawn_failure_suggests_dev_teleop() {
        let config = project_config_with_nodes(&["teleop"]);
        let registry = NodeRegistry::new();
        let log_buffer = LogBuffer::new(100);
        let running = Arc::new(AtomicBool::new(true));

        // No spawn_failure passed — teleop was never asked to start (e.g. the
        // user pressed 't' with no lifecycle mode configured).
        enter_teleop_mode(&config, &registry, "redis://localhost", &running, &log_buffer, None)
            .await
            .unwrap();

        let logs = log_buffer.get_logs();
        assert!(
            logs.iter().any(|l| l.contains("Teleop node not running")),
            "expected a plain 'not running' log entry, got: {logs:?}"
        );
        assert!(
            !logs.iter().any(|l| l.contains("failed to start during mode change")),
            "should not claim a spawn failure when none occurred, got: {logs:?}"
        );
    }

    #[tokio::test]
    async fn running_under_scoped_pid_key_is_detected_as_running() {
        // Regression test for LAB-1883: `pids` is keyed by the full scoped
        // node identifier (e.g. `@mecha10/teleop`), exactly as
        // `spawn_nodes_by_name` inserts it — never the bare `"teleop"`
        // string. `enter_teleop_mode` must strip the scope before comparing
        // against the short node name, or a genuinely running teleop node
        // is reported as "not running".
        let config = project_config_with_nodes(&["teleop"]);
        let mut registry = NodeRegistry::new();
        registry.mark_started("@mecha10/teleop", 12345);
        let log_buffer = LogBuffer::new(100);
        let running = Arc::new(AtomicBool::new(true));

        enter_teleop_mode(&config, &registry, "redis://localhost", &running, &log_buffer, None)
            .await
            .unwrap();

        let logs = log_buffer.get_logs();
        assert!(
            !logs.iter().any(|l| l.contains("not running")),
            "a teleop pid tracked under the scoped '@mecha10/teleop' key should be recognized as \
             running, not reported as not running, got: {logs:?}"
        );
        assert!(
            !logs.iter().any(|l| l.contains("not found in project")),
            "teleop is configured, should never hit the 'not found' branch, got: {logs:?}"
        );
    }

    #[tokio::test]
    async fn configured_but_not_running_with_spawn_failure_surfaces_the_real_error() {
        let config = project_config_with_nodes(&["teleop"]);
        let registry = NodeRegistry::new();
        let log_buffer = LogBuffer::new(100);
        let running = Arc::new(AtomicBool::new(true));
        let spawn_failure = ("teleop".to_string(), "binary download failed: HTTP 404".to_string());

        enter_teleop_mode(
            &config,
            &registry,
            "redis://localhost",
            &running,
            &log_buffer,
            Some(&spawn_failure),
        )
        .await
        .unwrap();

        let logs = log_buffer.get_logs();
        assert!(
            logs.iter()
                .any(|l| l.contains("failed to start during mode change") && l.contains("HTTP 404")),
            "expected the actual spawn error to be surfaced, got: {logs:?}"
        );
        // Must not fall back to the misleading "add it to dev nodes" message —
        // teleop IS already configured and was already asked to start.
        assert!(
            !logs.iter().any(|l| l.contains("Teleop node not running (run")),
            "should not show the generic 'not running' message when the real cause is known, got: {logs:?}"
        );
    }
}