use nalgebra::{vector, Vector3};
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],
    pub frequency: f32,
}

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

/// Now each layer is explicitly typed
#[derive(Debug, Deserialize, Clone)]
pub struct ControllerStackConfig {
    /// PID for the rotation (angle → angular rate) layer
    pub rotation_pid: PidConfig,

    pub acceleration_pid: PidConfig,

    /// PID for the angular rate (angular rate → torque) layer
    pub rate_pid: PidConfig,
    /// Maximum angular rate (rad/s) that joystick input maps to
    pub max_rate: f32,
}

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

    /// Controller stack
    pub stack: ControllerStackConfig,

    /// 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,
}