use embedded_hal::digital::v2::PinState;

pub use fixed::types::I16F16 as FixedI32;

pub use fixed_macro::types::I16F16 as FixedI32;

#[derive(Clone, Copy)]
pub struct PidConfig<const FORWARD_HIGH: bool> {
    d_gain: FixedI32,
    i_gain: FixedI32,
    i_max: FixedI32,
    i_min: FixedI32,
    p_gain: FixedI32,
}

impl<const FORWARD_HIGH: bool> PidConfig<FORWARD_HIGH> {
    pub fn new() -> Self {
        Self {
            d_gain: FixedI32::ZERO,
            i_gain: FixedI32::ZERO,
            i_max: FixedI32::ZERO,
            i_min: FixedI32::ZERO,
            p_gain: FixedI32::ZERO,
        }
    }

    pub fn d_gain(mut self, d_gain: FixedI32) -> Self {
        self.d_gain = d_gain;
        self
    }

    pub fn i_gain(mut self, i_gain: FixedI32) -> Self {
        self.i_gain = i_gain;
        self
    }

    pub fn i_max(mut self, i_max: FixedI32) -> Self {
        self.i_max = i_max;
        self
    }

    pub fn i_min(mut self, i_min: FixedI32) -> Self {
        self.i_min = i_min;
        self
    }

    pub fn p_gain(mut self, p_gain: FixedI32) -> Self {
        self.p_gain = p_gain;
        self
    }
}

use inner::{Config, Dir};

pub struct Pid<'a, CONFIG, const FORWARD_HIGH: bool>
where
    CONFIG: Config<FORWARD_HIGH> + Dir,
{
    config: &'a CONFIG,
    d_state: FixedI32,
    i_state: FixedI32,
}

impl<'a, CONFIG, const FORWARD_HIGH: bool> Pid<'a, CONFIG, FORWARD_HIGH>
where
    CONFIG: Config<FORWARD_HIGH> + Dir,
{
    pub fn new(config: &'a CONFIG, initial_position: u16) -> Self {
        Self {
            config,
            d_state: FixedI32::saturating_from_num(initial_position),
            i_state: FixedI32::ZERO,
        }
    }

    pub fn update(&mut self, error: i16, position: u16) -> (u16, PinState) {
        let config = self.config.config();
        let error = FixedI32::from_num(error);
        let position = FixedI32::saturating_from_num(position);

        let p = config.p_gain * error;

        self.i_state = match self.i_state + error {
            i if i > config.i_max => config.i_max,
            i if i < config.i_min => config.i_min,
            i => i,
        };

        let i = config.i_gain * self.i_state;

        let d = config.d_gain * (self.d_state - position);
        self.d_state = position;

        let output = (p + i + d).round();
        return (
            output.abs().saturating_to_num::<u16>(),
            self.config.dir(output),
        );
    }
}

mod inner {
    use embedded_hal::digital::v2::PinState;

    use fixed::types::I16F16;

    use super::PidConfig;

    pub trait Config<const FORWARD_HIGH: bool> {
        fn config(&self) -> &PidConfig<FORWARD_HIGH>;
    }

    impl<const FORWARD_HIGH: bool> Config<FORWARD_HIGH> for PidConfig<FORWARD_HIGH> {
        fn config(&self) -> &PidConfig<FORWARD_HIGH> {
            self
        }
    }

    pub trait Dir {
        fn dir(&self, output: I16F16) -> PinState;
    }

    impl Dir for PidConfig<false> {
        fn dir(&self, output: I16F16) -> PinState {
            //! If FORWARD_HIGH is false, return PinState::High for reverse direction
            PinState::from(output.is_negative())
        }
    }

    impl Dir for PidConfig<true> {
        fn dir(&self, output: I16F16) -> PinState {
            //! If FORWARD_HIGH is true, return PinState::High for forward direction
            PinState::from(output.is_positive())
        }
    }
}