use embassy_stm32::{
    pac,
    peripherals::{PA0, PA1, PA6, PA7, TIM2, TIM3},
    timer::qei::{Direction, Qei, QeiPin},
};

pub struct Encoders<'a> {
    left: Qei<'a, TIM3>,
    right: Qei<'a, TIM2>,
}

pub struct EncoderValue {
    count: u16,
    direction: Direction,
}

pub struct Relative {
    count_left: u16,
    count_right: u16,
}

impl core::fmt::Debug for EncoderValue {
    fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result {
        write!(
            f,
            "{} ({})",
            self.count,
            if matches!(self.direction, Direction::Upcounting) {
                "upcounting"
            } else {
                "downcounting"
            }
        )
    }
}

impl Encoders<'_> {
    pub fn new(pa0: PA0, pa1: PA1, pa6: PA6, pa7: PA7, tim2: TIM2, tim3: TIM3) -> Self {
        let left = Qei::new(tim3, QeiPin::new_ch1(pa6), QeiPin::new_ch2(pa7));
        let right = Qei::new(tim2, QeiPin::new_ch1(pa0), QeiPin::new_ch2(pa1));
        pac::TIM2.ccer().modify(|w| w.set_ccp(1, true));
        Self { left, right }
    }

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

impl Relative {
    pub fn new(encoders: &Encoders<'_>) -> Self {
        let (count_left, count_right) = (encoders.left.count(), encoders.right.count());
        Self {
            count_left,
            count_right,
        }
    }

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

#[allow(clippy::cast_possible_wrap)]
fn ticks_since(old: &mut u16, new: u16) -> i16 {
    let diff = (new - *old) as i16;
    *old = new;
    diff
}