/*
 * FSM demo
 * 
 * Seat belt alarm
 * 
 * button pin 4 (low side) pressed to simulate car motion. 
 *   a series of presses over a few seconds are required to simulate  motion to start the 
     seat belt check sequence.

 * slide switch pin 5 (low = seat belt engaged OK, otherwise seat belt NOT OK )
 * 
 * 
 * 
 */
 
enum state_t {
  INIT, 
  START,
  WAITING_FOR_CAR_IN_MOTION,
  CAR_CONFIRMED_IN_MOTION,
  IN_GRACE_PERIOD,
  IN_WARNING_1,
  IN_WARNING_2,
  SEAT_BELT_OK,
  SEAT_BELT_NOK_ACCEPTED,
} ;

state_t state = state_t::INIT ;
state_t oldState = state_t::INIT ;
uint32_t inCurrentStateAtMs ;
bool stateChangeNew = false ;

const uint8_t movPin = 4 ;    // active low - press on and off 
const uint8_t seatPin = 5 ;   // active low




void setState( state_t newState ) {
  // Note: any printing here of the states will be out of phase with the model
  oldState = state ;
  state = newState ;
  inCurrentStateAtMs = millis() ;
  stateChangeNew = true ;
}


void setup() {
  Serial.begin( 115200 ) ;
  Serial.println("FSM Demo") ;
  pinMode( movPin, INPUT_PULLUP ) ;
  pinMode( seatPin, INPUT_PULLUP ) ;
  setState( state_t::START ) ;
}

void loop() {

  
  static uint32_t lastMovementCheckAtMs = 0  ;
  static uint32_t lastWarning1AtMs = 0  ;
  static uint32_t lastWarning2AtMs = 0  ;
  static bool movementInCurrentWindow = false ;
 
  static bool carDeemedStopped = true ;
  static uint32_t lastMovAtMs = millis() ;

  static uint32_t lastLoopAtMs = 0 ;

  uint32_t ms = millis() ;

  // we execute the main loop once every Xms to debounce keys etc
  if ( ms - lastLoopAtMs > 100 ) lastLoopAtMs = ms ;
  else return ; 


  if ( !digitalRead( movPin ) ) {
    lastMovAtMs = ms ;
    carDeemedStopped = false ;
    movementInCurrentWindow = true ;
  }
  else if ( ! carDeemedStopped && ms - lastMovAtMs > 25000 ) {
    carDeemedStopped = true ;
     movementInCurrentWindow = false ;
  }
  
/*
  // debug messages (in phase)
  if (  stateChangeNew ) {
    Serial.print( "+++ currentState=" ) ;
    Serial.print(  (int)oldState ) ;
    Serial.print( "; newState=" ) ;
    Serial.println(  (int)state ) ;
    stateChangeNew = false ;
  }
*/

  // general rule. skip through if seat belt engaged OK. 
 if ( !digitalRead( seatPin)  && state != state_t::SEAT_BELT_OK && state != state_t::START) {
   setState( state_t::SEAT_BELT_OK ) ;
 }



  switch ( state ) {

    case state_t::START : {
        if ( state != oldState) {
          // we are newly in this state
          oldState = state ;
          Serial.println( "$$$ entering START" ) ;
        }
        if ( movementInCurrentWindow )  {
          setState( state_t::WAITING_FOR_CAR_IN_MOTION ) ;
          movementInCurrentWindow = false ;
        }
        break ;
      }


    case state_t::WAITING_FOR_CAR_IN_MOTION : {
        if ( state != oldState) {
          // we are newly in this state
          oldState = state ;
          Serial.println( "$$$ entering WAITING_FOR_CAR_IN_MOTION" ) ;
        }
        if ( ms - lastMovementCheckAtMs > 1000 ) {
          // we have to see a new movement every iteration for the 3 conseutive seconds
          // or we revert to start
          if ( ! movementInCurrentWindow ) setState( state_t::START ) ;
          if ( ms - inCurrentStateAtMs > 3000 ) setState( state_t::CAR_CONFIRMED_IN_MOTION) ;
          lastMovementCheckAtMs = ms ;
          movementInCurrentWindow = false ;
        }
        break ;
      }


    case state_t::CAR_CONFIRMED_IN_MOTION : {
        if ( state != oldState) {
          // we are newly in this state
          oldState = state ;
          Serial.println( "$$$ entering CAR_CONFIRMED_IN_MOTION" ) ;
        }
        // state possibly redundant
        if ( ms - inCurrentStateAtMs > 100 ) {
          setState( state_t::IN_GRACE_PERIOD ) ;
        }
        break ;
      }


    case state_t::IN_GRACE_PERIOD : {
        if ( state != oldState) {
          // we are newly in this state
          oldState = state ;
          Serial.println( "$$$ entering IN_GRACE_PERIOD" ) ;
        }
        if ( ms - inCurrentStateAtMs > 4000 ) {
          setState( state_t::IN_WARNING_1 ) ;
        }
        break ;
      }


    case state_t::IN_WARNING_1 : {
        if ( state != oldState) {
          // we are newly in this state
          oldState = state ;
          Serial.println( "$$$ entering IN_WARNING_1" ) ;
        }
        if ( ms - inCurrentStateAtMs > 5000 ) {
          setState( state_t::IN_WARNING_2 ) ;
        }
        else if (  ms -  lastWarning1AtMs  > 1000 ) {
          // beep
          Serial.println( ">>> beep1" ) ;
          lastWarning1AtMs = ms ;
        }
        break ;
      }


    case state_t::IN_WARNING_2 : {
        if ( state != oldState) {
          // we are newly in this state
          oldState = state ;
          Serial.println( "$$$ entering IN_WARNING_2" ) ;
        }
        if ( ms - inCurrentStateAtMs > 10000 ) {
          setState( state_t::SEAT_BELT_NOK_ACCEPTED ) ;
        }
        else if ( ms - lastWarning2AtMs > 400 ) {
          // beep
          Serial.println( ">>> beep2" ) ;
          lastWarning2AtMs = ms ;
        }
        break ;
      }


    case state_t::SEAT_BELT_OK : {
        if ( state != oldState) {
          // we are newly in this state
          oldState = state ;
          Serial.println( "$$$ entering SEAT_BELT_OK" ) ;
        }
        // handle case where user removes seatbelt
        if ( digitalRead( seatPin) ) setState( state_t::CAR_CONFIRMED_IN_MOTION ) ;
        if ( carDeemedStopped ) setState( state_t::START ) ;
        break ;
      }


    case state_t::SEAT_BELT_NOK_ACCEPTED : {
        if ( state != oldState) {
          // we are newly in this state
          oldState = state ;
          Serial.println( "$$$ entering SEAT_BELT_NOK_ACCEPTED" ) ;
        }
        // exit by general - if car not in motion for X seconds - setState( state_t::START ) ;
        if ( carDeemedStopped ) setState( state_t::START ) ;
        break ;
      }

  }  // switch

}  // loop