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

use crate::leds::{LedError, Leds, Toggle};

enum States {
    Red(StateRed),
    Orange(StateOrange),
    Green(StateGreen),
    BridgeOpen(StateBridgeOpen)
}

trait State<
    RP: OutputPin, RT: Toggle<RP>,
    GP: OutputPin, GT: Toggle<GP>,
    OP: OutputPin, OT: Toggle<OP>,
    BP: OutputPin, BT: Toggle<BP>
> {
    fn new(leds: &mut Leds<RP, RT, GP, GT, OP, OT, BP, BT>)
        -> Result<Self, LedError<RP::Error, GP::Error, OP::Error, BP::Error>>
    where
        Self: Sized;
    fn second_passed(&self, leds: &mut Leds<RP, RT, GP, GT, OP, OT, BP, BT>)
        -> Result<States, LedError<RP::Error, GP::Error, OP::Error, BP::Error>>;
    fn button_press(&self, leds: &mut Leds<RP, RT, GP, GT, OP, OT, BP, BT>)
        -> Result<States, LedError<RP::Error, GP::Error, OP::Error, BP::Error>>;
    fn button_release(&self, leds: &mut Leds<RP, RT, GP, GT, OP, OT, BP, BT>)
        -> Result<States, LedError<RP::Error, GP::Error, OP::Error, BP::Error>>;
}

pub struct StateMachine<'a,
    RP: OutputPin, RT: Toggle<RP>,
    GP: OutputPin, GT: Toggle<GP>,
    OP: OutputPin, OT: Toggle<OP>,
    BP: OutputPin, BT: Toggle<BP>
> {
    state: States,
    leds: &'a mut Leds<'a, RP, RT, GP, GT, OP, OT, BP, BT>
}
impl<'a,
    RP: OutputPin, RT: Toggle<RP>,
    GP: OutputPin, GT: Toggle<GP>,
    OP: OutputPin, OT: Toggle<OP>,
    BP: OutputPin, BT: Toggle<BP>
> StateMachine<'a, RP, RT, GP, GT, OP, OT, BP, BT>
{
    pub fn new(
        leds: &'a mut Leds<'a, RP, RT, GP, GT, OP, OT, BP, BT>,
    ) -> Result<Self, LedError<RP::Error, GP::Error, OP::Error, BP::Error>> {
        let state = StateRed::new(leds)?;
        Ok(Self {
            state: States::Red(state),
            leds,
        })
    }
    pub fn second_passed(
        &mut self,
    ) -> Result<(), LedError<RP::Error, GP::Error, OP::Error, BP::Error>> {
        self.state = match &self.state {
            States::Red(state) => state.second_passed(self.leds)?,
            States::Orange(state) => state.second_passed(self.leds)?,
            States::Green(state) => state.second_passed(self.leds)?,
            States::BridgeOpen(state) => state.second_passed(self.leds)?,
        };
        Ok(())
    }
    pub fn button_press(
        &mut self,
    ) -> Result<(), LedError<RP::Error, GP::Error, OP::Error, BP::Error>> {
        self.state = match &self.state {
            States::Red(state) => state.button_press(self.leds)?,
            States::Orange(state) => state.button_press(self.leds)?,
            States::Green(state) => state.button_press(self.leds)?,
            States::BridgeOpen(state) => state.button_press(self.leds)?,
        };
        Ok(())
    }
    pub fn button_release(
        &mut self,
    ) -> Result<(), LedError<RP::Error, GP::Error, OP::Error, BP::Error>> {
        self.state = match &self.state {
            States::Red(state) => state.button_release(self.leds)?,
            States::Orange(state) => state.button_release(self.leds)?,
            States::Green(state) => state.button_release(self.leds)?,
            States::BridgeOpen(state) => state.button_release(self.leds)?,
        };
        Ok(())
    }
}

// ####################################################
// ## RED #############################################
// ####################################################

struct StateRed {
    time_passed: u32,
}
impl<
    RP: OutputPin, RT: Toggle<RP>,
    GP: OutputPin, GT: Toggle<GP>,
    OP: OutputPin, OT: Toggle<OP>,
    BP: OutputPin, BT: Toggle<BP>
> State<RP, RT, GP, GT, OP, OT, BP, BT> for StateRed
{
    fn new(
        leds: &mut Leds<RP, RT, GP, GT, OP, OT, BP, BT>,
    ) -> Result<Self, LedError<RP::Error, GP::Error, OP::Error, BP::Error>> {
        leds.set_all(PinState::High, PinState::Low, PinState::Low, PinState::Low)?;
        Ok(Self { time_passed: 0 })
    }
    fn second_passed(
        &self,
        leds: &mut Leds<RP, RT, GP, GT, OP, OT, BP, BT>,
    ) -> Result<States, LedError<RP::Error, GP::Error, OP::Error, BP::Error>> {
        if self.time_passed + 1 >= 4 {
            Ok(States::Green(StateGreen::new(leds)?))
        } else {
            Ok(States::Red(Self {
                time_passed: self.time_passed + 1,
            }))
        }
    }

    fn button_press(
        &self,
        leds: &mut Leds<RP, RT, GP, GT, OP, OT, BP, BT>,
    ) -> Result<States, LedError<RP::Error, GP::Error, OP::Error, BP::Error>> {
        Ok(States::BridgeOpen(StateBridgeOpen::new(leds)?))
    }
    fn button_release(
        &self,
        _leds: &mut Leds<RP, RT, GP, GT, OP, OT, BP, BT>,
    ) -> Result<States, LedError<RP::Error, GP::Error, OP::Error, BP::Error>> {
        Ok(States::Red(Self {
            time_passed: self.time_passed,
        }))
    }
}

// ####################################################
// ## GREEN ###########################################
// ####################################################

struct StateGreen {
    time_passed: u32
}
impl<
    RP: OutputPin, RT: Toggle<RP>,
    GP: OutputPin, GT: Toggle<GP>,
    OP: OutputPin, OT: Toggle<OP>,
    BP: OutputPin, BT: Toggle<BP>
> State<RP, RT, GP, GT, OP, OT, BP, BT> for StateGreen
{
    fn new(
        leds: &mut Leds<RP, RT, GP, GT, OP, OT, BP, BT>,
    ) -> Result<Self, LedError<RP::Error, GP::Error, OP::Error, BP::Error>> {
        leds.set_all(PinState::Low, PinState::High, PinState::Low, PinState::Low)?;
        Ok(Self { time_passed: 0 })
    }
    fn second_passed(
        &self,
        leds: &mut Leds<RP, RT, GP, GT, OP, OT, BP, BT>,
    ) -> Result<States, LedError<RP::Error, GP::Error, OP::Error, BP::Error>> {
        if self.time_passed + 1 >= 3 {
            Ok(States::Orange(StateOrange::new(leds)?))
        } else {
            Ok(States::Green(Self {
                time_passed: self.time_passed + 1,
            }))
        }
    }

    fn button_press(
        &self,
        _leds: &mut Leds<RP, RT, GP, GT, OP, OT, BP, BT>,
    ) -> Result<States, LedError<RP::Error, GP::Error, OP::Error, BP::Error>> {
        Ok(States::Green(Self {
            time_passed: self.time_passed,
        }))
    }
    fn button_release(
        &self,
        _leds: &mut Leds<RP, RT, GP, GT, OP, OT, BP, BT>,
    ) -> Result<States, LedError<RP::Error, GP::Error, OP::Error, BP::Error>> {
        Ok(States::Green(Self {
            time_passed: self.time_passed,
        }))
    }
}

// ####################################################
// ## ORANGE ############################################
// ####################################################

struct StateOrange {}
impl<
    RP: OutputPin, RT: Toggle<RP>,
    GP: OutputPin, GT: Toggle<GP>,
    OP: OutputPin, OT: Toggle<OP>,
    BP: OutputPin, BT: Toggle<BP>
> State<RP, RT, GP, GT, OP, OT, BP, BT> for StateOrange
{
    fn new(
        leds: &mut Leds<RP, RT, GP, GT, OP, OT, BP, BT>,
    ) -> Result<Self, LedError<RP::Error, GP::Error, OP::Error, BP::Error>> {
        leds.set_all(PinState::Low, PinState::Low, PinState::High, PinState::Low)?;
        Ok(Self {})
    }
    fn second_passed(
        &self,
        leds: &mut Leds<RP, RT, GP, GT, OP, OT, BP, BT>,
    ) -> Result<States, LedError<RP::Error, GP::Error, OP::Error, BP::Error>> {
        Ok(States::Red(StateRed::new(leds)?))
    }

    fn button_press(
        &self,
        _leds: &mut Leds<RP, RT, GP, GT, OP, OT, BP, BT>,
    ) -> Result<States, LedError<RP::Error, GP::Error, OP::Error, BP::Error>> {
        Ok(States::Orange(Self {}))
    }
    fn button_release(
        &self,
        _leds: &mut Leds<RP, RT, GP, GT, OP, OT, BP, BT>,
    ) -> Result<States, LedError<RP::Error, GP::Error, OP::Error, BP::Error>> {
        Ok(States::Orange(Self {}))
    }
}

// ####################################################
// ## BRIDGE_OPEN #####################################
// ####################################################

struct StateBridgeOpen {
    time_passed: u32
}
impl<
    RP: OutputPin, RT: Toggle<RP>,
    GP: OutputPin, GT: Toggle<GP>,
    OP: OutputPin, OT: Toggle<OP>,
    BP: OutputPin, BT: Toggle<BP>
> State<RP, RT, GP, GT, OP, OT, BP, BT> for StateBridgeOpen
{
    fn new(
        leds: &mut Leds<RP, RT, GP, GT, OP, OT, BP, BT>,
    ) -> Result<Self, LedError<RP::Error, GP::Error, OP::Error, BP::Error>> {
        leds.set_all(PinState::Low, PinState::Low, PinState::Low, PinState::High)?;
        Ok(Self { time_passed: 0 })
    }
    fn second_passed(
        &self,
        leds: &mut Leds<RP, RT, GP, GT, OP, OT, BP, BT>,
    ) -> Result<States, LedError<RP::Error, GP::Error, OP::Error, BP::Error>> {
        if self.time_passed + 1 == 2 {
            leds.blue.low().or_else(|err| {
                Err(LedError::<RP::Error, GP::Error, OP::Error, BP::Error>::Blue(err))
            });
            Ok(States::BridgeOpen(Self {
                time_passed: self.time_passed + 1,
            }))
        } else if self.time_passed + 1 >= 3 {
            leds.blue.high().or_else(|err| {
                Err(LedError::<RP::Error, GP::Error, OP::Error, BP::Error>::Blue(err))
            });
            Ok(States::BridgeOpen(Self { time_passed: 0 }))
        } else {
            Ok(States::BridgeOpen(Self {
                time_passed: self.time_passed + 1,
            }))
        }
    }

    fn button_press(
        &self,
        _leds: &mut Leds<RP, RT, GP, GT, OP, OT, BP, BT>,
    ) -> Result<States, LedError<RP::Error, GP::Error, OP::Error, BP::Error>> {
        Ok(States::BridgeOpen(Self {
            time_passed: self.time_passed,
        }))
    }
    fn button_release(
        &self,
        leds: &mut Leds<RP, RT, GP, GT, OP, OT, BP, BT>,
    ) -> Result<States, LedError<RP::Error, GP::Error, OP::Error, BP::Error>> {
        Ok(States::Red(StateRed::new(leds)?))
    }
}
