#define W  25
#define X  14
#define Y  12
#define H  13

#define A  27
#define B  26//rojo

#define M1  2
#define M0  15

#define C  4


void setup() {
  // put your setup code here, to run once:
  Serial.begin(115200);
  Serial.println("Hello, ESP32!");
  pinMode(W, INPUT_PULLUP);
  pinMode(X, INPUT_PULLUP);
  pinMode(Y, INPUT_PULLUP);
  pinMode(H, INPUT_PULLUP);
  pinMode(A, INPUT_PULLUP);
  pinMode(B, INPUT_PULLUP);

  pinMode(M1, OUTPUT);
  pinMode(M0, OUTPUT);

  pinMode(C, OUTPUT);
}
void loop() {
  String pedido = pulsado();
  mover(pedido);
}

void mover(String quienPidio){
  if(estaEnH()){
    if(estaLleno()){
      if(quienPidio== "A"){
        do{
          avanzar();
        }while(!estaEnX());
        detener();
      }
      if(quienPidio== "B"){
        do{
          avanzar();
        }while(!estaEnY());
        detener();
      }
    }else{
      llenar();
    }
  }
 if(estaEnX()){
    if(estaLleno()){
      if(quienPidio== "B" ){
        do{
          avanzar();
        }while(!estaEnY());
        detener();
      }
    }else{
      llenar();
    }
  } 
 if(estaEnY()){
    if(estaLleno()){
      if(quienPidio== "A"){
        do{
          retroceder();
        }while(!estaEnX());
        detener();
      }
    }else{
      llenar();
    }
  } 
}

String pulsado(){
  if(digitalRead(A)==LOW){
    return "A";
  }
  if(digitalRead(B)==LOW){
    return "B";
  }
}
void llenar(){
  if(estaEnH()){
    suministrar();
  }
  if((estaEnX()) || (estaEnY())){
     do{
      retroceder();
    }while(estaEnH);
     detener();
     suministrar();
  }
}

void suministrar(){
  if(!estaLleno()){
    digitalWrite(C, HIGH);
    while(!estaLleno());
  }
  digitalWrite(C, LOW);
}

bool estaLleno(){
  if(digitalRead(W) == HIGH){
    return true;
  }
  return false;
}

bool estaEnH(){
  if(digitalRead(H) == HIGH){
    return true;
  }
  return false;
}

bool estaEnX(){
  if(digitalRead(X) == HIGH){
    return true;
  }
  return false;
}

bool estaEnY(){
  if(digitalRead(Y) == HIGH){
    return true;
  }
  return false;
}

void avanzar(){
  digitalWrite(M1, LOW);
  digitalWrite(M0, HIGH);
}

void retroceder(){
  digitalWrite(M1, HIGH);
  digitalWrite(M0, LOW);
}

void detener(){
  digitalWrite(M1, LOW);
  digitalWrite(M0, LOW);
}