mecha10-sim 0.8.2

//! Robot profile extraction from mecha10.json

use anyhow::Result;
use serde::{Deserialize, Serialize};
use std::collections::HashSet;
use std::path::Path;

#[derive(Debug, Clone, Serialize, Deserialize)]
pub struct RobotProfile {
    pub platform: String,
    pub sensors: Vec<String>,
    pub sensor_configs: Vec<SensorConfig>,
    pub task_nodes: Vec<TaskNode>,
    pub capabilities: Vec<String>,
    pub difficulty_preference: String,
}

#[derive(Debug, Clone, Serialize, Deserialize)]
pub struct SensorConfig {
    pub name: String,
    pub sensor_type: String,
    pub mount_point: Option<Vec<f32>>,
    pub orientation: Option<Vec<f32>>,
    pub rl_config: Option<RLConfig>,
}

#[derive(Debug, Clone, Serialize, Deserialize)]
pub struct RLConfig {
    pub observation: Option<bool>,
    pub num_rays: Option<i32>,
    pub ray_length: Option<f32>,
    pub resolution: Option<Vec<i32>>,
    pub fov: Option<f32>,
    pub sensor_type: Option<String>,
    pub update_rate_hz: Option<f32>,
}

#[derive(Debug, Clone, Serialize, Deserialize)]
pub struct TaskNode {
    pub name: String,
    pub description: String,
    pub tags: Vec<String>,
}

#[derive(Debug, Deserialize)]
struct Mecha10Config {
    #[serde(flatten)]
    robot: RobotConfig,
    #[serde(default)]
    drivers: Vec<DriverConfig>,
    #[serde(default)]
    nodes: NodesConfig,
    #[serde(default)]
    simulation: Option<SimulationConfig>,
}

#[derive(Debug, Deserialize)]
struct RobotConfig {
    #[serde(rename = "robot_platform", default = "default_platform")]
    platform: String,
}

fn default_platform() -> String {
    "rover".to_string()
}

#[derive(Debug, Deserialize)]
struct DriverConfig {
    #[serde(rename = "type")]
    driver_type: String,
    #[serde(default)]
    name: String,
    #[serde(default)]
    physics: Option<PhysicsConfig>,
    #[serde(default)]
    rl: Option<RLConfig>,
}

#[derive(Debug, Deserialize)]
struct PhysicsConfig {
    #[serde(default)]
    mount_point: Option<Vec<f32>>,
    #[serde(default)]
    orientation: Option<Vec<f32>>,
}

#[derive(Debug, Deserialize, Default)]
#[serde(transparent)]
struct NodesConfig(Vec<String>);

impl NodesConfig {
    /// Extract project node names (plain `<name>` identifiers)
    fn project_node_names(&self) -> Vec<String> {
        self.0
            .iter()
            .filter_map(|id| {
                let spec = mecha10_cli_core::types::NodeSpec::parse(id).ok()?;
                if spec.source == mecha10_cli_core::types::NodeSource::Project {
                    Some(spec.name)
                } else {
                    None
                }
            })
            .collect()
    }
}

#[derive(Debug, Deserialize)]
struct SimulationConfig {
    #[serde(default = "default_difficulty")]
    difficulty_preference: String,
}

fn default_difficulty() -> String {
    "medium".to_string()
}

impl RobotProfile {
    /// Extract robot profile from mecha10.json file
    pub fn from_config_file(path: &Path) -> Result<Self> {
        let config: Mecha10Config = super::load_json(path, "config file")?;

        Ok(Self::from_config(config))
    }

    /// Extract robot profile from parsed config
    fn from_config(config: Mecha10Config) -> Self {
        let platform = config.robot.platform;

        let sensors: Vec<String> = config
            .drivers
            .iter()
            .map(|d| d.driver_type.clone())
            .collect::<HashSet<_>>()
            .into_iter()
            .collect();

        let sensor_configs: Vec<SensorConfig> = config
            .drivers
            .iter()
            .filter(|d| d.driver_type != "motor")
            .map(|d| {
                let mount_point = d.physics.as_ref().and_then(|p| p.mount_point.clone());
                let orientation = d.physics.as_ref().and_then(|p| p.orientation.clone());

                SensorConfig {
                    name: if d.name.is_empty() {
                        format!("{}_{}", d.driver_type, "default")
                    } else {
                        d.name.clone()
                    },
                    sensor_type: d.driver_type.clone(),
                    mount_point,
                    orientation,
                    rl_config: d.rl.clone(),
                }
            })
            .collect();

        let task_nodes: Vec<TaskNode> = config
            .nodes
            .project_node_names()
            .into_iter()
            .map(|name| {
                let tags = extract_tags_from_node(&name, "");
                TaskNode {
                    name,
                    description: String::new(),
                    tags,
                }
            })
            .collect();

        let capabilities = infer_capabilities(&task_nodes, &sensors);

        let difficulty_preference = config
            .simulation
            .as_ref()
            .map(|s| s.difficulty_preference.clone())
            .unwrap_or_else(|| "medium".to_string());

        Self {
            platform,
            sensors,
            sensor_configs,
            task_nodes,
            capabilities,
            difficulty_preference,
        }
    }
}

/// Extract tags from node name and description
fn extract_tags_from_node(name: &str, description: &str) -> Vec<String> {
    let mut tags = HashSet::new();

    let name_lower = name.to_lowercase();
    let desc_lower = description.to_lowercase();

    if name_lower.contains("task") || name_lower.contains("planner") {
        tags.insert("task".to_string());
        tags.insert("planner".to_string());
    }
    if name_lower.contains("delivery") {
        tags.insert("delivery".to_string());
        tags.insert("navigation".to_string());
    }
    if name_lower.contains("follow") {
        tags.insert("following".to_string());
        tags.insert("tracking".to_string());
        tags.insert("hri".to_string());
    }
    if name_lower.contains("object") && name_lower.contains("detector") {
        tags.insert("object".to_string());
        tags.insert("detection".to_string());
        tags.insert("vision".to_string());
    }
    if name_lower.contains("patrol") {
        tags.insert("patrol".to_string());
        tags.insert("security".to_string());
        tags.insert("monitoring".to_string());
    }
    if name_lower.contains("manipulation") || name_lower.contains("gripper") {
        tags.insert("manipulation".to_string());
        tags.insert("gripper".to_string());
        tags.insert("arm".to_string());
    }
    if name_lower.contains("person") || name_lower.contains("human") {
        tags.insert("hri".to_string());
        tags.insert("person".to_string());
    }
    if name_lower.contains("tray") {
        tags.insert("delivery".to_string());
        tags.insert("waiter".to_string());
    }

    if desc_lower.contains("delivery") || desc_lower.contains("waiter") {
        tags.insert("delivery".to_string());
        tags.insert("service".to_string());
    }
    if desc_lower.contains("navigation") || desc_lower.contains("navigate") {
        tags.insert("navigation".to_string());
    }
    if desc_lower.contains("avoid") {
        tags.insert("avoidance".to_string());
    }
    if desc_lower.contains("follow") || desc_lower.contains("tracking") {
        tags.insert("following".to_string());
        tags.insert("tracking".to_string());
    }
    if desc_lower.contains("person") || desc_lower.contains("human") {
        tags.insert("hri".to_string());
        tags.insert("person".to_string());
    }
    if desc_lower.contains("object") {
        tags.insert("object".to_string());
    }
    if desc_lower.contains("detection") || desc_lower.contains("detect") {
        tags.insert("detection".to_string());
    }
    if desc_lower.contains("workflow") {
        tags.insert("workflow".to_string());
    }

    tags.into_iter().collect()
}

/// Infer robot capabilities from task nodes and sensors
fn infer_capabilities(task_nodes: &[TaskNode], sensors: &[String]) -> Vec<String> {
    let mut capabilities = HashSet::new();

    let has_camera = sensors.iter().any(|s| s == "camera");
    let has_lidar = sensors.iter().any(|s| s == "lidar");
    let has_gripper = sensors.iter().any(|s| s == "gripper");
    let has_imu = sensors.iter().any(|s| s == "imu");

    if has_camera && has_lidar {
        capabilities.insert("vision_navigation".to_string());
    }
    if has_camera {
        capabilities.insert("vision".to_string());
        capabilities.insert("object_detection".to_string());
    }
    if has_lidar {
        capabilities.insert("navigation".to_string());
        capabilities.insert("obstacle_avoidance".to_string());
    }
    if has_gripper {
        capabilities.insert("manipulation".to_string());
    }
    if has_imu {
        capabilities.insert("stabilization".to_string());
    }

    for node in task_nodes {
        if node.name.contains("planner") {
            capabilities.insert("planning".to_string());
        }
        if node.name.contains("controller") {
            capabilities.insert("control".to_string());
        }
        if node.tags.contains(&"delivery".to_string()) {
            capabilities.insert("delivery".to_string());
        }
        if node.tags.contains(&"following".to_string()) {
            capabilities.insert("following".to_string());
        }
    }

    capabilities.into_iter().collect()
}