TaskHandle_t thread1;
TaskHandle_t thread2;
TaskHandle_t thread3;
TaskHandle_t thread4;

int stepX = 8;
int dirX = 7;
int stepY = 6;
int dirY = 5;
int btn = 1;
int led = 18;

int enable = 9;
int factor = 41;

bool startX = false;
bool startY = false;
bool start = false;


int movPX[4] = {85, 300, 200, 500};
int dirPX[4] = {0, 1, 1, 1};
int movPY[4] = {100, 400, 250, 200};
int dirPY[4] = {1, 0, 1, 0};
int pos = 0;

void process1(void* parameter) {
  for (;;) {
    if (startX == true) {
      moveXmm(movPX[pos], dirPX[pos]);
      startX = false;
    }
    vTaskDelay(1);
  }
}

void process2(void* parameter) {
  for (;;) {
    if (startY == true) {
      moveYmm(movPY[pos], dirPY[pos]);
      startY = false;
    }
    vTaskDelay(1);
  }
}

void process3(void* parameter) {
  for (;;) {
    if (digitalRead(btn) == 0) {
      digitalWrite(enable, LOW);
      startX = true;
      startY = true;
      digitalWrite(led, HIGH);
      vTaskDelay(100);
      start = true;
    }
    vTaskDelay(1);
  }
}

void process4(void* parameter) {
  for (;;) {
    if (startX == false && startY == false) {
      if (start) {
        if (pos < 4) {
          digitalWrite(led, LOW);
          vTaskDelay(1000);
          pos++;
          startX = true;
          startY = true;
          digitalWrite(enable, LOW);
          digitalWrite(led, HIGH);
        }
      }
    }
    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(enable, OUTPUT);
  pinMode(btn, INPUT_PULLUP);
  pinMode(led, OUTPUT);
  digitalWrite(enable, HIGH);

  xTaskCreatePinnedToCore(process1, "thread1", 10000, NULL, 1, NULL, 0);
  xTaskCreatePinnedToCore(process2, "thread2", 10000, NULL, 1, NULL, 0);
  xTaskCreatePinnedToCore(process3, "thread3", 10000, NULL, 1, NULL, 0);
  xTaskCreatePinnedToCore(process4, "thread4", 10000, NULL, 1, NULL, 0);
}

void loop() {
  vTaskDelay(1000);
}

void moveX(int steps, int dir) {
  digitalWrite(dirX, dir);
  for (int index = 0 ; index < steps; index++) {
    digitalWrite(stepX, HIGH);
    vTaskDelay(1);
    digitalWrite(stepX, LOW);
    vTaskDelay(1);
  }
}

void moveY(int steps, int dir) {
  digitalWrite(dirY, dir);
  for (int index = 0 ; index < steps; index++) {
    digitalWrite(stepY, HIGH);
    vTaskDelay(1);
    digitalWrite(stepY, LOW);
    vTaskDelay(1);
  }
}

void moveXmm(int mm, int dir) {
  int steps = (mm * 200) / factor;
  moveX(steps, dir);
}


void moveYmm(int mm, int dir) {
  int steps = (mm * 200) / factor;
  moveY(steps, dir);
}