//***************************************************************************
// CrcDuino Tutorial05.1 - example of a sequential program structure for autonomous robots; using if statements
//NOTE: In contrary to the other tutorials, the Normally Closed contact of the buttons and limit switch is used here.
//      This means that, when the button or switch is not press, "LOW" will be read since an electrical continuity to GND
//      is established. When they are pressed, the pin will therefore read "HIGH".
//      Why do we do that here, instead of the method using the NO contact? Just to show you that all paths lead to Rome :)
//***************************************************************************
#include <CrcLib.h>

//Renaming pins with significant names
#define NEXT_STEP_ODD CRC_DIG_1
#define NEXT_STEP_EVEN CRC_DIG_2

using namespace
Crc;

//Variable declaration
//Step management variables
bool stepSleep = LOW;
bool step1 = LOW;
bool step2 = LOW;
bool step3 = LOW;

//***************************************************************************

void setup() {
  CrcLib::Initialize();
  
  Serial.begin(2000000);  //Start the Serial Monitor communication for on computer screen display, at a high baud rate for faster code execution

  CrcLib::SetDigitalPinMode(NEXT_STEP_ODD,INPUT); //Digital ports need to be tell wether they are used as inputs or outputs
  CrcLib::SetDigitalPinMode(NEXT_STEP_EVEN,INPUT);
  
  //We want to be in sleep mode upon startup
  stepSleep = HIGH; step1 = LOW; step2 = LOW; step3 = LOW;
  
}

//***************************************************************************

void loop() {
  CrcLib::Update();
  //We continuously want to print on serial monitor the state of our program (what step it's in)
  //So we will print at every program scan ( a scan being an exectuion of "loop()"
  //Remember: LOW means "0", and HIGH means "anything but "0"
  Serial.print("stepSleep: "); Serial.print(stepSleep);
  Serial.print(" step1: "); Serial.print(step1);
  Serial.print(" step2: "); Serial.print(step2);
  Serial.print(" step3: "); Serial.print(step3);
  Serial.print(" scan time: "); Serial.println( CrcLib::GetDeltaTimeMicros() ); //Print the value returned by GetDeltaTime, which is the scan time 

//------------------
  if(stepSleep)
  {

    //The code you'd want to execute during stepSleep would go here

    //When the step exiting condition is met (we press the button/switch and the other one is not pressed)...
    if( CrcLib::GetDigitalInput(NEXT_STEP_ODD) == HIGH && CrcLib::GetDigitalInput(NEXT_STEP_EVEN) == LOW)  
    {
      //...we change the steps configuration to take another branch ( if() statement ) during the next scans
      stepSleep = LOW; step1 = HIGH; step2 = LOW; step3 = LOW;
    }
  }
//------------------
  if(step1)
  {

    //The code you'd want to execute during step1 would go here

    //When the step exiting condition is met (we press the button/switch and the other one is not pressed)...
    if( CrcLib::GetDigitalInput(NEXT_STEP_EVEN) == HIGH && CrcLib::GetDigitalInput(NEXT_STEP_ODD) == LOW )
    {
      //...we change the steps configuration to take another branch ( if() statement ) during the next scans
      stepSleep = LOW; step1 = LOW; step2 = HIGH; step3 = LOW;
    }  
  }
//------------------
  if(step2)
  {
    //The code you'd want to execute during step2 would go here

    //When the step exiting condition is met (we press the button/switch and the other one is not pressed)...
    if( CrcLib::GetDigitalInput(NEXT_STEP_ODD) && CrcLib::GetDigitalInput(NEXT_STEP_EVEN) == LOW )
    {
      stepSleep = LOW; step1 = LOW; step2 = LOW; step3 = HIGH;
    }
  }
//------------------
  if(step3)
  {
    //The code you'd want to execute during step3 would go here

    //When the step exiting condition is met (we press the button/switch and the other one is not pressed)...
    if( CrcLib::GetDigitalInput(NEXT_STEP_EVEN) == HIGH && CrcLib::GetDigitalInput(NEXT_STEP_ODD) == LOW )
    {
      //...we change the steps configuration to take another branch ( if() statement ) during the next scans
      stepSleep = HIGH; step1 = LOW; step2 = LOW; step3 = LOW;
    }
  }
//------------------
}