// SEGUIDOR DE LINEA BLANCA "H2"

// ARDUINO / DRV8833 / MOTORREDUCTOR CC ≈ 200RPM / 9V + ELCAP 1000µF / 200 mAh / r - 2.75 cm / High-lug tire / 13x18 cm / 250g / 3 IR /


// Vars for PIN Read (Sensors)


const int R = A5;
const int M = A3;
const int L = A4;

// Vars for PIN Write (Motors)


//Adelante
const int MR = 9;
const int ML = 5;

//Atrás
const int MRB = 10;
const int MLB = 6;

// Vars for Logics


// Memory
char Memory = 'R';

// Last
char Last = 'F';

// TIMER
int TIME = 0;

// Softs

//Adelante
int Soft_R = 255;
int Soft_L = 255;
//Atrás
int Soft_RB = 0;
int Soft_LB = 0;

// Line

// 0 (false) - White 
// 1 (true) - Black

const bool Line = false;

// Umbral 
const int Umbral = 475; 

// Índice Acc
const int IAc = 1;

// Índice AccB
const int IAcB = 1;

// Índice Des
const int Ide = 1;

// FUNCIONES

// Parpadeo Rápido
void LED_PFast() {

  TIME += 1;

  if (TIME < 200) {
 
    digitalWrite(13, HIGH);

  }

  else {

    digitalWrite(13, LOW);

  }

  if (TIME >= 400) {

    TIME = 0;

  }
}

//Parpadeo Lento
void LED_PSlow() {

  TIME += 1;

  if (TIME < 500) {

   digitalWrite(13, HIGH);

  }

  else {

   digitalWrite(13, LOW);

  }

  if (TIME == 1000) {

   TIME = 0;

  }
}

//SOFTacc
void Acc() {

  if (TIME % 2 == 0) {

  //MR
  if (Soft_R >= 245) {

   Soft_R = 255;

  }

  else {

   Soft_R += IAc;

  }

  //ML
  if (Soft_L >= 245) {

   Soft_L = 255;

  }

  else {

   Soft_L += IAc;

  }
  }
}


//SOFTaccB
void AccB() {

  if (TIME % 2 == 0) {

  //MRB
  if (Soft_RB >= 245) {

   Soft_RB = 255;

  }

  else {

   Soft_RB += IAcB;

  }

  //MLB
  if (Soft_LB >= 245) {

   Soft_LB = 255;

  }

  else {

   Soft_LB += IAcB;

  }
  }
}









void setup() {

// Baudios (SOLO EN CASO DE NESESITAR CONSOLA)
// Serial.begin(9600);


// Pin MOD MOTORS

pinMode(MR, OUTPUT);
pinMode(ML, OUTPUT);
pinMode(MRB, OUTPUT);
pinMode(MLB, OUTPUT);

// --- COMPATIBILIDAD DRV8833 ---

  digitalWrite(MR, LOW);
  digitalWrite(MRB, LOW);
  digitalWrite(ML, LOW);
  digitalWrite(MLB, LOW);

// --- Indicador LED ---

pinMode(13, OUTPUT);
digitalWrite(13, LOW);
}




void loop() {

 //Lectura y Digitalización

  bool See_R = analogRead(R) > Umbral;
  bool See_M = analogRead(M) > Umbral;
  bool See_L = analogRead(L) > Umbral;

  if (Line == false) { 
    See_R = !See_R; 
    See_M = !See_M;
    See_L = !See_L;
  }


  // Algoritmo de Movimiento

  // *1 0 0 0 BLIND 

  if (!See_L && !See_M && !See_R) {

    if (Memory == 'R') {

     digitalWrite(MLB, LOW);
     analogWrite(MR, 0);
     analogWrite(MRB, 150);
     analogWrite(ML, 150);

    }

    else {

     digitalWrite(MRB, LOW);
     analogWrite(ML, 0);
     analogWrite(MLB, 150);
     analogWrite(MR, 150);

    }

   //LED (Parpadeo Rápido)

   LED_PFast(); 

    // SOFT
    Soft_R = 255;
    Soft_L = 255;

    Last = 'B';

  }

  // *2 1 1 1 FULL || *3 0 1 0 FULL (Line)

  else if ((See_L && See_M && See_R) || (!See_L && See_M && !See_R)) {

    digitalWrite(MRB, LOW);
    digitalWrite(MLB, LOW);
    analogWrite(MR, 255);
    analogWrite(ML, 255);

    // LED
    digitalWrite(13, HIGH); // LED (Encendido)

    // TIMER

    TIME += 1;

    if (TIME >= 400) {

     TIME = 0;

    }

    // Acc (Segundo Plano)

    Acc();

    Soft_RB = 0;
    Soft_LB = 0;

    Last = 'F';
  }

    // *4 1 1 0 Left Soft

    else if (See_L && See_M && !See_R) {

      //SOFTdes L

      if (Soft_L < 10) {

        Soft_L = 0;

      }

      else if (Soft_L > 0) {

        Soft_L -= Ide;

      }

      digitalWrite(MRB, LOW);
      digitalWrite(MLB, LOW);
      analogWrite(MR, 200);
      analogWrite(ML, Soft_L);

      Memory = 'L';
      Last = 'l';

      digitalWrite(13, LOW); // LED (Apagado)
      TIME = 0;

      Soft_RB = 0;
      Soft_LB = 0;

    }


    // *5 1 0 0 Left Hard

    else if (See_L && !See_M && !See_R) {

      AccB(); // !Acc

      digitalWrite(MRB, LOW);
      analogWrite(ML, 0);
      analogWrite(MLB, Soft_LB);
      analogWrite(MR, 255);
     

      Memory = 'L';
      Last = 'L';

      //LED (Parpadeo Lento)
      LED_PSlow();

      // SOFT
      Soft_R = 255;
      Soft_L = 255;
    }


    // *6 0 1 1 Right Soft

    else if (!See_L && See_M && See_R) {

      //SOFTdes

      if (Soft_R < 10) {

        Soft_R = 0;

      }

      else if (Soft_R > 0) {

        Soft_R -= Ide;

      }

      digitalWrite(MRB, LOW);
      digitalWrite(MLB, LOW);
      analogWrite(MR, Soft_R);
      analogWrite(ML, 255);

      Memory = 'R';
      Last = 'r';

      digitalWrite(13, LOW); // LED (Apagado)
      TIME = 0;

      Soft_RB = 0;
      Soft_LB = 0;
    }


    // *7 0 0 1 Right Hard

    else if (!See_L && !See_M && See_R) {

      AccB(); // !Acc

      digitalWrite(MLB, LOW);
      analogWrite(MR, 0);
      analogWrite(ML, 200);
      analogWrite(MRB, Soft_RB);

      Memory = 'R';
      Last = 'R';

      //LED (Parpadeo Lento)
      LED_PSlow();

      // SOFT
      Soft_R = 255;
      Soft_L = 255;
    }


    // *8 1 0 1 NOISE
 
    else if (See_L && !See_M && See_R) {

      digitalWrite(MRB, LOW);
      digitalWrite(MLB, LOW);
      analogWrite(MR, 100);
      analogWrite(ML, 100);

      digitalWrite(13, LOW); // LED (Apagado)
      TIME = 0;

      // SOFT
      Soft_R = 255;
      Soft_L = 255;
      Soft_RB = 0;
      Soft_LB = 0;

      Last = 'F';
   }

// --- Consola ---

// Serial.print(" "); Serial.print(See_L); Serial.print(" "); Serial.print(See_M); Serial.print(" "); Serial.print(See_R); Serial.print(" | "); Serial.print(Last); Serial.print(" | "); Serial.print(Soft_L); Serial.print(" | "); Serial.print(Soft_R); Serial.print(" | "); Serial.print(Soft_LB); Serial.print(" | "); Serial.println(Soft_RB); 
// delay(100);

}

 // Usar un arranque suave en el estado de FULL daría como resultado un giro adicional indeseado en el tiempo que Acc tarda en llegar al máximo.
 // Sin embargo, cuando se usa el estado SOFT repetidamente, no tiene sentido empezar constantemente a partir de 255.
 // Una Acc en segundo plano mantiene ambos beneficios al mismo tiempo.