// Line Follower Test 01

#include <DistanceSensor.h>

int R2 = 26;
int R1 = 27;
int C1 = 14;
int L1 = 12;
int L2 = 13;
int RightMotor = 16;
int LeftMotor = 4;
int Led = 2;


int L = 1; // Line
int S = 128; // Sensor Sensivity 128
int S1 = 170; // R1 and L1 Sensivity 170

int R1m;
int R2m;
int L1m;
int L2m;

int des = 0; // Desvia do objeto
int pos = 0; // Passo 0
int MtrR = 0;
int MtrL = 0;
int d1 = 0;
int d2 = 0;
int d3 = 0;

#define trigPin = 18; // For the TRIG pins of the Ultrassonic Sensor.
DistanceSensor sens1(trigPin, 5); // (trig, echo);
DistanceSensor sens2(trigPin, 12);


void setup(){
  Serial.begin(115200);

  pinMode(R2, INPUT);
  pinMode(R1, INPUT);
  pinMode(C1, INPUT);
  pinMode(L1, INPUT);
  pinMode(L2, INPUT);
  pinMode(RightMotor, OUTPUT);
  pinMode(LeftMotor, OUTPUT);
  pinMode(Led, OUTPUT);

  digitalWrite(Led, 1);
  delay(500);
  digitalWrite(Led, 0);
  delay(500);
  digitalWrite(Led, 1);
  delay(1000);
  digitalWrite(Led, 0);

  analogReadResolution(8);
}

void loop(){
  // Identify the Line and Base colors
  if (analogRead(C1) < S && analogRead(L2) > S && analogRead(R2) > S){
    L = 1;
  }
  if (analogRead(C1) > S && analogRead(L2) < S && analogRead(R2) < S){
    L = 0;
  }
  digitalWrite(Led, L);

  d1 = (sens1.getCM())*10;
  delay(60);
  d2 = (sens2.getCM())*10;
  delay(60);
  Serial.print("Distance: ");
  Serial.print(d1);
  Serial.print("mm - ");
  Serial.print(d2);
  Serial.print("mm - ");
  
  if (d1 < 200){
    des = 1;
  }

  if (des == 0){ // Segue Linha
    // Ajuste fino
    if (L == 1){                    // White line
      R1m = analogRead(R1);
      R1m = map(R1m, 0, 255, S1, 0);
      L1m = analogRead(L1);
      L1m = map(L1m, 0, 255, S1, 0);
    }
    if (L == 0){                    // Black line
      R1m = analogRead(R1);
      R1m = map(R1m, 0, 255, 0, S1);
      L1m = analogRead(L1);
      L1m = map(L1m, 0, 255, 0, S1);
    }
    
    // Ajuste grosso
    if(analogRead(L2) < S && analogRead(R2) > S && L == 1){ // White line
      R2m = 255;
    }
    else {
      R2m = 0;
    }
    if(analogRead(L2) > S && analogRead(R2) < S && L == 1){
      L2m = 255;
    }
    else {
      L2m = 0;
    }

    if(analogRead(L2) > S && analogRead(R2) < S && L == 0){ // Black line
      R2m = 255;
    }
    else {
    R2m = 0;
    }
    if(analogRead(L2) < S && analogRead(R2) > S && L == 0){
      L2m = 255;
    }
    else {
      L2m = 0;
    }

      // Saída
    MtrR = 255 - L1m - R2m;
    MtrL = 255 - R1m - L2m;
  }
  else if (des == 1){
    if(pos == 0 && d2 > 200){
      MtrR = 0;
      MtrL = 255;
    }
    else if(pos == 0 && d2 < 200){
      MtrR = 0;
      MtrL = 0;
      pos = 1;
      d3 = (sens2.getCM())*10;
      delay(60);
    }
    
    if(pos == 1 && d2 > d3){
      MtrR = 255;
      MtrL = 255;
    }
    else if(pos == 1 && d2 < d3){
      MtrR = 0;
      MtrL = 0;
      pos = 2;
    }
    
    if(pos == 2){
      MtrR = 255;
      MtrL = 0;
      delay(1500);
      MtrR = 0;
      MtrL = 0;
      pos = 3;
    }

    if(pos == 3 && d2 < 200){
      MtrR = 255;
      MtrL = 255;
    }
    else if(pos == 3 && d2 > 200){
      MtrR = 0;
      MtrL = 0;
      pos = 4;
    }

    if(pos == 4 && d2 > d3){
      MtrR = 255;
      MtrL = 255;
    }
    else if(pos == 4 && d2 < d3){
      MtrR = 0;
      MtrL = 0;
      pos = 5;
    }

    if(pos == 4 && d2 > 200){
      MtrR = 0;
      MtrL = 255;
    }
    else if(pos == 4 && d2 < 200){
      MtrR = 0;
      MtrL = 0;
      pos = 1;
    }
  }

  analogWrite(RightMotor, MtrR);
  analogWrite(LeftMotor, MtrL);

//Serial
  Serial.print("L2: ");
  Serial.print(analogRead(L2));
  Serial.print(" | L1: ");
  Serial.print(analogRead(L1));
  Serial.print(" | C1: ");
  Serial.print(analogRead(C1));
  Serial.print(" | R1: ");
  Serial.print(analogRead(R1));
  Serial.print(" | R2: ");
  Serial.print(analogRead(R2));
  Serial.print(" | | R1m: ");
  Serial.print(R1m);
  Serial.print(" | L1m: ");
  Serial.print(L1m);
  Serial.print(" | L: ");
  Serial.print(L);
  Serial.print(" | | MtrR: ");
  Serial.print(MtrR);
  Serial.print(" | MtrL: ");
  Serial.println(MtrL);

  delay(100);
}