use stm32f4xx_hal::timer::{CounterUs, Event};
use stm32f4xx_hal::pac::{interrupt, Interrupt, TIM2};
use uom::si::{f64::ElectricCurrent, electric_current::ampere};
use fugit::{TimerDurationU32, KilohertzU32};
use core::marker::PhantomData;

pub struct LdCurrentOutCtrlTimer {
    target_i: ElectricCurrent,
    now_i: ElectricCurrent,
    timer: CounterUs<TIM2>,
    timeout: bool,
}
static mut LD_CURRENT_OUT_CTRL_TIMER: Option<LdCurrentOutCtrlTimer> = None;

/// This timer notifies the main loop to set the correct output current so that ld output current can ramp up/down slowly.
///     The current output slope is guaranteed to be larger but not necessarily equal to than the preset value.
impl LdCurrentOutCtrlTimer {
    const TIME_STEP_MS: TimerDurationU32<1000000> = TimerDurationU32::from_rate(KilohertzU32::from_raw(1));
    const STEP_SIZE: ElectricCurrent = ElectricCurrent {
        dimension: PhantomData,
        units: PhantomData,
        value: 0.0001,
    };

    pub fn setup(mut tim2: CounterUs<TIM2>) {
        tim2.start(LdCurrentOutCtrlTimer::TIME_STEP_MS).unwrap();
        unsafe {
            cortex_m::peripheral::NVIC::unmask(Interrupt::TIM2);
        }
        unsafe {
            LD_CURRENT_OUT_CTRL_TIMER = Some(
                LdCurrentOutCtrlTimer {
                    target_i: ElectricCurrent::new::<ampere>(0.0),
                    now_i: ElectricCurrent::new::<ampere>(0.0),
                    timer: tim2,
                    timeout: false
                }
            );
        }
    }

    fn get() -> Option<&'static mut Self> {
        unsafe { LD_CURRENT_OUT_CTRL_TIMER.as_mut() }
    }

    pub fn reset() {
        if let Some(ref mut ld_current_out_ctrl_timer ) = LdCurrentOutCtrlTimer::get() {
            ld_current_out_ctrl_timer.target_i = ElectricCurrent::new::<ampere>(0.0);
            ld_current_out_ctrl_timer.now_i = ElectricCurrent::new::<ampere>(0.0);
            ld_current_out_ctrl_timer.timeout = false;
            ld_current_out_ctrl_timer.timer.unlisten(Event::Update);
        }
    }

    pub fn set_target_i_and_listen_irq(target: ElectricCurrent, now: ElectricCurrent) {
        if let Some(ref mut ld_current_out_ctrl_timer ) = LdCurrentOutCtrlTimer::get() {
            cortex_m::interrupt::free(|_| {
                    ld_current_out_ctrl_timer.target_i = target;
                    ld_current_out_ctrl_timer.now_i = now;
                    ld_current_out_ctrl_timer.timer.listen(Event::Update);
                }
            )
        }
    }

    pub fn set_alarm() {
        if let Some(ref mut ld_current_out_ctrl_timer ) = LdCurrentOutCtrlTimer::get() {
            ld_current_out_ctrl_timer.timeout = true;
        }
    }
    pub fn clear_alarm_and_resume_listening() {
        if let Some(ref mut ld_current_out_ctrl_timer ) = LdCurrentOutCtrlTimer::get() {
            ld_current_out_ctrl_timer.timeout = false;
            ld_current_out_ctrl_timer.timer.listen(Event::Update);
        }
    }
    pub fn get_irq_status() -> Option<ElectricCurrent> {
        if let Some(ref mut ld_current_out_ctrl_timer ) = LdCurrentOutCtrlTimer::get() {
            if ld_current_out_ctrl_timer.timeout {
                return Some(ld_current_out_ctrl_timer.now_i);
            }
        }
        None
    }

    pub fn clear_irq_flag() {
        if let Some(ref mut ld_current_out_ctrl_timer ) = LdCurrentOutCtrlTimer::get() {
            ld_current_out_ctrl_timer.timer.clear_interrupt(Event::Update);
        }
    }

    pub fn update_next_i_set() -> bool {
        if let Some(ref mut ld_current_out_ctrl_timer ) = LdCurrentOutCtrlTimer::get() {
            let update = ld_current_out_ctrl_timer.now_i != ld_current_out_ctrl_timer.target_i;
            if ld_current_out_ctrl_timer.target_i > ld_current_out_ctrl_timer.now_i {
                ld_current_out_ctrl_timer.now_i = (ld_current_out_ctrl_timer.now_i + LdCurrentOutCtrlTimer::STEP_SIZE).min(ld_current_out_ctrl_timer.target_i);
            }
            else {
                ld_current_out_ctrl_timer.now_i = (ld_current_out_ctrl_timer.now_i - LdCurrentOutCtrlTimer::STEP_SIZE).max(ld_current_out_ctrl_timer.target_i);
            }
            return update
        }
        false
    }

    pub fn stop_listening() {
        if let Some(ref mut ld_current_out_ctrl_timer ) = LdCurrentOutCtrlTimer::get() {
            ld_current_out_ctrl_timer.timer.unlisten(Event::Update);
        }
    }
}

#[interrupt]
fn TIM2(){
    if LdCurrentOutCtrlTimer::update_next_i_set() {
        LdCurrentOutCtrlTimer::set_alarm();
    }
    LdCurrentOutCtrlTimer::stop_listening();
    LdCurrentOutCtrlTimer::clear_irq_flag();
}