timer.rs

use embedded_hal::timer::CountDown;
use stm32l4xx_hal::time::Hertz;
use void::Void;

pub struct Millis(pub u32);

impl From<u32> for Millis {
    fn from(ms: u32) -> Self {
        Millis(ms)
    }
}

pub struct TimerWrapper<T>
where
    T: CountDown<Time = Hertz>,
{
    inner: T,
    remaining_ms: Millis,
}

impl<T> TimerWrapper<T>
where
    T: CountDown<Time = Hertz>,
{
    pub fn new(timer: T) -> Self {
        TimerWrapper {
            inner: timer,
            remaining_ms: Millis(0),
        }
    }
}

impl<T> CountDown for TimerWrapper<T>
where
    T: CountDown<Time = Hertz>,
{
    type Time = Millis;

    fn start<M>(&mut self, count: M)
    where
        M: Into<Self::Time>,
    {
        let ms: Millis = count.into();

        self.inner.start::<Hertz>(core::cmp::min(ms.0, 1000).into());
        self.remaining_ms = Millis(ms.0.checked_sub(1000).unwrap_or_else(|| 0));
    }

    fn wait(&mut self) -> nb::Result<(), Void> {
        if self.remaining_ms.0 > 0 {
            self.inner.wait()?;
            self.inner
                .start::<Hertz>(core::cmp::min(self.remaining_ms.0, 1000).into());
            self.remaining_ms = Millis(self.remaining_ms.0.checked_sub(1000).unwrap_or_else(|| 0));
        }
        self.inner.wait()
    }
}