//! Sensor configuration for simulation scenes

use anyhow::Result;

use crate::sim::profile::SensorConfig;
use crate::sim::scene::GodotScene;

/// Add LiDAR sensor with configuration from mecha10.json
pub fn add_lidar_sensor_with_config(scene: &mut GodotScene, config: &SensorConfig) -> Result<()> {
    let lidar_path = "res://packages/godot-components/sensors/lidar_rl/lidar_rl.tscn";
    let lidar_id = scene.add_ext_resource("PackedScene", lidar_path);

    let mut lidar_node = crate::sim::scene::SceneNode::instance(&config.name, &lidar_id);
    lidar_node = lidar_node.with_parent("Robot/RoverAIController");

    // Build transform from mount_point and orientation
    let transform = build_transform_3d(
        &config.mount_point,
        &config.orientation,
        [0.0, 0.0, 0.25], // Default mount point
    );
    lidar_node.set_property("transform", &transform);

    // Configure RL parameters from config or use defaults
    if let Some(rl_config) = &config.rl_config {
        if let Some(num_rays) = rl_config.num_rays {
            lidar_node.set_property("num_rays", &num_rays.to_string());
        } else {
            lidar_node.set_property("num_rays", "16");
        }

        if let Some(ray_length) = rl_config.ray_length {
            lidar_node.set_property("ray_length", &ray_length.to_string());
        } else {
            lidar_node.set_property("ray_length", "10.0");
        }

        if let Some(observation) = rl_config.observation {
            lidar_node.set_property("observation_enabled", &observation.to_string());
        } else {
            lidar_node.set_property("observation_enabled", "true");
        }
    } else {
        // Default configuration
        lidar_node.set_property("num_rays", "16");
        lidar_node.set_property("ray_length", "10.0");
        lidar_node.set_property("observation_enabled", "true");
    }

    // Enable normalization for RL training (godot_rl_agents compatibility)
    lidar_node.set_property("normalize_for_rl", "true");

    scene.add_node(lidar_node);
    Ok(())
}

/// Add Camera sensor with configuration from mecha10.json
pub fn add_camera_sensor_with_config(scene: &mut GodotScene, config: &SensorConfig) -> Result<()> {
    let camera_path = "res://packages/godot-components/sensors/camera_rl/camera_rl.tscn";
    let camera_id = scene.add_ext_resource("PackedScene", camera_path);

    let mut camera_node = crate::sim::scene::SceneNode::instance(&config.name, &camera_id);
    camera_node = camera_node.with_parent("Robot/RoverAIController");

    // Build transform from mount_point and orientation
    let transform = build_transform_3d(
        &config.mount_point,
        &config.orientation,
        [0.15, 0.0, 0.2], // Default mount point
    );
    camera_node.set_property("transform", &transform);

    // Configure RL parameters from config or use defaults
    if let Some(rl_config) = &config.rl_config {
        if let Some(resolution) = &rl_config.resolution {
            let res_str = format!(
                "Vector2i({}, {})",
                resolution.first().unwrap_or(&84),
                resolution.get(1).unwrap_or(&84)
            );
            camera_node.set_property("resolution", &res_str);
        } else {
            camera_node.set_property("resolution", "Vector2i(84, 84)");
        }

        if let Some(fov) = rl_config.fov {
            camera_node.set_property("fov_degrees", &fov.to_string());
        } else {
            camera_node.set_property("fov_degrees", "90.0");
        }

        if let Some(sensor_type) = &rl_config.sensor_type {
            camera_node.set_property("sensor_type", &format!("\"{}\"", sensor_type));
        } else {
            camera_node.set_property("sensor_type", "\"rgb\"");
        }

        if let Some(update_rate) = rl_config.update_rate_hz {
            camera_node.set_property("update_rate_hz", &update_rate.to_string());
        } else {
            camera_node.set_property("update_rate_hz", "30.0");
        }

        if let Some(observation) = rl_config.observation {
            camera_node.set_property("observation_enabled", &observation.to_string());
        } else {
            camera_node.set_property("observation_enabled", "true");
        }
    } else {
        // Default configuration
        camera_node.set_property("resolution", "Vector2i(84, 84)");
        camera_node.set_property("fov_degrees", "90.0");
        camera_node.set_property("sensor_type", "\"rgb\"");
        camera_node.set_property("observation_enabled", "true");
        camera_node.set_property("update_rate_hz", "30.0");
    }

    camera_node.set_property("normalize_output", "true");
    scene.add_node(camera_node);
    Ok(())
}

/// Add IMU sensor with configuration from mecha10.json
pub fn add_imu_sensor_with_config(scene: &mut GodotScene, config: &SensorConfig) -> Result<()> {
    let imu_path = "res://packages/godot-components/sensors/imu_rl/imu_rl.tscn";
    let imu_id = scene.add_ext_resource("PackedScene", imu_path);

    let mut imu_node = crate::sim::scene::SceneNode::instance(&config.name, &imu_id);
    imu_node = imu_node.with_parent("Robot");

    // Build transform from mount_point and orientation
    let transform = build_transform_3d(
        &config.mount_point,
        &config.orientation,
        [0.0, 0.0, 0.1], // Default mount point
    );
    imu_node.set_property("transform", &transform);

    // Configure RL parameters from config or use defaults
    if let Some(rl_config) = &config.rl_config {
        if let Some(sensor_type) = &rl_config.sensor_type {
            imu_node.set_property("sensor_type", &format!("\"{}\"", sensor_type));
        } else {
            imu_node.set_property("sensor_type", "\"full\"");
        }

        if let Some(update_rate) = rl_config.update_rate_hz {
            imu_node.set_property("update_rate_hz", &update_rate.to_string());
        } else {
            imu_node.set_property("update_rate_hz", "100.0");
        }

        if let Some(observation) = rl_config.observation {
            imu_node.set_property("observation_enabled", &observation.to_string());
        } else {
            imu_node.set_property("observation_enabled", "true");
        }
    } else {
        // Default configuration
        imu_node.set_property("sensor_type", "\"full\"");
        imu_node.set_property("update_rate_hz", "100.0");
        imu_node.set_property("observation_enabled", "true");
    }

    imu_node.set_property("normalize_output", "true");
    imu_node.set_property("enable_orientation", "true");
    imu_node.set_property("enable_gyroscope", "true");
    imu_node.set_property("enable_accelerometer", "true");

    scene.add_node(imu_node);
    Ok(())
}

/// Build a Godot Transform3D string from mount point and orientation (Euler angles)
///
/// # Arguments
/// * `mount_point` - Optional [x, y, z] position
/// * `orientation` - Optional [pitch, yaw, roll] in radians
/// * `default_mount` - Default [x, y, z] if mount_point is None
///
/// # Returns
/// String formatted as "Transform3D(r11, r12, r13, r21, r22, r23, r31, r32, r33, x, y, z)"
pub fn build_transform_3d(
    mount_point: &Option<Vec<f32>>,
    orientation: &Option<Vec<f32>>,
    default_mount: [f32; 3],
) -> String {
    // Extract position
    let (x, y, z) = if let Some(mp) = mount_point {
        (
            mp.first().copied().unwrap_or(default_mount[0]),
            mp.get(1).copied().unwrap_or(default_mount[1]),
            mp.get(2).copied().unwrap_or(default_mount[2]),
        )
    } else {
        (default_mount[0], default_mount[1], default_mount[2])
    };

    // Extract orientation (Euler angles: pitch, yaw, roll)
    let (pitch, yaw, roll) = if let Some(orient) = orientation {
        (
            orient.first().copied().unwrap_or(0.0),
            orient.get(1).copied().unwrap_or(0.0),
            orient.get(2).copied().unwrap_or(0.0),
        )
    } else {
        (0.0, 0.0, 0.0)
    };

    // Convert Euler angles to rotation matrix
    // Rotation order: Z (yaw) * Y (pitch) * X (roll)
    let (sp, cp) = (pitch.sin(), pitch.cos());
    let (sy, cy) = (yaw.sin(), yaw.cos());
    let (sr, cr) = (roll.sin(), roll.cos());

    // Combined rotation matrix
    let r11 = cy * cr - sy * sp * sr;
    let r12 = -cy * sr - sy * sp * cr;
    let r13 = -sy * cp;

    let r21 = cp * sr;
    let r22 = cp * cr;
    let r23 = -sp;

    let r31 = sy * cr + cy * sp * sr;
    let r32 = -sy * sr + cy * sp * cr;
    let r33 = cy * cp;

    format!(
        "Transform3D({}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {})",
        r11, r12, r13, r21, r22, r23, r31, r32, r33, x, y, z
    )
}
