#include <QuarkTS.h>

using namespace qOS;

input::digitalChannel in_engine_on( 2, true );
input::digitalChannel in_accelerate( 3, true );
input::digitalChannel in_resume( 4, true );
input::digitalChannel in_off( 5, true );
input::digitalChannel in_break_pressed( 6, true );
input::digitalChannel in_cruise( 7, true );
input::digitalChannel in_reached_cruising( 8, true );
input::digitalChannel in_engine_off( 9, true );
input::watcher inWatcher( digitalRead, nullptr , 50_ms );

/*define the FSM application event-signals*/
enum : sm::signalIDType {
  SIGNAL_ENGINE_ON         = sm::SIGNAL_USER( 2 ),
  SIGNAL_ACCEL             = sm::SIGNAL_USER( 3 ),
  SIGNAL_RESUME            = sm::SIGNAL_USER( 4 ),
  SIGNAL_OFF               = sm::SIGNAL_USER( 5 ),
  SIGNAL_BRAKE_PRESSED     = sm::SIGNAL_USER( 6 ),
  SIGNAL_CRUISE            = sm::SIGNAL_USER( 7 ),
  SIGNAL_REACHED_CRUISING  = sm::SIGNAL_USER( 8 ),
  SIGNAL_ENGINE_OFF        = sm::SIGNAL_USER( 9 ),
};

task CruiseControlTask;
task inputTask;
stateMachine Top_SM;
/*highest level states*/
sm::state state_idle, state_initial, state_cruisingoff, state_automatedcontrol;
/*states inside the state_automatedcontrol*/
sm::state state_accelerating, state_cruising, state_resuming;
sm::signalQueue<10> top_sigqueue;

/*=======================================================================*/
/*                      EVENT-SIGNAL ACTIONS AND GUARDS                  */
/*=======================================================================*/
bool SigAct_ClearDesiredSpeed( sm::handler_t h ) {
  (void)h;
  logger::out(logger::debug);
  return true;
}
/*---------------------------------------------------------------------*/
bool SigAct_BrakeOff( sm::handler_t h ) {
  logger::out(logger::debug);
  (void)h; /*unused*/
  return true;
  //return ( BSP_BREAK_READ() == OFF ) ? true : false;  /*check guard*/
}
/*=======================================================================*/
/*                             TRANSITION TABLES                         */
/*=======================================================================*/
sm::transition idle_transitions[] = {
  { SIGNAL_ENGINE_ON, SigAct_ClearDesiredSpeed, state_initial }
};
 
sm::transition initial_transitions[] = {
  { SIGNAL_ACCEL, SigAct_BrakeOff, state_accelerating }
};
 
sm::transition accel_transitions[] = {
  { SIGNAL_CRUISE, state_cruising }
};
 
sm::transition cruising_transitions[] = {
  { SIGNAL_OFF,    state_cruisingoff },
  { SIGNAL_ACCEL,  state_accelerating }
};
 
sm::transition resuming_transitions[] = {
  { SIGNAL_OFF,              state_cruisingoff },
  { SIGNAL_ACCEL,            state_accelerating },
  { SIGNAL_REACHED_CRUISING, state_cruising }
};
 
sm::transition cruisingoff_transitions[] = {
  { SIGNAL_ACCEL,      SigAct_BrakeOff, state_accelerating },
  { SIGNAL_RESUME,     SigAct_BrakeOff, state_resuming },
  { SIGNAL_ENGINE_OFF, state_idle  }
};
 
sm::transition automated_transitions[] = {
  { SIGNAL_BRAKE_PRESSED, state_cruisingoff }
};


/*=======================================================================*/
/*                  STATE CALLBACK FOR THE TOP FSM                      */
/*=======================================================================*/
sm::status state_top_callback( sm::handler_t h ) {
  sm::status RetVal = sm::status::SUCCESS;
  switch ( h.signal() ) {
    case sm::signalID::SIGNAL_ENTRY:
      logger::out(logger::debug) << h.thisState();
      break;
    case sm::signalID::SIGNAL_EXIT:
      break;
  }
  return RetVal;
}
/*=======================================================================*/
/*                  CALLBACKS FOR THE STATES ABOVE TOP                    */
/*=======================================================================*/
sm::status state_idle_callback( sm::handler_t h ) {
  /*TODO : state activities*/
  switch ( h.signal() ) {
    case sm::signalID::SIGNAL_ENTRY:
      logger::out(logger::debug) << h.thisState();
      break;
  }
  return sm::status::SUCCESS;
}
/*---------------------------------------------------------------------*/
sm::status state_initial_callback( sm::handler_t h ) {
  /*TODO : state activities*/
  switch ( h.signal() ) {
    case sm::signalID::SIGNAL_ENTRY:
      logger::out(logger::debug) << h.thisState();
      break;
  }
  return sm::status::SUCCESS;
}
/*---------------------------------------------------------------------*/
sm::status state_cruisingoff_callback( sm::handler_t h ) {
  /*TODO : state activities*/
  switch ( h.signal() ) {
    case sm::signalID::SIGNAL_ENTRY:
      logger::out(logger::debug) << h.thisState();
      break;
  }
  return sm::status::SUCCESS;
}
/*---------------------------------------------------------------------*/
sm::status state_automatedcontrol_callback( sm::handler_t h ) {
  /*TODO : state activities*/
  switch ( h.signal() ) {
    case sm::signalID::SIGNAL_ENTRY:
      logger::out(logger::debug) << h.thisState();
      break;
  }
  return sm::status::SUCCESS;
}
/*=======================================================================*/
/*          STATE CALLBACKS FOR THE AUTOMATED CONTROL FSM                */
/*=======================================================================*/
sm::status state_accelerating_callback( sm::handler_t h ) {
  switch ( h.signal() ) {
    case sm::signalID::SIGNAL_ENTRY:
      logger::out(logger::debug) << h.thisState();
      break;
    case sm::signalID::SIGNAL_EXIT:
      logger::out() << "Select Desired Speed";
      break;
  }
  return sm::status::SUCCESS;
}
/*---------------------------------------------------------------------*/
sm::status state_resuming_callback( sm::handler_t h ) {
  /*TODO : state activities*/
  switch ( h.signal() ) {
    case sm::signalID::SIGNAL_ENTRY:
      logger::out(logger::debug) << h.thisState();
      break;
  }
  return sm::status::SUCCESS;
}
/*---------------------------------------------------------------------*/
sm::status state_cruising_callback( sm::handler_t h ) {
  /*TODO : state activities*/
  switch ( h.signal() ) {
    case sm::signalID::SIGNAL_ENTRY:
      logger::out(logger::debug) << h.thisState();
      break;
  }
  return sm::status::SUCCESS;
}

void idleTaskCallback( event_t e ) {
  if ( e.firstCall() ) {
    logger::out() << product::caption;
  }
}

void pinEvent( input::channel& c ) {
  if ( input::event::RISING_EDGE == c.getEvent() ) {
    Top_SM.sendSignal( static_cast<sm::signalIDType>( c.getChannel() ) );
  }
}

void setup() {
  pinMode( LED_BUILTIN, OUTPUT );
  pinMode( in_engine_on.getChannel(), INPUT_PULLUP );
  pinMode( in_accelerate.getChannel(), INPUT_PULLUP );
  pinMode( in_resume.getChannel(), INPUT_PULLUP );
  pinMode( in_off.getChannel(), INPUT_PULLUP );
  pinMode( in_break_pressed.getChannel(), INPUT_PULLUP );
  pinMode( in_cruise.getChannel(), INPUT_PULLUP );
  pinMode( in_reached_cruising.getChannel(), INPUT_PULLUP );
  pinMode( in_engine_off.getChannel(), INPUT_PULLUP );
  Serial.begin(115200);

  logger::setOutputFcn( []( void *arg, const char c ){ Serial.write( c ); } );
  os.init( millis, idleTaskCallback );
  /*setup watcher*/
  inWatcher.add( in_engine_on, pinEvent );
  inWatcher.add( in_accelerate, pinEvent );
  inWatcher.add( in_resume, pinEvent );
  inWatcher.add( in_off, pinEvent );
  inWatcher.add( in_break_pressed, pinEvent );
  inWatcher.add( in_cruise, pinEvent );
  inWatcher.add( in_reached_cruising, pinEvent );
  inWatcher.add( in_engine_off, pinEvent );
  os.add( inputTask, inWatcher );
  /*setup state machine*/
  Top_SM.setup( state_top_callback, state_idle );
  /*subscribe to the highest level states*/
  Top_SM.add( state_idle, state_idle_callback );
  Top_SM.add( state_initial, state_initial_callback );
  Top_SM.add( state_cruisingoff, state_cruisingoff_callback );
  Top_SM.add( state_automatedcontrol, state_automatedcontrol_callback );
  /*subscribe to the states within the state_automatedcontrol*/
  state_automatedcontrol.add( state_accelerating, state_accelerating_callback );
  state_automatedcontrol.add( state_resuming, state_resuming_callback );
  state_automatedcontrol.add( state_cruising, state_cruising_callback );
  Top_SM.install( top_sigqueue );
  state_idle.install( idle_transitions );
  state_initial.install( initial_transitions );
  state_cruisingoff.install( cruisingoff_transitions );
  state_automatedcontrol.install( automated_transitions );
  state_accelerating.install( accel_transitions );
  state_resuming.install( resuming_transitions );
  state_cruising.install( cruising_transitions );
  os.add( CruiseControlTask, Top_SM, core::MEDIUM_PRIORITY, 100_ms );
}

void loop() {
  os.run();
}