/// State machine of the motor's current state, that takes as inputs
/// command messages.

use log::*; //{trace, debug, info, warn, error}
use anyhow::{anyhow, Result};
use async_channel::{Receiver, Sender};
use crate::commands::{Commands, Button, Toggle};
use crate::dispatch::{Dispatch, DispatchRecvQ, DispatchSendQ};

// The main internal state of the controller, representing the current control method of the motors.
#[derive(Clone, Copy, Debug, PartialEq)]
enum ControllerStates {
    Stopped,
    Stopping,
    GoingUp,
    AutoUp,
    GoingDown,
    AutoDown,
}

#[derive(Clone, Copy, Debug, PartialEq)]
pub enum MotorCommands {
    StartUp,
    StartDown,
    Stop
}

pub type MotorSendQ = Sender<MotorCommands>;
pub type MotorRecvQ = Receiver<MotorCommands>;

#[derive(Clone, Copy, Debug, PartialEq)]
pub enum AutoMode {
    Disallowed = 0,
    Allowed = 1,
}

#[derive(Clone, Copy, Debug, PartialEq)]
pub enum LimitState {
    NoLimitsHit,
    UpperHit,
    LowerHit,
    BothHit,
}

pub struct Controller {
    state: ControllerStates,
    recv: DispatchRecvQ,
    send: DispatchSendQ,
    motor_q: MotorSendQ,
    auto_mode: AutoMode,
    limit_state: LimitState,
}

impl Controller {
    pub fn new(recv: DispatchRecvQ, send: DispatchSendQ, motor_q: MotorSendQ) -> Self {
        Controller {
            state: ControllerStates::Stopping,
            recv: recv,
            send: send,
            motor_q: motor_q,
            auto_mode: AutoMode::Disallowed,  // Use safe default
            limit_state: LimitState::BothHit, // Use safe default
        }
    }

    /// Tell the message dispatch which messages we are interested in receiving, and get
    /// a callback channel that receives those messages.
    pub fn prepare_controller(dp: &mut Dispatch) -> DispatchRecvQ {
        let cmds = vec![
            Commands::PicRecvUp{data: Button::Released},
            Commands::PicRecvDown{data: Button::Released},
            Commands::PicRecvStop{data: Button::Released},
            Commands::BluetoothUp{data: Button::Released},
            Commands::BluetoothDown{data: Button::Released},
            Commands::BluetoothStop{data: Button::Released},
            Commands::PicRecvLimitUp{data: Button::Released},
            Commands::PicRecvLimitDown{data: Button::Released},
            Commands::PicRecvAutoMode{data: Toggle::Inactive},
            Commands::StopTimerExpired,
            Commands::ButtonTimerExpired,
        ];
        dp.get_callback_channel(&cmds)
    }

    async fn enter_state(&mut self, new_s: &ControllerStates) -> Result<()> {
        match new_s {
            ControllerStates::Stopped => {
                // Other notify commands are sent directly from the motor controller
                self.send.send(Commands::NotifyMotorStop{data: Button::Released}).await?;
            }
            ControllerStates::Stopping => {
                self.send.send(Commands::StopTimerRestart).await?;
                self.motor_q.send(MotorCommands::Stop).await?;
                self.send.send(Commands::NotifyMotorStop{data: Button::Pressed}).await?;
            }
            ControllerStates::GoingUp => {
                self.send.send(Commands::ButtonTimerRestart).await?;
                self.motor_q.send(MotorCommands::StartUp).await?;
                self.send.send(Commands::NotifyMotorUp{data: Button::Pressed}).await?;
            }
            ControllerStates::AutoUp => {
                self.motor_q.send(MotorCommands::StartUp).await?;
                self.send.send(Commands::NotifyMotorUp{data: Button::Pressed}).await?;
            }
            ControllerStates::GoingDown => {
                self.send.send(Commands::ButtonTimerRestart).await?;
                self.motor_q.send(MotorCommands::StartDown).await?;
                self.send.send(Commands::NotifyMotorUp{data: Button::Pressed}).await?;
            }
            ControllerStates::AutoDown => {
                self.motor_q.send(MotorCommands::StartDown).await?;
                self.send.send(Commands::NotifyMotorUp{data: Button::Pressed}).await?;
            }
        }
        Ok(())
    }

    async fn exit_state(&mut self, old_s: &ControllerStates) -> Result <()> {
        match old_s {
            ControllerStates::Stopped => {}
            ControllerStates::Stopping => {
                self.send.send(Commands::StopTimerClear).await?;
            }
            ControllerStates::GoingUp => {
                self.send.send(Commands::ButtonTimerClear).await?;
                self.send.send(Commands::NotifyMotorUp{data: Button::Released}).await?;
            }
            ControllerStates::AutoUp => {
                self.send.send(Commands::NotifyMotorUp{data: Button::Released}).await?;
            }
            ControllerStates::GoingDown => {
                self.send.send(Commands::ButtonTimerClear).await?;
                self.send.send(Commands::NotifyMotorDown{data: Button::Released}).await?;
            }
            ControllerStates::AutoDown => {
                self.send.send(Commands::NotifyMotorDown{data: Button::Released}).await?;
            }
        }
        Ok(())
    }

    fn change_state_if_released(&self, data: &Button, new_state: ControllerStates) -> ControllerStates {
        match data {
            Button::Released => {new_state}
            Button::Pressed => {self.state.clone()}
        }
    }

    fn change_state_if_pressed(&self, data: &Button, new_state: ControllerStates) -> ControllerStates {
        match data {
            Button::Released => {self.state.clone()}
            Button::Pressed => {new_state}
        }
    }

    /// Determines the state the controller should be in based on the command received.
    async fn handle_cmd(&mut self, cmd: &Commands) -> Result<ControllerStates> {
        let mut rc = self.state.clone(); // Don't transition by default.
        match cmd {
            Commands::PicRecvUp { data } | Commands::BluetoothUp { data }=> {
                match self.state {
                    ControllerStates::Stopped => {rc = self.change_state_if_pressed(data, self.remote_up_or_auto_up())}
                    ControllerStates::Stopping => {}
                    ControllerStates::GoingUp => {
                        self.send.send(Commands::ButtonTimerRestart).await.expect("Failed to necessary timer");
                        rc =  self.change_state_if_released(data, ControllerStates::Stopping)
                    }
                    ControllerStates::AutoUp => {} // Don't stop auto on button release
                    ControllerStates::GoingDown | 
                    ControllerStates::AutoDown => {
                        rc= self.change_state_if_pressed(data, ControllerStates::Stopping)
                    }
                }
            }
            Commands::PicRecvDown { data } | Commands::BluetoothDown { data } => {
                match self.state {
                    ControllerStates::Stopped => {rc = self.change_state_if_pressed(data, self.remote_down_or_auto_down())}
                    ControllerStates::Stopping => {}
                    ControllerStates::GoingUp | 
                    ControllerStates::AutoUp => {
                        rc = self.change_state_if_pressed(data, ControllerStates::Stopping)
                    }
                    ControllerStates::GoingDown => {
                        self.send.send(Commands::ButtonTimerRestart).await.expect("Failed to necessary timer");
                        rc = self.change_state_if_released(data, ControllerStates::Stopping)
                    }
                    ControllerStates::AutoDown => {}
                }
            }
            Commands::PicRecvStop { data } | Commands::BluetoothStop { data } => {
                match self.state {
                    ControllerStates::Stopped => {}
                    ControllerStates::Stopping => {}
                    ControllerStates::GoingUp |
                    ControllerStates::AutoUp | 
                    ControllerStates::GoingDown |
                    ControllerStates::AutoDown => {
                        rc = self.change_state_if_pressed(data, ControllerStates::Stopping)
                    }
                }
            }
            Commands::PicRecvLimitUp { data } => {
                self.adjust_limit(LimitState::UpperHit, data);
                match self.state {
                    ControllerStates::Stopped => {} // Ignore; this could just be our initial notification on startup.
                    ControllerStates::Stopping => {}
                    ControllerStates::GoingUp | 
                    ControllerStates::AutoUp=> {
                        released_warning(data, "Limit switches may be installed incorrectly!");
                        rc = self.change_state_if_pressed(data, ControllerStates::Stopping)
                    }
                    ControllerStates::GoingDown | 
                    ControllerStates::AutoDown=> {
                        pressed_warning(data, "Limit switches may be installed incorrectly!");
                        // Stop out of an abundance of caution.  We should not get a limit press, even if it's the wrong one.
                        rc = self.change_state_if_pressed(data, ControllerStates::Stopping)
                    } 
                }
            }
            Commands::PicRecvLimitDown { data } => {
                self.adjust_limit(LimitState::LowerHit, data);
                match self.state {
                    ControllerStates::Stopped => {} // Ignore; this could just be our initial notification on startup.
                    ControllerStates::Stopping => {}
                    ControllerStates::GoingUp |
                    ControllerStates::AutoUp => {
                        pressed_warning(data, "Limit switches may be installed incorrectly!");
                        // Stop out of an abundance of caution.  We should not get a limit press, even if it's the wrong one.
                        rc = self.change_state_if_pressed(data, ControllerStates::Stopping)
                    }
                    ControllerStates::GoingDown |
                    ControllerStates::AutoDown => {
                        released_warning(data, "Limit switches may be installed incorrectly!");
                        rc = self.change_state_if_pressed(data, ControllerStates::Stopping)
                    }
                }
            }
            Commands::PicRecvAutoMode { data } => {
                self.set_auto(data);
                match self.state {
                    ControllerStates::Stopped => {}
                    ControllerStates::Stopping => {}
                    ControllerStates::GoingUp => {}
                    ControllerStates::AutoUp => {rc = ControllerStates::GoingUp}
                    ControllerStates::GoingDown => {}
                    ControllerStates::AutoDown => {rc = ControllerStates::GoingDown}
                }
            }
            Commands::StopTimerExpired => {
                match self.state {
                    ControllerStates::Stopped => {}
                    ControllerStates::Stopping => {rc = ControllerStates::Stopped}
                    ControllerStates::GoingUp |
                    ControllerStates::AutoUp |
                    ControllerStates::GoingDown |
                    ControllerStates::AutoDown => {
                        warn!("Stop timer returned in an inappropriate state! {:?}", self.state)
                    }
                }
            }
            Commands::ButtonTimerExpired => {
                match self.state {
                    ControllerStates::Stopped => {}
                    ControllerStates::Stopping => {}
                    ControllerStates::GoingUp => {rc = ControllerStates::Stopping}
                    ControllerStates::AutoUp => {}
                    ControllerStates::GoingDown => {rc = ControllerStates::Stopping}
                    ControllerStates::AutoDown => {}
                }
            }
            // Commands we should ignore (not using _ because we want to ensure all commands are accounted for!)
            Commands::StopTimerRestart | 
            Commands::StopTimerClear | 
            Commands::ButtonTimerRestart |
            Commands::ButtonTimerClear |
            Commands::PairTimerClear |
            Commands::PairTimerExpired |
            Commands::AllowPairing |
            Commands::EraseBleBonds => {
                warn!("Unexpected command received by motor controller") // TODO: internal "us" error.
            }
            Commands::NotifyMotorDown { data } |
            Commands::NotifyMotorStop { data } |
            Commands::NotifyMotorUp { data } => {
                warn!("Unexpected command received by motor controller {:?}", data) // TODO: internal "us" error.
            }
            Commands::BluetoothName { data } => {
                warn!("Unexpected command received by motor controller {:?}", data) // TODO: internal "us" error.
            }
            Commands::TestingExit => {
                return Err(anyhow!("Exiting due to testing"))
            }
        }
    //self.state.clone() // Don't transition by default
    Ok(rc)
    }

    async fn transition_state(&mut self, old_s: &ControllerStates, new_s: &ControllerStates) -> Result<()> {
        if old_s != new_s {
            self.exit_state(&old_s).await?;
            self.enter_state(&new_s).await?;
        }
        self.state = new_s.clone();
        Ok(())
    }

    pub async fn run(&mut self) -> Result<()> {
        // On entry, assume initial stopping state
        debug!("Setting motor controller initial state to stopping");
        self.state = ControllerStates::Stopping;
        self.enter_state(&ControllerStates::Stopping).await?;
        loop {
            let cmd = self.recv.recv().await.expect("Motor controller command queue unexpectedly failed");
            trace!("Got command {:?}",cmd);
            match self.handle_cmd(&cmd).await {
                Ok(new_s) => {
                    trace!("State current {:?} new {:?}", self.state, new_s);
                    self.transition_state(&self.state.clone(), &new_s)
                        .await
                        .expect("Unexpected state change failure in motor controller");
                }
                Err(_) => {break;}
            }
        }
        Err(anyhow!("Unexpectedly exited loop"))
    }

    fn remote_up_or_auto_up(&self) -> ControllerStates {
        self.up_or_auto_up(ControllerStates::GoingUp)
    }

    fn up_or_auto_up(&self, up: ControllerStates) -> ControllerStates {
        // Assume that checking the limit against the direction has already been performed.
        match self.auto_mode {
            AutoMode::Disallowed => {
                up // TODO: this allows manual buttons to override limits as long as "auto" mode is off.
            }
            AutoMode::Allowed => {
                match self.limit_state {
                    LimitState::NoLimitsHit | LimitState::LowerHit=> {
                        ControllerStates::AutoUp
                    }
                    LimitState::UpperHit => {
                        ControllerStates::Stopping // Failsafe as we are already at our upper limit. TODO: maybe should be manual up?
                    }
                    LimitState::BothHit => {
                        up
                    }
                }
            }
        }
    }

    fn remote_down_or_auto_down(&self) -> ControllerStates {
        self.down_or_auto_down(ControllerStates::GoingDown)
    }

    fn down_or_auto_down(&self, down: ControllerStates) -> ControllerStates {
        // Assume that checking the limit against the direction has already been performed.
        match self.auto_mode {
            AutoMode::Disallowed => {
                down // TODO: this allows manual buttons to override limits as long as "auto" mode is off.
            }
            AutoMode::Allowed => {
                match self.limit_state {
                    LimitState::NoLimitsHit | LimitState::LowerHit=> {
                        ControllerStates::AutoDown
                    }
                    LimitState::UpperHit => {
                        ControllerStates::Stopping // Failsafe as we are already at our upper limit. TODO: Maybe should be manual down?
                    }
                    LimitState::BothHit => {
                        down
                    }
                }
            }
        }
    }

    fn set_auto(&mut self, data: &Toggle) {
        match data {
            Toggle::Inactive => {
                self.auto_mode = AutoMode::Disallowed;
            }
            Toggle::Active => {
                if self.limit_state == LimitState::BothHit {
                    warn!("Limit switches not detected.  Aborting auto  mode.");
                } else {
                    self.auto_mode = AutoMode::Allowed;
                }
            }
        }
    }

    // Adjusts the current limit state, based on its present state and an incoming button press or release.
    fn adjust_limit(&mut self, limit: LimitState, pressed: &Button) {
        match pressed {
            Button::Released => {
                match limit {
                    LimitState::NoLimitsHit => {
                        unreachable!("There is no way to press NoLimits")
                    }
                    LimitState::LowerHit => {
                        match self.limit_state {
                            LimitState::NoLimitsHit |
                            LimitState::UpperHit => {warn!("removed limit we never hit {:?}", LimitState::LowerHit);} //TODO intenral error
                            LimitState::LowerHit => {self.limit_state = LimitState::NoLimitsHit;}
                            LimitState::BothHit => {self.limit_state = LimitState::UpperHit;}
                        }
                    }
                    LimitState::UpperHit => {
                        match self.limit_state {
                            LimitState::NoLimitsHit |
                            LimitState::LowerHit => {warn!("removed limit we never hit {:?}", LimitState::UpperHit);} //TODO intenral error
                            LimitState::UpperHit => {self.limit_state = LimitState::NoLimitsHit;}
                            LimitState::BothHit => {self.limit_state = LimitState::LowerHit;}
                        }
                    }
                    LimitState::BothHit => {
                        unreachable!("There is no way to press BothHit")
                    }
                }
            }
            Button::Pressed => {
                match limit {
                    LimitState::NoLimitsHit => {
                        unreachable!("There is no way to press BothHit")
                    }
                    LimitState::LowerHit => {
                        match self.limit_state {
                            LimitState::NoLimitsHit => {self.limit_state = LimitState::LowerHit;}
                            LimitState::LowerHit => {}
                            LimitState::UpperHit => {self.limit_state = LimitState::BothHit;}
                            LimitState::BothHit => {}
                        }
                    }
                    LimitState::UpperHit => {
                        match self.limit_state {
                            LimitState::NoLimitsHit => {self.limit_state = LimitState::UpperHit;}
                            LimitState::LowerHit => {self.limit_state = LimitState::BothHit;}
                            LimitState::UpperHit => {}
                            LimitState::BothHit => {}
                        }
                    }
                    LimitState::BothHit => {
                        unreachable!("There is no way to press BothHit")
                    }
                }
            }
        }
    }

}

// Give a user warning if the given button state is pressed at the time
fn pressed_warning(data: &Button, warn: &str) {
    match data {
        Button::Pressed => {warn!("{}", warn);} // TODO: user warning, not internal
        Button::Released => {}
    }
}

// Give a user warning if the given button state is released at the time
fn released_warning(data: &Button, warn: &str) {
    match data {
        Button::Released => {warn!("{}", warn);} // TODO: user warning, not internal
        Button::Pressed => {}
    }
}

//// Test ////////////////////////////////////////////////////////////////////////////////////////

#[cfg(test)]
mod tests {
    use super::*;
    use async_channel::{unbounded, TryRecvError};
    use async_io::block_on;

    // Creates a new controller for use in tests.  Has async-channen send, recv, and motor
    // endpoints to use.  Starts in AutoMode::Disallowed and LimitState::BothHit
    fn create_controller() -> (Controller, DispatchSendQ, DispatchRecvQ, MotorRecvQ) {
        let (ch1s, ch1r) = unbounded();
        let (ch2s, ch2r) = unbounded();
        let (chms, chmr) = unbounded();
        let con = Controller::new(ch1r, ch2s, chms);
        (con, ch1s, ch2r, chmr)
    }

    // Send a series of messages to the controller, then exit.
    fn run_cmd_queue(con: &mut Controller, cs: DispatchSendQ, q: &mut Vec<Commands>) -> Result<()>{
        block_on (
            async {
                q.push(Commands::TestingExit);
                for c in q {
                    cs.send(c.clone()).await?
                }
                let _ = con.run().await; // If this doesn't error out of the loop, we'll hang here
                Ok(())
            }
        )
    }

    /* States we want to test:
    Controller states:
        Stopped,
        Stopping,
        GoingUp,
        AutoUp,
        GoingDown,
        AutoDown,
    Auto states:
        Disallowed
        Allowed
    Limit states:
        NoLimitsHit,
        UpperHit,
        LowerHit,
        BothHit,
     */

    #[test]
    fn motor_stops_on_initialization() -> Result<()>{
        let (mut con, cs, _cr, mr) = create_controller();
        let mut q = vec![];
        run_cmd_queue(&mut con, cs, &mut q)?;
        assert_eq!(con.state, ControllerStates::Stopping);
        assert_eq!(mr.try_recv().unwrap(), MotorCommands::Stop);
        assert_eq!(mr.try_recv(), Err(TryRecvError::Empty)); // Make sure queue is empty
        Ok(())
    }

    #[test]
    fn motor_stop_state_after_timeout() -> Result<()>{
        let (mut con, cs, _cr, mr) = create_controller();
        let mut q = vec![
            Commands::StopTimerExpired,
            ];
        run_cmd_queue(&mut con, cs, &mut q)?;
        assert_eq!(con.state, ControllerStates::Stopped);
        assert_eq!(mr.try_recv().unwrap(), MotorCommands::Stop);
        assert_eq!(mr.try_recv(), Err(TryRecvError::Empty)); // Make sure queue is empty
        Ok(())
    }

    #[test]
    fn motor_refuses_auto_when_bothhit() -> Result<()>{
        let (mut con, cs, _cr, _mr) = create_controller();
        let mut q = vec![
            Commands::PicRecvAutoMode{data: Toggle::Active},
            ];
        run_cmd_queue(&mut con, cs, &mut q)?;
        assert_eq!(con.auto_mode, AutoMode::Disallowed);
        Ok(())
    }

    #[test]
    fn motor_succeeds_auto_when_not_bothhit() -> Result<()>{
        let (mut con, cs, _cr, _mr) = create_controller();
        let mut q = vec![
            Commands::PicRecvLimitDown { data: Button::Released },
            Commands::PicRecvAutoMode{data: Toggle::Active},
            ];
        run_cmd_queue(&mut con, cs, &mut q)?;
        assert_eq!(con.auto_mode, AutoMode::Allowed);
        Ok(())
    }

    #[test]
    fn motor_enters_timed_up() -> Result<()>{
        let (mut con, cs, _cr, mr) = create_controller();
        let mut q = vec![
            Commands::StopTimerExpired,
            Commands::PicRecvUp { data: Button::Pressed }
            ];
        run_cmd_queue(&mut con, cs, &mut q)?;
        assert_eq!(con.state, ControllerStates::GoingUp);
        assert_eq!(mr.try_recv().unwrap(), MotorCommands::Stop);
        assert_eq!(mr.try_recv().unwrap(), MotorCommands::StartUp);
        assert_eq!(mr.try_recv(), Err(TryRecvError::Empty)); // Make sure queue is empty
        Ok(())
    }

    #[test]
    fn motor_stops_after_no_input_no_auto() -> Result<()>{
        let (mut con, cs, _cr, mr) = create_controller();
        let mut q = vec![
            Commands::StopTimerExpired,
            Commands::BluetoothDown { data: Button::Pressed },
            Commands::ButtonTimerExpired,
            ];
        run_cmd_queue(&mut con, cs, &mut q)?;
        assert_eq!(con.state, ControllerStates::Stopping);
        assert_eq!(mr.try_recv().unwrap(), MotorCommands::Stop);
        assert_eq!(mr.try_recv().unwrap(), MotorCommands::StartDown);
        assert_eq!(mr.try_recv().unwrap(), MotorCommands::Stop);
        assert_eq!(mr.try_recv(), Err(TryRecvError::Empty)); // Make sure queue is empty
        Ok(())
    }

    #[test]
    fn motor_stops_on_release_timed_up() -> Result<()>{
        let (mut con, cs, _cr, mr) = create_controller();
        let mut q = vec![
            Commands::StopTimerExpired,
            Commands::PicRecvUp { data: Button::Pressed },
            Commands::PicRecvUp { data: Button::Released },
            ];
        run_cmd_queue(&mut con, cs, &mut q)?;
        assert_eq!(con.state, ControllerStates::Stopping);
        assert_eq!(mr.try_recv().unwrap(), MotorCommands::Stop);
        assert_eq!(mr.try_recv().unwrap(), MotorCommands::StartUp);
        assert_eq!(mr.try_recv().unwrap(), MotorCommands::Stop);
        assert_eq!(mr.try_recv(), Err(TryRecvError::Empty)); // Make sure queue is empty
        Ok(())
    }
    #[test]
    fn motor_stops_on_limit_timed_up() -> Result<()>{
        let (mut con, cs, _cr, mr) = create_controller();
        let mut q = vec![
            Commands::StopTimerExpired,
            Commands::PicRecvUp { data: Button::Pressed },
            Commands::PicRecvLimitUp { data: Button::Pressed },
            ];
        run_cmd_queue(&mut con, cs, &mut q)?;
        assert_eq!(con.state, ControllerStates::Stopping);
        assert_eq!(mr.try_recv().unwrap(), MotorCommands::Stop);
        assert_eq!(mr.try_recv().unwrap(), MotorCommands::StartUp);
        assert_eq!(mr.try_recv().unwrap(), MotorCommands::Stop);
        assert_eq!(mr.try_recv(), Err(TryRecvError::Empty)); // Make sure queue is empty
        Ok(())
    }

    #[test]
    fn motor_enters_auto_up() -> Result<()>{
        let (mut con, cs, _cr, mr) = create_controller();
        let mut q = vec![
            Commands::StopTimerExpired,
            Commands::PicRecvLimitUp{data: Button::Released},
            Commands::PicRecvAutoMode{data: Toggle::Active},
            Commands::PicRecvUp { data: Button::Pressed },
            ];
        run_cmd_queue(&mut con, cs, &mut q)?;
        assert_eq!(con.state, ControllerStates::AutoUp);
        assert_eq!(mr.try_recv().unwrap(), MotorCommands::Stop);
        assert_eq!(mr.try_recv().unwrap(), MotorCommands::StartUp);
        assert_eq!(mr.try_recv(), Err(TryRecvError::Empty)); // Make sure queue is empty
        Ok(())
    }

    #[test]
    fn motor_does_not_stop_after_no_input_in_auto() -> Result<()>{
        let (mut con, cs, _cr, mr) = create_controller();
        let mut q = vec![
            Commands::StopTimerExpired,
            Commands::PicRecvLimitUp{data: Button::Released},
            Commands::PicRecvAutoMode{data: Toggle::Active},
            Commands::BluetoothDown { data: Button::Pressed },
            Commands::ButtonTimerExpired,
            ];
        run_cmd_queue(&mut con, cs, &mut q)?;
        assert_eq!(con.state, ControllerStates::AutoDown);
        assert_eq!(mr.try_recv().unwrap(), MotorCommands::Stop);
        assert_eq!(mr.try_recv().unwrap(), MotorCommands::StartDown);
        assert_eq!(mr.try_recv(), Err(TryRecvError::Empty)); // Make sure queue is empty
        Ok(())
    }

    #[test]
    fn motor_does_not_stop_auto_release() -> Result<()>{
        let (mut con, cs, _cr, mr) = create_controller();
        let mut q = vec![
            Commands::StopTimerExpired,
            Commands::PicRecvLimitUp{data: Button::Released},
            Commands::PicRecvAutoMode{data: Toggle::Active},
            Commands::PicRecvUp { data: Button::Pressed },
            Commands::PicRecvUp { data: Button::Released },
            ];
        run_cmd_queue(&mut con, cs, &mut q)?;
        assert_eq!(con.state, ControllerStates::AutoUp);
        assert_eq!(mr.try_recv().unwrap(), MotorCommands::Stop);
        assert_eq!(mr.try_recv().unwrap(), MotorCommands::StartUp);
        assert_eq!(mr.try_recv(), Err(TryRecvError::Empty)); // Make sure queue is empty
        Ok(())
    }

    #[test]
    fn motor_switches_to_timed_when_auto_toggled_off() -> Result<()>{
        let (mut con, cs, _cr, mr) = create_controller();
        let mut q = vec![
            Commands::StopTimerExpired,
            Commands::PicRecvLimitUp{data: Button::Released},
            Commands::PicRecvAutoMode{data: Toggle::Active},
            Commands::PicRecvUp { data: Button::Pressed },
            Commands::PicRecvAutoMode{data: Toggle::Inactive},
            ];
        run_cmd_queue(&mut con, cs, &mut q)?;
        assert_eq!(con.state, ControllerStates::GoingUp);
        assert_eq!(mr.try_recv().unwrap(), MotorCommands::Stop);
        assert_eq!(mr.try_recv().unwrap(), MotorCommands::StartUp);
        assert_eq!(mr.try_recv().unwrap(), MotorCommands::StartUp); // Second up commanded should be okay
        assert_eq!(mr.try_recv(), Err(TryRecvError::Empty)); // Make sure queue is empty
        Ok(())
    }

    #[test]
    fn motor_continues_with_additional_presses() -> Result<()>{
        let (mut con, cs, _cr, mr) = create_controller();
        let mut q = vec![
            Commands::StopTimerExpired,
            Commands::PicRecvLimitUp{data: Button::Released},
            Commands::PicRecvAutoMode{data: Toggle::Active},
            Commands::PicRecvUp { data: Button::Pressed },
            Commands::PicRecvUp { data: Button::Released },
            Commands::BluetoothUp { data: Button::Pressed },
            Commands::BluetoothUp { data: Button::Released },
            ];
        run_cmd_queue(&mut con, cs, &mut q)?;
        assert_eq!(con.state, ControllerStates::AutoUp);
        assert_eq!(mr.try_recv().unwrap(), MotorCommands::Stop);
        assert_eq!(mr.try_recv().unwrap(), MotorCommands::StartUp);
        assert_eq!(mr.try_recv(), Err(TryRecvError::Empty)); // Make sure queue is empty
        Ok(())
    }

    #[test]
    fn motor_stops_on_opposite_button() -> Result<()>{
        let (mut con, cs, _cr, mr) = create_controller();
        let mut q = vec![
            Commands::StopTimerExpired,
            Commands::PicRecvLimitUp{data: Button::Released},
            Commands::PicRecvAutoMode{data: Toggle::Active},
            Commands::PicRecvUp { data: Button::Pressed },
            Commands::PicRecvUp { data: Button::Released },
            Commands::BluetoothDown { data: Button::Pressed },
            Commands::BluetoothDown { data: Button::Released },
            ];
        run_cmd_queue(&mut con, cs, &mut q)?;
        assert_eq!(con.state, ControllerStates::Stopping);
        assert_eq!(mr.try_recv().unwrap(), MotorCommands::Stop);
        assert_eq!(mr.try_recv().unwrap(), MotorCommands::StartUp);
        assert_eq!(mr.try_recv().unwrap(), MotorCommands::Stop);
        assert_eq!(mr.try_recv(), Err(TryRecvError::Empty)); // Make sure queue is empty
        Ok(())
    }

    #[test]
    fn motor_stops_if_limits_installed_backwards() -> Result<()>{
        let (mut con, cs, _cr, mr) = create_controller();
        let mut q = vec![
            Commands::StopTimerExpired,
            Commands::PicRecvLimitUp{data: Button::Released},
            Commands::PicRecvLimitDown{data: Button::Released},
            Commands::PicRecvAutoMode{data: Toggle::Active},
            Commands::PicRecvUp { data: Button::Pressed },
            Commands::PicRecvUp { data: Button::Released },
            Commands::PicRecvLimitDown { data: Button::Pressed },
            ];
        run_cmd_queue(&mut con, cs, &mut q)?;
        assert_eq!(con.state, ControllerStates::Stopping);
        assert_eq!(mr.try_recv().unwrap(), MotorCommands::Stop);
        assert_eq!(mr.try_recv().unwrap(), MotorCommands::StartUp);
        assert_eq!(mr.try_recv().unwrap(), MotorCommands::Stop);
        assert_eq!(mr.try_recv(), Err(TryRecvError::Empty)); // Make sure queue is empty
        Ok(())
    }
}