// Line Follower Test 01


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;



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);

  
  // 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
  int MtrR = 255 - L1m - R2m;
  int MtrL = 255 - R1m - L2m;

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

  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);
}