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

use log::*; //{trace, debug, info, warn, error}
//use async_channel::Receiver;
use anyhow::Result;
use std::boxed::Box;

use crate::commands::Commands;
use crate::motor_driver::MotorDriver;
use crate::dispatch::{Dispatch, RecvQ, SendQ};

#[derive(Debug, PartialEq, Clone)]
enum ControllerStates {
    Stopped,
    Stopping,
    GoingUp,
    GoingDown,
    //TODO: AutoUp and AutoDown
}

pub struct Controller {
    state: ControllerStates,
    recv: RecvQ,
    send: SendQ,
    driver: Box<dyn MotorDriver>,
}

impl Controller {
    pub fn new(recv: RecvQ, send: SendQ, driver: Box<dyn MotorDriver>) -> Controller {
        Controller {
            state: ControllerStates::Stopping,
            recv: recv,
            send: send,
            driver: driver}
    }

    /// 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) -> RecvQ {
        let cmds = vec![
            Commands::PicRecvUp,
            Commands::PicRecvDown,
            Commands::PicRecvStop,
            Commands::BluetoothUp{data: 0},
            Commands::BluetoothDown{data: 0},
            Commands::BluetoothStop{data: 0},
            Commands::StopTimerExpired,
            Commands::ButtonTimerExpired,
        ];
        dp.get_callback_channel(&cmds)
    }

    async fn enter_state(&mut self, new_s: &ControllerStates) -> Result<()> {
        match new_s {
            ControllerStates::Stopped => {}
            ControllerStates::Stopping => {
                self.send.send(Commands::StopTimerRestart).await?;
                self.driver.stop()?;
            }
            ControllerStates::GoingUp => {
                self.send.send(Commands::ButtonTimerRestart).await?;
                self.driver.start_up()?;
            }
            ControllerStates::GoingDown => {
                self.send.send(Commands::ButtonTimerRestart).await?;
                self.driver.start_down()?;
            }
        }
        Ok(())
    }

    /// Maintain the current state - e.g. reset button timers, etc.
    async fn maintain_state(&mut self) -> Result<()> {
        match self.state {
            ControllerStates::Stopped => {}
            ControllerStates::Stopping => {} // Do not reset this timer; let it expire.
            ControllerStates::GoingUp => {self.send.send(Commands::ButtonTimerRestart).await?;}
            ControllerStates::GoingDown => {self.send.send(Commands::ButtonTimerRestart).await?;}
        }
        Ok(())
    }

    async fn exit_state(&mut self, old_s: &ControllerStates) -> Result <()> {
        match old_s {
            //TODO: We need to notify the BLE controller!
            ControllerStates::Stopped => {}
            ControllerStates::Stopping => {
                self.send.send(Commands::StopTimerClear).await?;
            }
            ControllerStates::GoingUp => {
                self.send.send(Commands::ButtonTimerClear).await?;
            }
            ControllerStates::GoingDown => {
                self.send.send(Commands::ButtonTimerClear).await?;
            }
        }
        Ok(())
    }

    /// Determines the state the controller should be in based on the command received.
    fn handle_cmd(&mut self, cmd: &Commands) -> ControllerStates {
        match self.state {

            ControllerStates::Stopped => {
                match cmd {
                    Commands::PicRecvUp => {return ControllerStates::GoingUp}
                    Commands::PicRecvDown => {return ControllerStates::GoingDown}
                    Commands::BluetoothUp { data } => {
                        match data {
                            0 => {}
                            1 => {return ControllerStates::GoingUp}
                            _ => {error!("Invalid data sent via BluetoothUp: {}", data); return ControllerStates::Stopping}
                        }
                    }
                    Commands::BluetoothDown { data } => {
                        match data {
                            0 => {}
                            1 => {return ControllerStates::GoingDown}
                            _ => {error!("Invalid data sent via BluetoothDown: {}", data); return ControllerStates::Stopping}
                        }
                    }
                    _ => {} // Ignore other commands when stopped.
                }
            }

            ControllerStates::Stopping => {
                match cmd {
                    Commands::StopTimerExpired => {return ControllerStates::Stopped}
                    _ => {} // Ignore other commands when stopping
                }
            }

            ControllerStates::GoingUp => {
                match cmd {
                    Commands::PicRecvUp => {return self.state.clone()}
                    Commands::BluetoothUp { data } => {
                        match data {
                            0 => {} // Not an error, but transition to stop
                            1 => {return self.state.clone()}
                            _ => {error!("Invalid data sent via BluetoothUp: {}", data)}
                        }
                    }
                    _ => {} // fall through to GoingUp default of stopping
                }
                return ControllerStates::Stopping; // Almost every command will be interpreted as stop
            }

            ControllerStates::GoingDown => {
                match cmd {
                    Commands::PicRecvDown => {return self.state.clone()}
                    Commands::BluetoothDown { data } => {
                        match data {
                            0 => {} // Not an error, but transition to stop
                            1 => {return self.state.clone()}
                            _ => {error!("Invalid data sent via BluetoothDown: {}", data)}
                        }
                    }
                    _ => {} // fall through to GoingDown default of stopping
                }
                return ControllerStates::Stopping
            }
        }
        self.state.clone() //Don't transition states by default.
    }

    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?;
        }
        else {
            self.maintain_state().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?;
            trace!("Got command {:?}",cmd);
            let new_s = self.handle_cmd(&cmd);
            trace!("State current {:?} new {:?}", self.state, new_s);
            self.transition_state(&self.state.clone(), &new_s).await?
        }
    }

}