use cortex_m::prelude::_embedded_hal_Qei;
pub use embedded_hal::Direction;
use stm32f1xx_hal::{
    afio::MAPR,
    gpio::{PA0, PA1, PA6, PA7},
    pac::{TIM2, TIM3},
    qei::{Qei, QeiOptions},
    rcc::Clocks,
    timer::{Tim2NoRemap, Tim3NoRemap, Timer},
};

pub struct Encoders {
    left: Qei<TIM3, Tim3NoRemap, (PA6, PA7)>,
    right: Qei<TIM2, Tim2NoRemap, (PA0, PA1)>,
    count_left: u16,
    count_right: u16,
}

#[derive(Debug)]
pub struct EncoderValue {
    count: u16,
    direction: Direction,
}

impl Encoders {
    pub fn new(
        pa0: PA0,
        pa1: PA1,
        pa6: PA6,
        pa7: PA7,
        tim2: TIM2,
        tim3: TIM3,
        mapr: &mut MAPR,
        clocks: &Clocks,
    ) -> Self {
        let options = QeiOptions::default();
        let qei = Self {
            left: Timer::new(tim3, clocks).qei((pa6, pa7), mapr, options),
            right: Timer::new(tim2, clocks).qei((pa0, pa1), mapr, options),
            count_left: 0,
            count_right: 0,
        };
        // Unsafe needed, see <https://github.com/stm32-rs/stm32f1xx-hal/issues/478>
        unsafe {
            stm32f1xx_hal::pac::Peripherals::steal()
                .TIM2
                .ccer
                .modify(|_, w| w.cc2p().set_bit());
        }
        qei
    }

    pub fn read(&self) -> (EncoderValue, EncoderValue) {
        (
            EncoderValue {
                count: self.left.count(),
                direction: self.left.direction(),
            },
            EncoderValue {
                count: self.right.count(),
                direction: self.right.direction(),
            },
        )
    }

    pub fn ticks(&mut self) -> (i16, i16) {
        let (left, right) = (self.left.count(), self.right.count());
        (
            ticks_since(&mut self.count_left, left),
            ticks_since(&mut self.count_right, right),
        )
    }
}

fn ticks_since(old: &mut u16, new: u16) -> i16 {
    let diff = (new - *old) as i16;
    *old = new;
    diff
}