use crate::stacked::modules::*;

pub struct AngularRateController {
    pid: PidProcessor,
}

impl AngularRateController {
    pub fn new(pid: PidProcessor) -> Self {
        Self { pid }
    }
}

impl ControllerModule for AngularRateController {
    type Input = AngularRate;
    type Output = Torque;

    fn process(&mut self, input: AngularRate, state: &DroneState, dt: f32) -> Torque {
        // Scale normalized rate command
        let target_rate = input.0;

        let output = self.pid.update(target_rate, state.angvel, dt);

        Torque(output)
    }
}