#include "project.h"
#include "RL_STT.h"

const int Conn_GPIO = 4;
const int MAS_GPIO = 38;

const int C_LK_GPIO = 37;
const int C_LB_GPIO = 36;
const int C_Ab_GPIO = 35;

const char* STATE_STR[] = {"START", "AARON", "LARRY", "ABBOT"};
Inputs input_values = {false,false,false,false,false,};

void setup() {
  Serial.begin(115200);
  Serial.println("Hello, ESP32-S3!");
  pinMode(Conn_GPIO, INPUT_PULLUP);
  pinMode(MAS_GPIO, INPUT_PULLUP);
  pinMode(C_LK_GPIO, INPUT_PULLUP);
  pinMode(C_LB_GPIO, INPUT_PULLUP);
  pinMode(C_Ab_GPIO, INPUT_PULLUP);
}

void loop() {
  delay(10);
  // setup input_values
  input_values.Conn_value = readConn();
  input_values.MAS_value = readMAS();
  input_values.C_LK_value = readC_LK();
  input_values.C_LB_value = readC_LB();
  input_values.C_Ab_value = readC_Ab();
    
  // run state_machine
  FSM_sstKernel(&currentState);
  // wait for 1 sec
  delay(MAIN_LOOP_DELAY);
}

bool readConn() {
  int data = digitalRead(Conn_GPIO);
  if (data == HIGH) {
    return true;
  }
  else {
    return false;
  }
}

bool readMAS() {
  int data = digitalRead(MAS_GPIO);
  if (data == HIGH) {
    return true;
  }
  else {
    return false;
  }
}

bool readC_LK() {
  int data = digitalRead(C_LK_GPIO);
  if (data == HIGH) {
    return true;
  }
  else {
    return false;
  }
}

bool readC_LB() {
  int data = digitalRead(C_LB_GPIO);
  if (data == HIGH) {
    return true;
  }
  else {
    return false;
  }
}

bool readC_Ab() {
  int data = digitalRead(C_Ab_GPIO);
  if (data == HIGH) {
    return true;
  }
  else {
    return false;
  }
}

/************************************************************/
/*              STT FSM                                     */
/************************************************************/

/* Events */
bool NoEvent(bool print) {
  if (print) Serial.printf("NoEvent\n");
    return true;
}

bool Conn_OFF(bool print) {
    if (print) Serial.printf("\tConn %d\n", input_values.Conn_value);
    return !input_values.Conn_value;
}
bool Conn_ON(bool print) {
    if (print) Serial.printf("\tConn %d\n", input_values.Conn_value);
    return input_values.Conn_value;
}

bool MAS_OFF(bool print) {
    if (print) Serial.printf("\tMAS %d\n", input_values.MAS_value);
    return !input_values.MAS_value;
}
bool MAS_ON(bool print) {
    if (print) Serial.printf("\tMAS %d\n", input_values.MAS_value);
    return input_values.MAS_value;
}

bool CLK_OFF(bool print) {
    if (print) Serial.printf("\tC_LK %d\n", input_values.C_LK_value);
    return !input_values.C_LK_value;
}
bool CLK_ON(bool print) {
    if (print) Serial.printf("\tC_LK %d\n", input_values.C_LK_value);
    return input_values.C_LK_value;
}

bool CLBG_OFF(bool print) {
    if (print) Serial.printf("\tC_LB %d\n", input_values.C_LB_value);
    return !input_values.C_LB_value;
}
bool CLBG_ON(bool print) {
    if (print) Serial.printf("\tC_LB %d\n", input_values.C_LB_value);
    return input_values.C_LB_value;
}

bool CAB_OFF(bool print) {
    if (print) Serial.printf("\tC_Ab %d\n", input_values.C_Ab_value);
    return !input_values.C_Ab_value;
}
bool CAB_ON(bool print) {
    if (print) Serial.printf("\tC_Ab %d\n", input_values.C_Ab_value);
    return input_values.C_Ab_value;
}

/* Actions */
void NoAction() {}

void LBG_ON() {
    Serial.printf("\tLB ON\n");
}
void LBG_OFF() {
    Serial.printf("\tLB OFF\n");
}

void LKG_ON() {
    Serial.printf("\tLKG ON\n");
}
void LKG_OFF() {
    Serial.printf("\tLKG ON\n");
}

void LBRG_ON() {
    Serial.printf("\tLBRG ON\n");
}
void LBRG_OFF() {
    Serial.printf("\tLBRG ON\n");
}

void AbG_ON() {
    Serial.printf("\tAbG ON\n");
}
void AbG_OFF() {
    Serial.printf("\tAbG ON\n");
}

void LRON() {
    Serial.printf("\tLRON ON\n");
}
void LROF() {
    Serial.printf("\tLROF OFF\n");
}

void FSM_sstKernel(S_FSM_STT_T * rl_stt) {
    int row, evn, act;
    int events;
    bool match = false;

    for (row = 0; row < MAX_ROWS; row++) {        
        events = 1;
        if (rl_stt->sttRow[row]->presentState == rl_stt->currentState) {
            for (evn = 0; evn < MAX_EVENTS; evn++) {
                events *= rl_stt->sttRow[row]->event[evn](false);
            }
            if (events) {
                match = true;
                Serial.printf("matching STT row = %i\n", row);
                for (evn = 0; evn < MAX_EVENTS; evn++) {
                    events *= rl_stt->sttRow[row]->event[evn](true);
                }
                Serial.printf("Initial state is %s\n", STATE_STR[rl_stt->currentState]);
                for (act = 0; act < MAX_ACTIONS; act++) {
                    rl_stt->sttRow[row]->action[act]();
                }
                rl_stt->currentState = rl_stt->sttRow[row]->nextState;
                Serial.printf("Final state is %s\n\n", STATE_STR[rl_stt->currentState]);
                break;
            }
        }
    }
    if (!match)  Serial.printf("No matching rows\n");
}