use crate::DroneState;
use crate::PidConfig;
use nalgebra as na;

pub mod acceleration;
pub mod angular_rate;
pub mod position;
pub mod rotation;
pub mod velocity;

pub use acceleration::AccelerationController;
pub use angular_rate::AngularRateController;
pub use position::PositionController;
pub use rotation::RotationController;
pub use velocity::VelocityController;

pub trait ControllerModule {
    type Input: Clone + Default;
    type Output: Clone + Default;

    fn process(&mut self, input: Self::Input, state: &DroneState, dt: f32) -> Self::Output;
}

pub struct ModuleRuntime<C>
where
    C: ControllerModule,
{
    pub module: C,
    pub target_dt: f32,
    pub accumulated_time: f32,
    pub last_output: Option<C::Output>,
}

impl<C> ModuleRuntime<C>
where
    C: ControllerModule,
{
    pub fn update(&mut self, input: C::Input, state: &DroneState, dt: f32) -> C::Output {
        self.accumulated_time += dt;

        if self.accumulated_time >= self.target_dt {
            self.accumulated_time = 0.0;

            let output = self.module.process(input, state, self.target_dt);
            self.last_output = Some(output.clone());
            output
        } else {
            self.last_output
                .as_ref()
                .unwrap_or(&C::Output::default())
                .clone()
        }
    }
    pub fn new(module: C, frequency: f32) -> Self {
        Self {
            module,
            target_dt: 1.0 / frequency,
            accumulated_time: 0.0,
            last_output: None,
        }
    }
}

#[derive(Clone, Copy, Debug, Default)]
pub struct Position(pub na::Vector3<f32>); // meters

#[derive(Clone, Copy, Debug, Default)]
pub struct Velocity(pub na::Vector3<f32>); // m/s

#[derive(Clone, Copy, Debug, Default)]
pub struct Acceleration(pub na::Vector3<f32>); // m/s

#[derive(Clone, Copy, Debug, Default)]
pub struct Rotation(pub na::Vector3<f32>); // radians

#[derive(Clone, Copy, Debug, Default)]
pub struct AngularRate(pub na::Vector3<f32>); // rad/s

#[derive(Clone, Copy, Debug, Default)]
pub struct Torque(pub na::Vector3<f32>); // control output

#[derive(Clone, Copy, Debug, Default)]
pub struct Throttle(pub f32);

pub struct PidProcessor {
    kp: na::Vector3<f32>,
    ki: na::Vector3<f32>,
    kd: na::Vector3<f32>,
    integral: na::Vector3<f32>,
    last_error: na::Vector3<f32>,
}

impl PidProcessor {
    pub fn new(c: &PidConfig) -> Self {
        Self {
            kp: c.p_vec(),
            ki: c.i_vec(),
            kd: c.d_vec(),
            integral: na::Vector3::zeros(),
            last_error: na::Vector3::zeros(),
        }
    }

    pub fn update(
        &mut self,
        target: na::Vector3<f32>,
        current: na::Vector3<f32>,
        dt: f32,
    ) -> na::Vector3<f32> {
        let error = target - current;
        self.integral += error * dt;
        let derivative = (error - self.last_error) / dt;
        self.last_error = error;

        error.component_mul(&self.kp)
            + self.integral.component_mul(&self.ki)
            + derivative.component_mul(&self.kd)
    }
}