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
    }
}

#[derive(Debug)]
pub enum LedError<R, G, O, B> {
    Red(R),
    Green(G),
    Orange(O),
    Blue(B)
}

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

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

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

    pub fn set_all(&mut self, red: PinState, green: PinState, orange: PinState, blue: PinState) 
        -> Result<(), LedError<RP::Error,GP::Error,OP::Error,BP::Error>>
    {
        self.red.set(red).or_else(|err| { Err(LedError::Red(err)) })?;
        self.orange.set(orange).or_else(|err| { Err(LedError::Orange(err)) })?;
        self.green.set(green).or_else(|err| { Err(LedError::Green(err)) })?;
        self.blue.set(blue).or_else(|err| { Err(LedError::Blue(err)) })?;
        Ok(())
    }
}