#include <Arduino_FreeRTOS.h>
#include <YA_FSM.h>

void TaskLoop1(void *parms);
void TaskLoop2(void *parms);
//void yellowFun(void *parms);

#define START_BTN 2
#define RESET_BTN 3

const byte WHITE_LED = 9; // idle
const byte GREEN_LED = 10; // running
const byte YELLOW_LED = 11; //starting
const byte RED_LED = 12;

// Create new FSM
YA_FSM stateMachine;

// State Alias
enum State {IDLE, SPEEDINGUP, RUNNING, SLOWDOWN, POWEROFF};

// Helper for print labels instead integer when state change
const char * const stateName[] PROGMEM = { "IDLE", "SPEEDINGUP", "RUNNING", "SLOWDOWN", "POWEROFF"};

// Pedestrian traffic light -> green ligth ON until button pressed
#define YELLOW_TIME  2000

// Input (trig transition from GREEN to CALL state, the others transitions on timeout)
bool callButton = false;
bool resetButton = false;

// Output variables
bool whiteLed = false;
bool redLed = false;
bool greenLed = false;
bool yellowLed = false;




/////////// STATE MACHINE CALLBACK FUNCTIONS //////////////////
// Define "on entering" callback function (the same for all "light" states)
void onEnter() {

}

// Define "on leaving" callback function (the same for all "light"  states)
void onExit() {
  //Serial.print(stateMachine.ActiveStateName());
  //Serial.println(F(" light OFF\n"));
}

// Define "on state" for yellow state
/*
void yellowFun(void *parms){
  //LOOP
  while(true) {
    for (int i = 0; i < 10; i++)
    {
    digitalWrite(YELLOW_LED,HIGH);
    vTaskDelay(100/portTICK_PERIOD_MS);
    digitalWrite(YELLOW_LED,LOW);
    vTaskDelay(100/portTICK_PERIOD_MS);
    Serial.println(i);
    if(!digitalRead(3)){
      vTaskDelete()
    }
    }
  }
}  
*/

void yellowFun() {
  for (int i = 0; i < 10; i++)
  {
    digitalWrite(YELLOW_LED,HIGH);
    vTaskDelay(100/portTICK_PERIOD_MS);
    digitalWrite(YELLOW_LED,LOW);
    vTaskDelay(100/portTICK_PERIOD_MS);
    Serial.println(i);
    if(!digitalRead(3)){
      break;
    }
  }
}

// Setup the State Machine
void setupStateMachine() {
  // Follow the order of defined enumeration for the state definition (will be used as index)
  // Add States => name,timeout, onEnter cb, onState cb, onLeave cb  
  stateMachine.AddState(stateName[IDLE], 0, onEnter, nullptr, onExit);
  stateMachine.AddState(stateName[SPEEDINGUP], YELLOW_TIME, onEnter, yellowFun, onExit);
  stateMachine.AddState(stateName[RUNNING], 0, onEnter, nullptr, onExit);
  stateMachine.AddState(stateName[SLOWDOWN], YELLOW_TIME, onEnter, yellowFun, onExit);
  stateMachine.AddState(stateName[POWEROFF], 0, onEnter, nullptr, onExit);

  // SE LO STATO é ATTIVO
  stateMachine.AddAction(IDLE, YA_FSM::N, whiteLed); //rimane acceso fino a che sono nello stato IDLE    
  stateMachine.AddAction(RUNNING, YA_FSM::N, greenLed); //rimane acceso fino a che sono nello stato RUNNING
  stateMachine.AddAction(POWEROFF, YA_FSM::N, redLed); //rimane acceso fino a che sono nello stato POWEROFF

  
  // Add "timed" transitions: it will be triggered on previous defined state timeout
  stateMachine.AddTimedTransition(SPEEDINGUP, RUNNING);
  stateMachine.AddTimedTransition(SLOWDOWN, POWEROFF);

  // Add transitions with related trigger bool variable
  stateMachine.AddTransition(IDLE,SPEEDINGUP,callButton);
  stateMachine.AddTransition(RUNNING,SLOWDOWN,callButton);
  //stateMachine.AddTransition(SLOWDOWN,POWEROFF,callButton); 
  //stateMachine.AddTransition(POWEROFF,IDLE,callButton);
  


  //RESET AT ANY TIME
  stateMachine.AddTransition(SPEEDINGUP,IDLE,resetButton);
  stateMachine.AddTransition(RUNNING,IDLE,resetButton);
  stateMachine.AddTransition(SLOWDOWN,IDLE,resetButton);
  stateMachine.AddTransition(POWEROFF,IDLE,resetButton);
  
}




void setup() {  
  xTaskCreate(TaskLoop1, "Loop1", 128, NULL, 1, NULL);
  xTaskCreate(TaskLoop2, "Loop2", 128, NULL, 1, NULL);
}





void loop() {}

// LED E AUDIO
//FINITE STATE MACHINE
void TaskLoop1(void *parms){
  //SETUP
  pinMode(START_BTN, INPUT_PULLUP);
  pinMode(RESET_BTN, INPUT_PULLUP);
  pinMode(GREEN_LED, OUTPUT);
  pinMode(YELLOW_LED, OUTPUT);
  pinMode(RED_LED, OUTPUT);
    Serial.begin(115200);
  Serial.println(F("Setup the Finite State Machine...\n"));
  setupStateMachine();  
  //fsmInfo(stateMachine);
  Serial.print(F("\nActive state: "));
  Serial.println(stateMachine.ActiveStateName());
  
  //LOOP
  while(true) {
    // Read inputs
  callButton = (digitalRead(START_BTN) == LOW);
  resetButton = (digitalRead(RESET_BTN) == LOW);
  // Update State Machine
  if (stateMachine.Update()) {    
    Serial.print(F("Active state: "));
    Serial.println(stateMachine.ActiveStateName());
  }
  // Set outputs
  digitalWrite(WHITE_LED,whiteLed);
  digitalWrite(RED_LED, redLed);
  digitalWrite(GREEN_LED, greenLed);
  digitalWrite(YELLOW_LED, yellowLed);  
  }
}



// qua metteremo l'ascolto di ROS
void TaskLoop2(void *parms){
  //SETUP
  pinMode(5, OUTPUT);
  pinMode(13, INPUT_PULLUP);
  Serial.println("ciao");
  //LOOP
  while(true) {
    if(!digitalRead(13)){
      digitalWrite(5, HIGH);
      vTaskDelay(100/portTICK_PERIOD_MS);  
      digitalWrite(5, LOW);
      vTaskDelay(300/portTICK_PERIOD_MS); 
      Serial.println(digitalRead(13));
    }
    else{
      digitalWrite(5,LOW);
    }
  }  
}