use embedded_hal as hal;
use either::Either;
use hal::digital::v2::InputPin;

#[derive(Clone, Debug, Eq, PartialEq)]
pub struct Rotary<A, B> {
    pub pin_a: A,
    pub pin_b: B,
    prev_a: bool,
    prev_c: bool,
}

/// The encoder direction is either `Clockwise`, `CounterClockwise`, or `None`
#[derive(Clone, Copy, Debug, Eq, PartialEq)]
pub enum Direction {
    /// A clockwise turn
    Clockwise,
    /// A counterclockwise turn
    CounterClockwise,
    /// No change
    None,
}

impl<A, B> Rotary<A, B>
where
    A: InputPin,
    B: InputPin,
{
    pub fn new(pin_a: A, pin_b: B) -> Self {
        Self {
            pin_a,
            pin_b,
            prev_a: false,
            prev_c: false,
        }
    }

    pub fn update(&mut self) -> Result<Direction, Either<A::Error, B::Error>> {
        let a = self.pin_a.is_high().map_err(Either::Left)?;
        let dir = if a != self.prev_a {
            self.prev_a = a;
            let b = self.pin_b.is_high().map_err(Either::Right)?;
            if b != self.prev_c {
                self.prev_c = b;
                if a == b {
                    Direction::CounterClockwise
                } else {
                    Direction::Clockwise
                }
            } else {
                Direction::None
            }
        } else {
            Direction::None
        };
        Ok(dir)
    }
}