use rapier3d::prelude::*;
use serde::Deserialize;

#[derive(Debug, Deserialize, Clone)]
pub struct PidConfig {
    pub kp: [f32; 3],
    pub ki: [f32; 3],
    pub kd: [f32; 3],
}

#[derive(Debug, Deserialize, Clone)]
#[serde(tag = "type")]
pub enum LayerConfig {
    /// Controls angular velocity (Input: joystick/previous layer -> Output: torque)
    Rate {
        #[serde(flatten)]
        pid: PidConfig,
        max_rate: f32,
        frequency: f32,
    },
    /// Controls orientation (Input: joystick -> Output: desired angular velocity)
    Angle {
        #[serde(flatten)]
        pid: PidConfig,
        max_angle: f32,
        frequency: f32,
    },
}

#[derive(Debug, Deserialize, Clone)]
pub struct SimulationConfig {
    pub tickrate: f32,
    pub drone_tick_rate: u64,

    // --- Modular Controller Stack ---
    // The order of this Vec defines the stack (e.g., [Angle, Rate])
    pub layers: Vec<LayerConfig>,

    /// Maps [Roll, Yaw, Pitch] to each of the 4 motors
    pub motor_map: [[f32; 3]; 4],

    // Motors & Physics
    pub max_thrust: f32,
    pub max_torque: f32,

    #[serde(default)]
    pub time_constant: f32,
    pub mass: f32,
}

impl PidConfig {
    pub fn p_vec(&self) -> Vector<f32> {
        vector![self.kp[0], self.kp[1], self.kp[2]]
    }
    pub fn i_vec(&self) -> Vector<f32> {
        vector![self.ki[0], self.ki[1], self.ki[2]]
    }
    pub fn d_vec(&self) -> Vector<f32> {
        vector![self.kd[0], self.kd[1], self.kd[2]]
    }
}