TaskHandle_t thread1;
TaskHandle_t thread2;
TaskHandle_t thread3;

int stepX = 7;
int dirX = 6;

const int btn1 = 1;
const int btn2 = 2;
const int btn3 = 3;

const int led1 = 19;
const int led2 = 18;
const int led3 = 4;

const int enable = 9;

bool Der = false;
bool Iz = false;

bool valDER = false;
bool valIZQ = false;
bool start = false;


void process1(void* parameter) {
  for (;;) {
    if (Der == true && Iz == false) {
      moveDer(200);
      digitalWrite(enable, LOW);
      Der = false;
    }
    vTaskDelay(1);

    if (Iz == true && Der == false) {
      moveIz(200);
      digitalWrite(enable, LOW);
      Iz = false;
    }
    vTaskDelay(1);
    
    if (valDER == true && valIZQ == true) {
      moveDerIz(200);
     digitalWrite(enable, LOW);
    }
    vTaskDelay(1);
  }
}

void process2(void* parameter) {
  for (;;) {
    if (digitalRead(btn1) == false) {
      Der = true;
      valDER = true;
    }
    vTaskDelay(1);

    if (digitalRead(btn2) == false) {
      Iz = true;
      valIZQ = true;
    }
    vTaskDelay(1);

    if (digitalRead(btn3) == false) {
      Der = false;
      Iz = false;
      valIZQ = false;
      valDER = false;
    }
    vTaskDelay(1);
  }
}

void process3(void* parameter) {
  for (;;) {
    if (Der == true && Iz == false) {
      if(valdDER == false || valIZQ == false){
        digitalWrite(led1, HIGH);
        digitalWrite(led2, LOW);
        digitalWrite(led3, LOW);
      }
    }
    vTaskDelay(1);

    if (Der == false && Iz == true) {
      if(valDER == false || valIZQ == false){
        digitalWrite(led1, LOW);
        digitalWrite(led2, HIGH);
        digitalWrite(led3, LOW);
      }
    }
    vTaskDelay(1);

    if (valDER == true && valIZQ == true) {
      if(valDER == false || valIZQ == false){
        digitalWrite(led1, LOW);
        digitalWrite(led2, LOW);
        digitalWrite(led3, HIGH);
      }

    }
    vTaskDelay(1);
    if (Der == false && Iz == false) {
      digitalWrite(led2, LOW);
      digitalWrite(led1, LOW);
      digitalWrite(led3, LOW);
    }
    vTaskDelay(1);       
  }
}

void process4(void* parameter) {
  for (;;) {
    vTaskDelay(1500);
    if (Der == true && Iz == false) {
      Serial.println("200 pasos a la derecha");
    }
    vTaskDelay(1);

    if (Der == false && Iz == true) {
      Serial.println("200 pasos a la Izquierda");
    }
    vTaskDelay(1);

    if (Der == true && Iz == true) {
      Serial.println("200 pasos a la derecha y 200 a la derecha");
    }
    vTaskDelay(1);

    if (Der == false && Iz == false) {
      Serial.println("Motor apagado");
    }
    vTaskDelay(1); 
  }
}




void setup() {
  Serial.begin(115200);
  pinMode(stepX, OUTPUT);
  pinMode(dirX, OUTPUT);
  pinMode(enable, OUTPUT);
  pinMode(btn1, INPUT_PULLUP);
  pinMode(btn2, INPUT_PULLUP);
  pinMode(btn3, INPUT_PULLUP);
  pinMode(led1, OUTPUT);
  pinMode(led2, OUTPUT);
  pinMode(led3, 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() {
  // put your main code here, to run repeatedly:
  delay(10); // this speeds up the simulation
}

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

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

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