use serde::{Deserialize, Serialize};

///The Config for the PID system used by the controller
#[derive(Serialize, Deserialize, Debug, Default, Clone, Copy, PartialEq)]
pub struct Config {
    ///The Proportional value for the controller
    pub kp: f32,
    ///The Integral value for the controller
    pub ki: f32,
    ///The Derivative value for the controller
    pub kd: f32,
    ///The Feedforward value for the controller
    pub ff: f32,
    ///The delta time Value used for the controller
    pub dt: f32,
    ///The max accel change from the controller, 0 means no limit
    pub accel: f32,
}

///A PID, the config field is used to handle all the needed configuration.
/// This PID using pid_step() then attempts to output a value between -1.0 and 1.0 to attempt to make the "current_value" passed in, match the "set_point"
#[derive(Debug, PartialEq, Clone, Copy)]
pub struct PID {
    pub config: Config,

    integral: f32,
    prev_error: f32,

    set_point: f32,

    output: f32,
}

impl PID {
    pub fn new(config: Config) -> Self {
        PID {
            config: config,
            integral: 0.0,
            prev_error: 0.0,
            set_point: 0.0,
            output: 0.0,
        }
    }
}

impl Default for PID {
    fn default() -> Self {
        PID::new(Config {
            kp: 0.0,
            ki: 0.0,
            kd: 0.0,
            ff: 0.0,
            dt: 0.0,
            accel: 0.0,
        })
    }
}

impl PID {
    ///Calculuates a single step of the PID+FF based on the config given to it.
    ///Will output a float between -1.0 and 1.0
    ///Current value is the current "position" of the system NOT the goal
    /// Use set_point() method to change the "goal position" for the system
    pub async fn pid_step(&mut self, current_value: f32) -> f32 {
        //If the values
        let error = self.set_point - current_value;

        //If the error is equal to 0.0, just return the output from before. All of this math will just end up not affecting anything
        if error == 0.0 {
            return self.output;
        }

        //Calculate the integral
        self.integral += error * self.config.dt;
        let i = self.integral * self.config.ki;

        //Calculate the derivative in a manner that is safe for 0.0 dt
        let d;
        if self.config.dt != 0.0 {
            let derivative = (error - self.prev_error) / self.config.dt;
            d = derivative * self.config.kd;
        } else {
            d = 0.0;
        }

        //store the original error
        self.prev_error = error;

        //Calculate the feedforward
        let f = self.set_point * self.config.ff;
        //Calculate the proportional factor
        let p = error * self.config.kp;

        //Calculate and store the output
        let output = match self.config.accel {
            0.0 => (f + p + i + d) * 0.0001,
            _ => {
                let pre_output = (f + p + i + d) * 0.0001;
                let change = pre_output - self.output;
                let change = change.clamp(-self.config.accel, self.config.accel);

                let accel_output = self.output + change;
                //Validate that there is a ki value
                if self.config.ki != 0.0 {
                    let excess = pre_output - accel_output;

                    self.integral += excess / self.config.ki;
                }

                accel_output
            }
        };

        self.output = output.clamp(-1.0, 1.0);

        //Return the output information
        return self.output;
    }

    pub async fn set_point(&mut self, set_point: f32) {
        if set_point != self.set_point {
            //Update all internal fields
            self.integral = 0.0;
            self.prev_error = 0.0;

            self.set_point = set_point;
        }
    }
}