mecha10-cli 0.1.47

//! Robot scene (.tscn) generator
//!
//! Generates standalone robot .tscn files from mecha10.json configuration.
//! The robot scene includes:
//! - CharacterBody3D root (physics body)
//! - CollisionShape3D (capsule collision)
//! - MeshInstance3D (visual representation)
//! - Sensor nodes (Camera3D, RayCast3D for LiDAR, etc.)

use anyhow::{Context, Result};
use std::fs;
use std::path::Path;

use super::profile::RobotProfile;

/// Robot scene generator
pub struct RobotGenerator {
    profile: RobotProfile,
    /// Enable RL training mode (adds AIController, uses RL-compatible sensors)
    rl_mode: bool,
}

impl RobotGenerator {
    /// Create new robot generator from config file
    pub fn from_config_file(config_path: &Path) -> Result<Self> {
        let profile = RobotProfile::from_config_file(config_path)?;
        Ok(Self {
            profile,
            rl_mode: true, // Default to RL mode for now
        })
    }

    /// Create robot generator with explicit RL mode setting
    pub fn from_config_file_with_mode(config_path: &Path, rl_mode: bool) -> Result<Self> {
        let profile = RobotProfile::from_config_file(config_path)?;
        Ok(Self { profile, rl_mode })
    }

    /// Generate robot .tscn file
    pub fn generate(&self, output_path: &Path) -> Result<()> {
        println!("🤖 Generating robot scene...");
        println!("   Platform: {}", self.profile.platform);
        println!("   Sensors: {}", self.profile.sensors.join(", "));

        // Create output directory
        if let Some(parent) = output_path.parent() {
            fs::create_dir_all(parent)
                .with_context(|| format!("Failed to create output directory: {}", parent.display()))?;
        }

        // Generate .tscn content
        let tscn_content = self.generate_tscn();

        // Write to file
        fs::write(output_path, tscn_content)
            .with_context(|| format!("Failed to write robot scene: {}", output_path.display()))?;

        println!("   ✅ Generated: {}", output_path.display());

        Ok(())
    }

    /// Generate .tscn file content
    fn generate_tscn(&self) -> String {
        let mut lines = Vec::new();

        // Header (more load steps if RL mode for scripts)
        let load_steps = if self.rl_mode { 5 } else { 3 };
        lines.push(format!("[gd_scene load_steps={} format=3]", load_steps));
        lines.push("".to_string());

        // External script resources (if RL mode)
        if self.rl_mode {
            lines.push(
                "[ext_resource type=\"Script\" path=\"res://components/robot_templates/rover_base_rl.gd\" id=\"1\"]"
                    .to_string(),
            );
            lines.push(
                "[ext_resource type=\"Script\" path=\"res://components/controllers/rover_ai_controller.gd\" id=\"2\"]"
                    .to_string(),
            );
            lines.push("".to_string());
        }

        // Sub-resources (collision shape, mesh)
        lines.push("[sub_resource type=\"CapsuleShape3D\" id=\"CapsuleShape3D_1\"]".to_string());
        lines.push(format!("radius = {}", self.get_robot_radius()));
        lines.push(format!("height = {}", self.get_robot_height()));
        lines.push("".to_string());

        lines.push("[sub_resource type=\"CapsuleMesh\" id=\"CapsuleMesh_1\"]".to_string());
        lines.push(format!("radius = {}", self.get_robot_radius()));
        lines.push(format!("height = {}", self.get_robot_height()));
        lines.push("".to_string());

        // Root node - CharacterBody3D with RoverBaseRL script if RL mode
        lines.push("[node name=\"Robot\" type=\"CharacterBody3D\"]".to_string());
        if self.rl_mode {
            lines.push("script = ExtResource(\"1\")".to_string());
        }
        lines.push("".to_string());

        // Collision shape
        lines.push("[node name=\"CollisionShape3D\" type=\"CollisionShape3D\" parent=\".\"]".to_string());
        lines.push("shape = SubResource(\"CapsuleShape3D_1\")".to_string());
        lines.push("".to_string());

        // Visual mesh
        lines.push("[node name=\"MeshInstance3D\" type=\"MeshInstance3D\" parent=\".\"]".to_string());
        lines.push("mesh = SubResource(\"CapsuleMesh_1\")".to_string());
        lines.push("".to_string());

        // AIController (if RL mode) - must be direct child of robot
        if self.rl_mode {
            lines.push("[node name=\"AIController3D\" type=\"Node3D\" parent=\".\"]".to_string());
            lines.push("script = ExtResource(\"2\")".to_string());
            lines.push("".to_string());
        }

        // Add sensor nodes (attached to AIController in RL mode, or to robot directly)
        self.add_sensor_nodes(&mut lines);

        // Markers container
        lines.push("[node name=\"Markers\" type=\"Node3D\" parent=\".\"]".to_string());
        lines.push("".to_string());

        // Center of mass marker
        lines.push("[node name=\"CenterOfMass\" type=\"Marker3D\" parent=\"Markers\"]".to_string());
        lines.push("".to_string());

        lines.join("\n")
    }

    /// Add sensor nodes based on robot profile
    fn add_sensor_nodes(&self, lines: &mut Vec<String>) {
        // Sensors are attached to robot root in RL mode (siblings of AIController)
        let parent = ".";

        // Check for camera
        if self.profile.sensors.contains(&"camera".to_string()) {
            if self.rl_mode {
                // Use CameraRL with script
                lines.push(format!(
                    "[node name=\"CameraRL\" type=\"Node3D\" parent=\"{}\"]",
                    parent
                ));
                lines.push("script = ExtResource(\"res://components/sensors/camera_rl/camera_rl.gd\")".to_string());
                lines.push(format!(
                    "transform = Transform3D(1, 0, 0, 0, 1, 0, 0, 0, 1, 0, {}, {})",
                    self.get_robot_height() / 2.0 - 0.1,
                    0.3
                ));
                lines.push("resolution = Vector2i(84, 84)".to_string());
                lines.push("fov_degrees = 90.0".to_string());
                lines.push("observation_enabled = true".to_string());
                lines.push("".to_string());
            } else {
                // Simple Camera3D
                lines.push(format!(
                    "[node name=\"Camera\" type=\"Camera3D\" parent=\"{}\"]",
                    parent
                ));
                lines.push(format!(
                    "transform = Transform3D(1, 0, 0, 0, 1, 0, 0, 0, 1, 0, {}, {})",
                    self.get_robot_height() / 2.0 - 0.1,
                    0.3
                ));
                lines.push("fov = 60.0".to_string());
                lines.push("".to_string());
            }
        }

        // Check for lidar
        if self.profile.sensors.contains(&"lidar".to_string()) {
            if self.rl_mode {
                // Use LidarRL with script
                lines.push(format!("[node name=\"LidarRL\" type=\"Node3D\" parent=\"{}\"]", parent));
                lines.push("script = ExtResource(\"res://components/sensors/lidar_rl/lidar_rl.gd\")".to_string());
                lines.push(format!(
                    "transform = Transform3D(1, 0, 0, 0, 1, 0, 0, 0, 1, 0, {}, 0)",
                    self.get_robot_height() / 2.0
                ));
                lines.push("num_rays = 16".to_string());
                lines.push("ray_length = 10.0".to_string());
                lines.push("observation_enabled = true".to_string());
                lines.push("".to_string());
            } else {
                // Basic Node3D with RayCast3D children
                lines.push(format!("[node name=\"LiDAR\" type=\"Node3D\" parent=\"{}\"]", parent));
                lines.push(format!(
                    "transform = Transform3D(1, 0, 0, 0, 1, 0, 0, 0, 1, 0, {}, 0)",
                    self.get_robot_height() / 2.0
                ));
                lines.push("".to_string());

                // Add RayCast3D nodes
                for i in 0..36 {
                    let angle_deg = i * 10;
                    let angle_rad = (angle_deg as f32).to_radians();
                    let x = angle_rad.cos();
                    let z = angle_rad.sin();

                    lines.push(format!(
                        "[node name=\"Ray{}\" type=\"RayCast3D\" parent=\"{}/LiDAR\"]",
                        i, parent
                    ));
                    lines.push(format!("target_position = Vector3({}, 0, {})", x * 10.0, z * 10.0));
                    lines.push("enabled = true".to_string());
                    lines.push("".to_string());
                }
            }
        }

        // Check for IMU
        if self.profile.sensors.contains(&"imu".to_string()) {
            // IMU is just a marker in both modes (data comes from physics)
            lines.push(format!("[node name=\"IMU\" type=\"Marker3D\" parent=\"{}\"]", parent));
            lines.push("".to_string());
        }
    }

    /// Get robot radius based on platform
    fn get_robot_radius(&self) -> f32 {
        match self.profile.platform.as_str() {
            "differential-drive" => 0.3, // Small rover
            "holonomic" => 0.35,         // Slightly larger
            "legged" => 0.25,            // Narrower
            "arm" => 0.15,               // Thin manipulator
            _ => 0.3,                    // Default
        }
    }

    /// Get robot height based on platform
    fn get_robot_height(&self) -> f32 {
        match self.profile.platform.as_str() {
            "differential-drive" => 1.0, // Rover height
            "holonomic" => 1.2,          // Taller
            "legged" => 1.5,             // Humanoid height
            "arm" => 2.0,                // Tall manipulator
            _ => 1.0,                    // Default
        }
    }
}