use core::marker::PhantomData;

use embedded_hal::digital::{OutputPin, PinState};

pub trait Toggle<T: OutputPin> {
    fn toggle(&mut self) -> Result<(), T::Error>;
    fn set(&mut self, state: PinState) -> Result<(), T::Error>;
    fn get(&self) -> PinState;

    fn high(&mut self) -> Result<(), T::Error> {
        self.set(PinState::High)
    }
    fn low(&mut self) -> Result<(), T::Error> {
        self.set(PinState::Low)
    }
}

pub struct ToggleLed<'a, T: OutputPin> {
    pin: &'a mut T,
    state: PinState
}
impl<'a, T: OutputPin> ToggleLed<'a, T> {
    pub fn new(pin: &'a mut T) -> Result<Self, T::Error> {
        match pin.set_low() {
            Ok(_) => Ok(Self { pin, state: PinState::Low }),
            Err(e) => Err(e)
        }
    }
}
impl<'a, T: OutputPin> Toggle<T> for ToggleLed<'a, T> {
    fn toggle(&mut self) -> Result<(), T::Error> {
        if self.state == PinState::High {
            self.state = PinState::Low;
        } else {
            self.state = PinState::Low;
        }
        self.pin.set_state(self.state)
    }
    fn set(&mut self, state: PinState) -> Result<(), T::Error> {
        if state != self.state {
            self.state = state;
            self.pin.set_state(self.state)
        } else {
            Ok(())
        }
    }
    fn get(&self) -> PinState {
        self.state
    }
}

pub struct Leds<'a,
    RP: OutputPin, RT: Toggle<RP>,
    GP: OutputPin, GT: Toggle<GP>,
    BP: OutputPin, BT: Toggle<BP>
> {
    pub red: &'a mut RT,
    pub green: &'a mut GT,
    pub blue: &'a mut BT,

    // phantom data to convince rustc the generics RP, GP and BP are used.
    _r: PhantomData<RP>,
    _g: PhantomData<GP>,
    _b: PhantomData<BP>,
}
impl<'a,
    RP: OutputPin, RT: Toggle<RP>,
    GP: OutputPin, GT: Toggle<GP>,
    BP: OutputPin, BT: Toggle<BP>
> Leds<'a, RP, RT, GP, GT, BP, BT> {
    pub fn new(red: &'a mut RT, green: &'a mut GT, blue: &'a mut BT) -> Self {
        Self {
            red,
            green,
            blue,

            _r: PhantomData,
            _g: PhantomData,
            _b: PhantomData,
        }
    }

    pub fn set_all(&mut self, red: PinState, green: PinState, blue: PinState) {
        let _ = self.red.set(red);
        let _ = self.blue.set(blue);
        let _ = self.green.set(green);
    }
}
