use crate::core::motor;
use crate::core::motor::{Direction, Speed};

use crate::core::logic::pid;

#[derive(Debug, Clone, Copy, PartialEq)]
/// # Motor Controller
/// This is a motor controller that takes in a generic type T that has implemented the needed traits.
/// Most specifically Driver and Sensor.
///
/// ## Usage
///
/// Creating this allows for the controller a motor via a simple PID f32 signed Speed command, rather than u16 speed commands
pub struct Controller<T>
where
    T: motor::Driver + motor::Sensor,
{
    pub motor: T,
    max_speed: Speed,
    pid: pid::PID,
    current_direction: Direction,
}

///Controller Impl for the adding the PID logic for handling the motor compiston
impl<T: motor::Driver + motor::Sensor> Controller<T> {
    ///Uses the given set_speed and then will handle the rest of the control logic to accurately* hit the requested speed
    pub fn control(&mut self, set_speed: Speed) {
        let max_speed_rads = self.max_speed.rads();
        let set_speed_rads = set_speed.rads().clamp(-max_speed_rads, max_speed_rads);

        self.pid.set_point(set_speed_rads);
        let pid_output = self.pid.pid_step(self.motor.get_speed().rads());

        self.current_direction.dir_from_f32(pid_output);

        let motor_command = (pid_output * 65535.0) as u16;

        self.motor
            .set_speed_and_direction(motor_command, self.current_direction);
    }
}