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 step = false;

int btn = 1;
int led = 2;

int moves = 0;

void threadMoveX(void* parameter) {
  for (;;) {
    if (endX == true) {
      switch (moves) {
        case 0:
          moveMotorX(500, 1);
          break;
        case 1:
          moveMotorX(200, 1);
          break;
        case 2:
          moveMotorX(500, 0);
          break;
      }
      endX = false;
    }
    vTaskDelay(1);
  }
}

void threadMoveY(void* parameter) {
  for (;;) {
    if (endY == true) {
      switch (moves) {
        case 0:
          moveMotorY(400, 1);
          break;
        case 1:
          moveMotorY(200, 1);
          break;
        case 2:
          moveMotorY(200, 0);
          break;
      }
       endY = false;
    }
    vTaskDelay(1);
  }
}

void threadButton(void* parameter) {
  for (;;) {
    if (digitalRead(btn) == LOW) {
      endX = true;
      endY = true;
      step = true;
    }
    if (endX == false && endY == false && step == true) {
      step = false;
      moves++;
      digitalWrite(led, HIGH);
    }
    else {
      digitalWrite(led, LOW);
    }
    vTaskDelay(1);
  }
}

void setup() {
  // put your setup code here, to run once:
  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, NULL, 0);
  xTaskCreatePinnedToCore(threadMoveY, "threadY", 10000, NULL, 1, NULL, 0);
  xTaskCreatePinnedToCore(threadButton, "threadBtn", 10000, NULL, 1, NULL, 0);
}

void loop() {
  vTaskDelay(1000);
  Serial.println("ok");
}


void moveMotorX(int steps, int dir) {
  digitalWrite(dirX, dir);
  for (int x = 0; x < steps; x++) {
    digitalWrite(stepX, HIGH);
    vTaskDelay(10);
    digitalWrite(stepX, LOW);
    vTaskDelay(10);
  }
}

void moveMotorY(int steps, int dir) {
  digitalWrite(dirY, dir);
  for (int x = 0; x < steps; x++) {
    digitalWrite(stepY, HIGH);
    vTaskDelay(10);
    digitalWrite(stepY, LOW);
    vTaskDelay(10);
  }
}