#include <SoftwareSerial.h>
SoftwareSerial BTSerial(2, 3);
#include <Servo.h>

// Definición de pines
#define SENSOR_IZQUIERDO 6
#define SENSOR_DERECHO 7

// Instancias de servos
Servo ruedaderecha;
Servo ruedaizquierda;
//El valor va entre 0 y 180. 90 es neutro, la rueda no se mueve, de 90 a 180 va hacia delante y de 90 a 0 va hacia atras
// como un motor tiene que ir invertido, tiene que tener valores por debajo de 90 para que vaya hacia delante con el otro motor

//Variables de velocidad para seguir lineas (ajustadas menor velocidad para que no se salga de la linea)
int velocidadAdelante = 105; // Velocidad reducida hacia adelante RUEDA IZQUIERDA
int velocidadRetroceso = 75; // Velocidad reducida hacia atrás RUEDA DERECHA
int velocidadGiroAdelante = 115; // Velocidad un poco aumentada para girar más rápido
int velocidadGiroRetroceso = 65; // Velocidad un poco aumentada para girar más rápido

int direccion1;
int direccion2;


void setup() {
  Serial.begin(9600);
  BTSerial.begin(9600);
  ruedaderecha.attach(11);
  ruedaizquierda.attach(10);
  pinMode(SENSOR_IZQUIERDO, INPUT);
  pinMode(SENSOR_DERECHO, INPUT);
  pinMode(5, OUTPUT); // LED trasero
  pinMode(4, OUTPUT); // LED trasero
  pinMode(13, OUTPUT); // LED delantero no los vamos a usar
  pinMode(12, OUTPUT); // LED delantero

}

void loop() {
  if (BTSerial.available()) {
    char C = BTSerial.read();
    Serial.write(C);

    if (C == 'A') {
      ruedaizquierda.write(180); // HACIA ADELANTE
      ruedaderecha.write(0);
      digitalWrite(5, LOW);
      digitalWrite(4, LOW);
      direccion1 = 180;
      direccion2 = 0;
    }

    if (C == 'B') {
      ruedaizquierda.write(0); // HACIA ATRAS
      ruedaderecha.write(180);
      digitalWrite(5, HIGH);
      digitalWrite(4, HIGH);
      direccion1 = 0;
      direccion2 = 180;
    }

    if (C == 'C') {
      ruedaizquierda.write(0); // HACIA IZQUIERDA
      ruedaderecha.write(0);
      direccion1 = 0;
      direccion2 = 0;
    }

    if (C == 'D') {
      ruedaizquierda.write(180); // HACIA DERECHA
      ruedaderecha.write(180);
      direccion1 = 180;
      direccion2 = 180;
    }

    if (C == 'E') {
      // Ejecutar el bloque continuamente hasta recibir un nuevo comando
      while (true) {
        // Lectura de sensores
        int sensorIzquierdo = digitalRead(SENSOR_IZQUIERDO);
        int sensorDerecho = digitalRead(SENSOR_DERECHO);

        // Imprimir valores de sensores
        Serial.print("Izquierdo: ");
        Serial.print(sensorIzquierdo);
        Serial.print(" - Derecho: ");
        Serial.println(sensorDerecho);

        if (sensorIzquierdo == LOW && sensorDerecho == LOW) {
          // Avanzar recto a menor velocidad
          ruedaizquierda.write(velocidadAdelante);
          ruedaderecha.write(velocidadRetroceso);   
        }
        else if (sensorIzquierdo == LOW && sensorDerecho == HIGH) {
          // Girar a la derecha a menor velocidad
          ruedaizquierda.write(90); // Rueda izquierda neutral
          ruedaderecha.write(velocidadGiroRetroceso); // Rueda derecha hacia atrás
        }
        else if (sensorIzquierdo == HIGH && sensorDerecho == LOW) {
          // Girar a la izquierda a menor velocidad
          ruedaizquierda.write(velocidadGiroAdelante); // Rueda izquierda hacia adelante
          ruedaderecha.write(90); // Rueda derecha neutral
        }
        else if (sensorIzquierdo == HIGH && sensorDerecho == HIGH) {
          // Detener el coche
          ruedaizquierda.write(90); // Neutral
          ruedaderecha.write(90);   // Neutral
        }

        // Verificar si hay un nuevo comando disponible
        if (BTSerial.available()) {
          char newC = BTSerial.read();
          if (newC != 'E') {
            // Si se recibe un nuevo comando que no sea 'E', salir del bucle
            C = newC;  // Actualizar C para manejar el nuevo comando fuera del bucle
            break;
          }
        }
      }
    }

    if (C == 'F') {
      ruedaizquierda.write(90); // Posición neutral freno
      ruedaderecha.write(90);
      digitalWrite(5, LOW);
      digitalWrite(4, LOW);
    }

    if (C == 'G') { //leds delanteros, no los vamos a usar
      digitalWrite(13, HIGH);
      digitalWrite(12, HIGH);
      ruedaizquierda.write(direccion1);
      ruedaderecha.write(direccion2);
      }
    }
  }