#include <Arduino_FreeRTOS.h>

// Definición de pines
#define PALLET_PIN 2
#define CYCLE_PIN 3
#define HOME_PIN 4
#define ERROR_PIN 5
#define EMERGENCY_STOP_PIN 6
#define REARME_PIN 7
#define LABEL_PIN 8
#define OK_LED_PIN 9

// Definición de tiempos
#define APL_TIME 5000 // 5 segundos
#define WAIT_TIME 2000 // 2 segundos

// Definición de tareas
void taskCheckPallet(void *pvParameters);
void taskControlCycle(void *pvParameters);
void taskCheckHome(void *pvParameters);
void taskCheckError(void *pvParameters);
void taskBlinkOKLED(void *pvParameters);

// Variables globales
bool palletState = true; // Inicializado en estado alto
bool cycleState = true; // Inicializado en estado alto
bool homeState = true; // Inicializado en estado alto
bool errorState = false; // Inicializado en estado bajo
bool emergencyStopState;
bool rearmeState;
bool labelState;
bool labelPressed = false;

void setup() {
  // Inicialización de pines
  pinMode(PALLET_PIN, INPUT);
  pinMode(CYCLE_PIN, OUTPUT);
  pinMode(HOME_PIN, OUTPUT);
  pinMode(ERROR_PIN, OUTPUT);
  pinMode(EMERGENCY_STOP_PIN, INPUT);
  pinMode(REARME_PIN, INPUT);
  pinMode(LABEL_PIN, INPUT);
  pinMode(OK_LED_PIN, OUTPUT);

  // Creación de tareas
  xTaskCreate(taskCheckPallet, "CheckPallet", 100, NULL, 1, NULL);
  xTaskCreate(taskControlCycle, "ControlCycle", 100, NULL, 2, NULL);
  xTaskCreate(taskCheckHome, "CheckHome", 100, NULL, 3, NULL);
  xTaskCreate(taskCheckError, "CheckError", 100, NULL, 4, NULL);
  xTaskCreate(taskBlinkOKLED, "BlinkOKLED", 100, NULL, 1, NULL);
}

void loop() {
  // No se realiza nada en el loop, todas las operaciones se manejan mediante las tareas
}

void taskCheckPallet(void *pvParameters) {
  TickType_t lastTime = xTaskGetTickCount();
  while (true) {
    bool currentPalletState = digitalRead(PALLET_PIN);
    if (currentPalletState != palletState) {
      // Cambio en el estado del pallet
      vTaskDelay(WAIT_TIME);
      digitalWrite(CYCLE_PIN, LOW);
      digitalWrite(HOME_PIN, LOW);
      if (!currentPalletState) {
        // PALLET cambió a bajo
        vTaskDelay(APL_TIME);
        if (!labelPressed) {
          // Esperar cambio bajo de LABEL
          while (digitalRead(LABEL_PIN)) {
            vTaskDelay(10);
          }
        }
        digitalWrite(HOME_PIN, HIGH);
        digitalWrite(CYCLE_PIN, HIGH);
      }
      palletState = currentPalletState;
    }
    vTaskDelayUntil(&lastTime, 10); // CheckPallet se ejecuta cada 10ms
  }
}

void taskControlCycle(void *pvParameters) {
  while (true) {
    if (palletState) {
      digitalWrite(CYCLE_PIN, HIGH);
    } else {
      digitalWrite(CYCLE_PIN, LOW);
    }
    vTaskDelay(10); // ControlCycle se ejecuta cada 10ms
  }
}

void taskCheckHome(void *pvParameters) {
  while (true) {
    if (digitalRead(HOME_PIN)) {
      digitalWrite(HOME_PIN, HIGH);
    } else {
      digitalWrite(HOME_PIN, LOW);
    }
    vTaskDelay(10); // CheckHome se ejecuta cada 10ms
  }
}

void taskCheckError(void *pvParameters) {
  while (true) {
    emergencyStopState = digitalRead(EMERGENCY_STOP_PIN);
    rearmeState = !digitalRead(REARME_PIN);
    labelState = !digitalRead(LABEL_PIN);

    if (emergencyStopState && homeState && rearmeState) {
      errorState = false;
    } else {
      errorState = true;
    }

    if (errorState) {
      digitalWrite(ERROR_PIN, LOW);
    } else {
      digitalWrite(ERROR_PIN, HIGH);
    }
    vTaskDelay(10); // CheckError se ejecuta cada 10ms
  }
}

void taskBlinkOKLED(void *pvParameters) {
  while (true) {
    if (labelPressed) {
      digitalWrite(OK_LED_PIN, HIGH);
      vTaskDelay(2000);
      digitalWrite(OK_LED_PIN, LOW);
      labelPressed = false;
    }
    vTaskDelay(10); // BlinkOKLED se ejecuta cada 10ms
  }
}