#include <Arduino_FreeRTOS.h>
#include <EventButton.h>


class Load {
private:
    byte pin;
    bool _isPoweredOn = false;
public:
    Load() {} // do not use
    Load(byte pin) {
        this->pin = pin;
    }

    bool isPoweredOn() const {
        return _isPoweredOn;
    }

    void init() {
        _isPoweredOn = false;
        pinMode(pin, OUTPUT);
    }

    void setState(byte newState) {
        if (newState == HIGH) {
            _isPoweredOn = true;
            digitalWrite(pin, HIGH);
        } else {
            _isPoweredOn = false;
            digitalWrite(pin, LOW);
        }
    }
};


// Possible States
enum PowerButtonState { POB_POWER_OFF, POB_POWER_ON };
enum PlayButtonState { PLB_WAIT_START, PLB_RUNNING, PLB_PAUSE };
enum ForwardButtonState { FRB_STOP, FRB_RUNNING };
enum ReverseButtonState { REB_STOP, REB_RUNNING };
enum LedState { OFF, ON };

// States 
PowerButtonState powerButtonState;
PlayButtonState playButtonState;
ForwardButtonState forwardButtonState;
ReverseButtonState reverseButtonState;
bool lastBuzzerState;
byte driver1State;
byte pauseLEDState;
byte driver2State;
byte time_driver;

unsigned long lastPrintTime;
unsigned long printDuration;

// Initilize the State Machine
void initStateMachine() {  
  vTaskSuspendAll();
  // States 
  powerButtonState = POB_POWER_OFF;
  playButtonState = PLB_WAIT_START;
  forwardButtonState = FRB_STOP;
  reverseButtonState = REB_STOP;
  lastBuzzerState = false;
  driver1State = 0;
  pauseLEDState = 0;
  driver2State = 0;
  lastPrintTime = 0;
  time_driver = 0;
  printDuration = 2000;
  //--//
  // vTaskResumeAll();
  vTaskSuspend(NULL);
}



// Timer timer(2*60000, initStateMachine);
// Pin Definitions
EventButton button1(2);
EventButton button2(3);
EventButton button3(4);
EventButton button4(5);
Load powerLed(6);
Load buzzer(7);
Load load1(8);
Load pauseLed(9);
Load load2(10);

TaskHandle_t task1, task2, task3;



void onbutton1Clicked(EventButton& eb) {
  // Serial.print("button1 clickCount: "); Serial.println(eb.clickCount());
  switch (powerButtonState) {
    case POB_POWER_ON:
      initStateMachine();
      break;

    case POB_POWER_OFF:
      vTaskSuspendAll();
      powerButtonState = POB_POWER_ON;
      break;
  }
}

void onbutton2Clicked(EventButton& eb) {
  // Serial.print("button2 clickCount: "); Serial.println(eb.clickCount());
  if (powerButtonState == POB_POWER_ON && forwardButtonState == FRB_STOP && reverseButtonState == REB_STOP) {
    
    switch (playButtonState) {
      case PLB_WAIT_START:
        time_driver = 1;
        playButtonState = PLB_RUNNING;
        vTaskResume(task1);
        // resume task 1
        break;

      case PLB_RUNNING:
        pauseLEDState = 1;
        playButtonState = PLB_PAUSE;
        vTaskSuspendAll();
        vTaskResume(task2);
        break;

      case PLB_PAUSE:
        pauseLEDState = 0;
        if (time_driver == 1) {
          vTaskResume(task1);
        } 
        else if(time_driver == 3) {
          vTaskResume(task3);
        }
        playButtonState = PLB_RUNNING;
        break;
    }
  }
}


void setup() {
  Serial.begin(9600);
  delay(500);
  Serial.println(F("Starting State Machine...\n"));

  load1.init();
  load2.init();
  pauseLed.init();

  //Link the event(s) to your function
  button1.setClickHandler(onbutton1Clicked);
  button2.setClickHandler(onbutton2Clicked);
  // button3.setClickHandler(onbutton3Clicked);
  // button4.setClickHandler(onbutton4Clicked);

  xTaskCreate(task1Function, "Time Driver 1", 100, NULL, 1, &task1);
  xTaskCreate(task2Function, "Time Pause", 100, NULL, 1, &task2);
  xTaskCreate(task3Function, "Time Driver 2", 100, NULL, 1, &task3);
  vTaskSuspend(task2);
  vTaskSuspend(task3);

  vTaskStartScheduler();
}

void loop() {
  // Empty loop because tasks handle the functionality
  button1.update();
  button2.update();
  button3.update();
  button4.update();
}

void task1Function(void* pvParameters) {
  for (;;) {
    load1.setState(HIGH);
    vTaskDelay(pdMS_TO_TICKS(5000));
    load1.setState(LOW);
    vTaskResume(task2);
    vTaskSuspend(task1);  // Suspend itself after completion
  }
}

void task2Function(void* pvParameters) {
  for (;;) {
    pauseLed.setState(HIGH);
    vTaskDelay(pdMS_TO_TICKS(5000));
    pauseLed.setState(LOW);
    vTaskResume(task3);  // Resume task3 to start the sequence
    vTaskSuspend(task2);  // Suspend itself after completion
  }
}

void task3Function(void* pvParameters) {
  for (;;) {
    load2.setState(HIGH);
    vTaskDelay(pdMS_TO_TICKS(5000));
    load2.setState(LOW);
    vTaskResume(task1);  // Resume task1 to start the sequence again
    vTaskSuspend(task3);  // Suspend itself after completion
  }
}