//////////////////////////////////////////////////////////////////////
// ASM Temp Profile
// Dominik Kowalczyk
// 20.11.2024
// V1.0
//////////////////////////////////////////////////////////////////////
//  global variable definition & preprocessor def

//  enum type for Finite State Machine
typedef enum {
  TIME_PROG_1,    
  TIME_PROG_2,   
  TIME_PROG_3,    
  TIME_PROG_4,  
  TIME_PROG_5,  
} fsm_time_t;

typedef enum {
  ASM_STATE_1,    
  ASM_STATE_2,   
  ASM_STATE_3, 
  ASM_STATE_4, 
  ASM_STATE_5, 
  ASM_STATE_6,
  ASM_STATE_7,  
  ASM_STATE_8,    
} fsm_asm_t;

// debug level 0 - no debug, 1 - normal level, 2 - high level, clk level
#define DEBUG_LEVEL 1
// 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
// debounce time to check edge, default time is 20ms
#define DEBOUNCE_INTERVAL 20


// pinout definition for Arduino nano
// A7, A6 ONLY !!! analog input

//int Analog_In_1 = A7
//int Analog_In_2 = A6

int ASM_Left = A5;
//int Digital_Out_1 = A5;
//int Digital_Out_2 = A4;
//int Digital_Out_3 = A3;
//int Digital_Out_4 = A2;
//int Digital_Out_5 = A1;
//int Digital_Out_6 = A0;
int ASM_Right = A0;

//int Digital_In_1 = 2;
//int Digital_In_2 = 3;
//int Digital_In_3 = 4;
int ACF_Dir = 4;
//int Digital_In_4 = 5;
//int Digital_In_5 = 6;
//int Digital_In_6 = 7;
//int Digital_In_7 = 8;
//int Digital_In_8 = 9;
//int Digital_In_9 = 10;
//int Digital_In_10 = 11;
//int Digital_In_11 = 12;

// global variables

bool current_signal_level = 0, last_signal_level = 0;
bool rising_edge_flag = 0, falling_edge_flag = 0; 
bool edge_detected = 0, rising_edge_detected = 0, falling_edge_detected = 0; 
//bool aux_flag_2 = 1;
bool drive_control_debug = 1;
bool drive_control_setup = 1;
bool time_prog_debug = 1;
bool time_prog_setup = 1;
bool time_counter_flag = 1;

unsigned long previous_millis_Debounce = 0;
unsigned long previous_millis_StandOff = 0;
unsigned long previous_millis_Main_Loop = 0;
unsigned long previous_millis_Timer = 0;
unsigned long previous_millis_Run = 0;

unsigned long T_Hour = 0;
int T_Min = 0;
int T_Sec = 0;
int T_Min_Prev = 0;
int T_Sec_Prev = 0;
int T_Right = 0;
int T_Left = 0;
int T_Right_Count = 0;
int T_Left_Count = 0;

// initial state machine

fsm_time_t state_time = TIME_PROG_1;
fsm_asm_t state_asm = ASM_STATE_1;

//////////////////////////////////////////////////////////////////////
// setup definition 
void setup() {

  // UART initialization
  Serial.begin(9600);

  // pin direction definition
  pinMode(ASM_Left, OUTPUT);
  pinMode(ASM_Right, OUTPUT);
  pinMode(ACF_Dir, INPUT_PULLUP);

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

  // Log after restart
  Serial.println("Arduino Nano restart ...");
  Serial.println("Wait until ADPU restart");
  delay(10000);
        
}
//////////////////////////////////////////////////////////////////////
// function to detect input signal edge
static void Edge_Detection(void)
{ 
  
  // reading current level of input signal
  //#if ADPU_TYPE == 0
  // Case 1
  //#else
  // Case 2
  //#endif

  current_signal_level = digitalRead(ACF_Dir);

  // 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 == 2
      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 == 2
      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 == 2
          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 == 2
          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 == 2
    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 == 2
    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 == 2
    Serial.println("Drive RIGHT");
  #endif
}
//////////////////////////////////////////////////////////////////////
// function set ASM rotation 
static void Drive_Control(void)
{ 
  // call fsm_asm
  switch (state_asm)
  {
    case ASM_STATE_1:
      // State Debug 
      #if DEBUG_LEVEL >= 1
        if (drive_control_debug) Serial.println("ASM: Initialization");
        drive_control_debug = 0;
      #endif

      // State Setup
      previous_millis_StandOff = millis();

      // State Loop

      // State Transition
      state_asm = ASM_STATE_2;
      drive_control_debug = 1;
      break;

    case ASM_STATE_2:
      // State Debug 
      #if DEBUG_LEVEL >= 1
      if (drive_control_debug) Serial.println("ASM: Wait ... 2s");
        drive_control_debug = 0;
      #endif

      // State Setup

      // State Loop

      // State Transition
      if (millis() - previous_millis_StandOff > STAND_OFF_TIME)
      {
        state_asm = ASM_STATE_3;
        drive_control_debug = 1;
      }
      break;

    case ASM_STATE_3:
      // State Debug 
      #if DEBUG_LEVEL >= 1
        if (drive_control_debug) Serial.println("ASM: Run right");
        drive_control_debug = 0;
      #endif

      // State Setup
      Drive_Right();
      previous_millis_Run = millis();
      T_Right_Count = T_Right * 1000;

      // State Loop

      // State Transition
      state_asm = ASM_STATE_4;
      drive_control_debug = 1;
      break;

    case ASM_STATE_4:
      // State Debug 
      #if DEBUG_LEVEL == 2
        if (drive_control_debug) Serial.println("ASM: Wait at rigth rotation ...");
        drive_control_debug = 0;
      #endif
      
      // State Setup

      // State Loop

      // State Transition
      if (millis() - previous_millis_Run > T_Right_Count)
      {
        state_asm = ASM_STATE_5;
        drive_control_debug = 1;
        Drive_Stop();
        previous_millis_StandOff = millis();
      }
      break;

    case ASM_STATE_5:
      // State Debug
      #if DEBUG_LEVEL >= 1
      if (drive_control_debug) Serial.println("ASM: Wait ... 2s");
        drive_control_debug = 0;
      #endif

      // State Setup

      // State Loop

      // State Transition
      if (millis() - previous_millis_StandOff > STAND_OFF_TIME)
      {
        state_asm = ASM_STATE_6;
        drive_control_debug = 1;
      }
      break;

      case ASM_STATE_6:
      // State Debug
      #if DEBUG_LEVEL >= 1
        if (drive_control_debug) Serial.println("ASM: Run left");
        drive_control_debug = 0;
      #endif

      // State Setup
      Drive_Left();
      previous_millis_Run = millis();
      T_Left_Count = T_Left * 1000;

      // State Loop

      // State Transition
      state_asm = ASM_STATE_7;
      drive_control_debug = 1;
      break;

    case ASM_STATE_7:
      // State Debug
      #if DEBUG_LEVEL == 2
        if (drive_control_debug) Serial.println("ASM: Wait at left rotation ...");
        drive_control_debug = 0;
      #endif

      // State Setup

      // State Loop

      // State Transition
      if (millis() - previous_millis_Run > T_Left_Count)
      {
        state_asm = ASM_STATE_8;
        drive_control_debug = 1; 
        Drive_Stop();
        previous_millis_StandOff = millis();
      }
      break;

    case ASM_STATE_8:
      // State Debug
      #if DEBUG_LEVEL == 2
        if (drive_control_debug) Serial.println("ASM: Current time");
        drive_control_debug = 0;
      #endif

      // State Setup
      time_counter_flag = 1;

      // State Loop

      // State Transition
      state_asm = ASM_STATE_2;
      drive_control_debug = 1; 
      break;

    default:
      // State Setup
      Serial.println("ASM: ERROR - STOP");
      Drive_Stop();

      // State Transition
      state_asm = ASM_STATE_1;
      break;
  }
}
//////////////////////////////////////////////////////////////////////
// Finite State Machine definition
static void TIME_PROG_FSM(void)
{
  switch (state_time)
  {
	  case TIME_PROG_1:
      // State Debug
    	#if DEBUG_LEVEL > 0
        if (time_prog_debug) Serial.println(">>> TIME PROG 1: 9h 20s/20s");
        time_prog_debug = 0;
      #endif

      // State Setup
      if (time_prog_setup)
      {
        time_prog_setup = 0;
        T_Right = 20;
        T_Left = 20;
      }
      
      // State Loop
      Drive_Control();

      // State Transition
      //if (T_Min >= 1)
      if (T_Hour >= 1)
      {
        state_time = TIME_PROG_5;
        time_prog_debug = 1;
        Drive_Stop();
      }
   	  break;

    case TIME_PROG_2:
      // free to use
      break;

    case TIME_PROG_3:
    // free to use
      break;

    case TIME_PROG_4:
    // free to use
      break;

    case TIME_PROG_5:
      // STOP
      #if (DEBUG_LEVEL > 0)
        if (time_prog_debug) Serial.println(">>> TIME PROG 5: PROGRAM COMPLETED");
        time_prog_debug = 0;
      #endif
      break;      

    default:
      // State Setup
      Serial.println(">>> ERROR: PROGRAM STOP");
      Drive_Stop();
      break;
	}
}
//////////////////////////////////////////////////////////////////////
// function Timer
static void TIMER_SYS(void)
{ 
  if (millis() - previous_millis_Timer > 1000)
  { 
    T_Sec ++;

    if (T_Sec > 59)
    { 
      T_Min ++;
      T_Sec = 0;
    } 

    if (T_Min > 59)
    { 
      T_Hour ++;
      T_Min = 0;
    }

    if (time_counter_flag)
    {
      Serial.println();
      Serial.print("Time: ");
      Serial.print(T_Hour);
      Serial.print(":");
      Serial.print(T_Min);
      Serial.print(":");
      Serial.println(T_Sec);
      Serial.println();
      time_counter_flag = 0;
    }

    previous_millis_Timer = millis();

  }
}
//////////////////////////////////////////////////////////////////////
// Main program here
void loop()
{  
  // call TIMER_SYS to update time
  TIMER_SYS();
  // call ASM_FSM state machine every TIME_GRAIN
  if (millis() - previous_millis_Main_Loop > MAIN_LOOP_TIME_GRAIN_MS)
  {

    // call state machine
    TIME_PROG_FSM();
    
    // set current time as previous
    previous_millis_Main_Loop = millis();
  }
}