//////////////////////////////////////////////////////////////////////
// Revers Drive Driver
// 11.08.2023
// V1.1
//////////////////////////////////////////////////////////////////////
//  global variable definition & preprocessor def

//  enum type for Finite State Machine
typedef enum {
  FSM_ASM_STOP,    
  FSM_SIG_EDGE_DETECTION,   
  FSM_ASM_RUN_LEFT,    
	FSM_ASM_RUN_RIGHT,  
} asm_fsm_t;

// debug level 0 - no debug, 1 - normal level, 2 - high level
#define DEBUG_LEVEL 2
// time grain for main loop, default time is 10ms
#define MAIN_LOOP_TIME_GRAIN_MS	10
// time interval between movement in left and right, default time is 2000ms
#define STAND_OFF_TIME 2000
// ADPU type: 0 means 1.0v, 1 means 1.5v
#define ADPU_TYPE 1
// debounce time to check edge, default time is 10ms
#define DEBOUNCE_INTERVAL 20


// pinout definition for Arduino nano
int ASM_Left = A5;
int ASM_Right = A0;
//int ACF_ADPU10_230V = 6;
//int ACF_ADPU15_12V = 4;
int ACF_230V_12V = 4;


// global variables
bool current_signal_level = 0, last_signal_level = 0;
bool rising_edge_flag = 0, falling_edge_flag = 0, aux_flag = 0;
bool edge_detected = 0, rising_edge_detected = 0, falling_edge_detected = 0;
unsigned long previous_millis_Debounce = 0;
unsigned long previous_millis_StandOff = 0;
unsigned long previous_millis_Main_Loop = 0;
asm_fsm_t state = FSM_ASM_RUN_RIGHT;

//////////////////////////////////////////////////////////////////////
// setup definition 

void setup() {

  Serial.begin(9600);

  pinMode(ASM_Left, OUTPUT);
  pinMode(ASM_Right, OUTPUT);
  //pinMode(ACF_ADPU10_230V, INPUT_PULLUP);
  //pinMode(ACF_ADPU15_12V, INPUT_PULLUP);
  pinMode(ACF_230V_12V, INPUT_PULLUP);

  //safety: set drive stop during startup
  digitalWrite(ASM_Left, LOW);
  digitalWrite(ASM_Right, LOW);

  // Log after restart
  Serial.println("Arduino Nano restart ...");
        
}
//////////////////////////////////////////////////////////////////////
// function to detect input signal edge
static void Edge_Detection(void)
{ 
  
  // reading current level of input signal
  //#if ADPU_TYPE == 0
  //  current_signal_level = digitalRead(ACF_ADPU10_230V);
  //#else
  //  current_signal_level = digitalRead(ACF_ADPU15_12V);
  //#endif

  current_signal_level = digitalRead(ACF_230V_12V);

  // po restarcie przy zalaczonym przelaczniku na stan wysoki generuje sygnal

  // rising edge detection
  if ((current_signal_level == HIGH) && (last_signal_level == LOW)) {
    rising_edge_flag = 1;
    falling_edge_flag = 0;
    last_signal_level = current_signal_level;
    previous_millis_Debounce = millis();
    #if DEBUG_LEVEL > 1
      Serial.println("RISING edge flag set");
    #endif
  }

  // falling edge detection
  if ((current_signal_level == LOW) && (last_signal_level == HIGH)) {
    rising_edge_flag = 0;
    falling_edge_flag = 1;
    last_signal_level = current_signal_level;
    previous_millis_Debounce = millis();
    #if DEBUG_LEVEL > 1
      Serial.println("FALLING edge flag set");
    #endif
  }

  // debounce time to check HIGH level is stable
  if (rising_edge_flag == HIGH) {
    if (millis() - previous_millis_Debounce > DEBOUNCE_INTERVAL) {
      if (current_signal_level == HIGH) {
        edge_detected = 1;
        rising_edge_detected = 1;
        falling_edge_detected = 0;
        rising_edge_flag = 0;
        #if DEBUG_LEVEL > 1
          Serial.println("RISING edge detected");
        #endif
      } 
    }
  }

  // debounce time to check LOW level is stable
  if (falling_edge_flag == HIGH) {
    if (millis() - previous_millis_Debounce > DEBOUNCE_INTERVAL) {
      if (current_signal_level == LOW) {
        edge_detected = 1;
        rising_edge_detected = 0;
        falling_edge_detected = 1;
        falling_edge_flag = 0;
        #if DEBUG_LEVEL > 1
          Serial.println("FALLING edge detected");
        #endif
      } 
    }
  }

}
//////////////////////////////////////////////////////////////////////
// function to stop ASM drive
static void Drive_Stop(void)
{ 
  digitalWrite(ASM_Left, LOW);
  digitalWrite(ASM_Right, LOW); 
  #if DEBUG_LEVEL > 1
    Serial.println("Drive STOP");
  #endif
}
//////////////////////////////////////////////////////////////////////
// function to set ASM rotaton left
static void Drive_Left(void)
{ 
  digitalWrite(ASM_Left, HIGH);
  digitalWrite(ASM_Right, LOW); 
  #if DEBUG_LEVEL > 1
    Serial.println("Drive LEFT");
  #endif
}
//////////////////////////////////////////////////////////////////////
// function to set ASM rotation right
static void Drive_Right(void)
{ 
  digitalWrite(ASM_Left, LOW);
  digitalWrite(ASM_Right, HIGH); 
  #if DEBUG_LEVEL > 1
    Serial.println("Drive RIGHT");
  #endif
}
//////////////////////////////////////////////////////////////////////
// ASynchronous Motor Finite State Machine definition
static void ASM_FSM(void)
{
  switch (state)
  {
	  case FSM_ASM_STOP:
    	#if DEBUG_LEVEL > 0
        Serial.println("State: ASM STOP");
      #endif
      Drive_Stop();
      state = FSM_SIG_EDGE_DETECTION;
      aux_flag = 1;
      previous_millis_StandOff = millis();
  	  break;

    case FSM_SIG_EDGE_DETECTION:
      //state FSM_SIG_EDGE_DETECTION
      #if (DEBUG_LEVEL > 0)
        if (aux_flag) Serial.println("State: SIGNAL EDGE DETECTION"); 
      #endif
      if (millis() - previous_millis_StandOff > STAND_OFF_TIME)
      {
        if (rising_edge_detected == HIGH) {
          state = FSM_ASM_RUN_LEFT; aux_flag = 1; rising_edge_detected = 0;
        }

        if (falling_edge_detected == HIGH) {
          state = FSM_ASM_RUN_RIGHT; aux_flag = 1; falling_edge_detected = 0;
        }
      }
      else {
        if (aux_flag) Serial.println("Waiting..."); aux_flag = 0;
      }
      break;
   
    case FSM_ASM_RUN_LEFT:
      //state FSM_ASM_RUN_LEFT
      #if (DEBUG_LEVEL > 0)
        if (aux_flag) Serial.println("State: ASM RUN LEFT");
      #endif
      if (aux_flag) Drive_Left(); aux_flag = 0;
      break;

    case FSM_ASM_RUN_RIGHT:
      //state FSM_ASM_RUN_RIGHT
      #if (DEBUG_LEVEL > 0)
        if(aux_flag) Serial.println("State: ASM RUN RIGHT");
      #endif
      if (aux_flag) Drive_Right(); aux_flag = 0;
      break; 

    default:
      //IN CASE OF ERROR
      #if (DEBUG_LEVEL > 0)
        if(aux_flag) Serial.println("State: DEFAULT in case of error");
      #endif
      if(aux_flag) Drive_Stop(); aux_flag = 0;
      break;
	}
}
//////////////////////////////////////////////////////////////////////
// Main program here
void loop()
{  
  // call ASM_FSM state machine every TIME_GRAIN
  if (millis() - previous_millis_Main_Loop > MAIN_LOOP_TIME_GRAIN_MS)
  {
    // run edge detection
    Edge_Detection();
    //check result
    if(edge_detected == HIGH)
    {
      state = FSM_ASM_STOP;
      edge_detected = 0;
    }
    // call state machine
    ASM_FSM();
    // set current time as previous
    previous_millis_Main_Loop = millis();
  }
}