#include <Arduino.h>
#include <freertos/FreeRTOS.h>
#include <freertos/task.h>

TaskHandle_t threadX;
TaskHandle_t threadY;
TaskHandle_t threadBtn;

int stepX = 5;
int dirX = 4;
int stepY = 6;
int dirY = 7;

bool endX = false;
bool endY = false;
bool startSequence = false;

int btn = 1;
int led = 2;

int moves = 0; // Variable para controlar el estado del movimiento
unsigned long lastActionTime = 0; // Variable para el control del tiempo
const unsigned long waitTime = 2000; // Tiempo de espera entre acciones en milisegundos

void threadMoveX(void* parameter) {
  for (;;) {
    if (startSequence && endX && (millis() - lastActionTime >= waitTime)) {
      switch (moves) {
        case 0:
          moveMotorX(500, 1);
          break;
        case 1:
          moveMotorX(300, 1);
          break;
        case 2:
          moveMotorX(500, 0);
          break;
      }
      endX = false;
      lastActionTime = millis(); // Actualiza el tiempo de la última acción
    }
    vTaskDelay(10);
  }
}

void threadMoveY(void* parameter) {
  for (;;) {
    if (startSequence && endY && (millis() - lastActionTime >= waitTime)) {
      switch (moves) {
        case 0:
          moveMotorY(400, 1);
          break;
        case 1:
          moveMotorY(200, 1);
          break;
        case 2:
          moveMotorY(200, 1);
          break;
      }
      endY = false;
      lastActionTime = millis(); // Actualiza el tiempo de la última acción
    }
    vTaskDelay(10);
  }
}

void threadButton(void* parameter) {
  for (;;) {
    if (digitalRead(btn) == LOW) {
      if (!startSequence) {
        // Inicia la secuencia de movimientos
        startSequence = true;
        endX = true;
        endY = true;
        digitalWrite(led, HIGH); // Enciende el LED cuando se presiona el botón
      }
    }
    else {
      if (startSequence && !endX && !endY && (millis() - lastActionTime >= waitTime)) {
        // Completa el movimiento actual y pasa al siguiente
        startSequence = false;
        moves = (moves + 1) % 3; // Incrementa el estado del movimiento y cicla entre 0, 1 y 2
        endX = true;
        endY = true;
        digitalWrite(led, LOW); // Apaga el LED cuando se completa un ciclo
      }
    }
    vTaskDelay(10);
  }
}

void setup() {
  Serial.begin(115200);
  Serial.println("Hello, ESP32-C3!");
  pinMode(stepX, OUTPUT);
  pinMode(dirX, OUTPUT);
  pinMode(stepY, OUTPUT);
  pinMode(dirY, OUTPUT);
  pinMode(btn, INPUT_PULLUP);
  pinMode(led, OUTPUT);
  xTaskCreatePinnedToCore(threadMoveX, "threadX", 10000, NULL, 1, &threadX, 0);
  xTaskCreatePinnedToCore(threadMoveY, "threadY", 10000, NULL, 1, &threadY, 0);
  xTaskCreatePinnedToCore(threadButton, "threadBtn", 10000, NULL, 1