int S0, S1, S2, S3, S4;
int ajustare = 0;
int ultimaDirectie = 0;

// Pini Motoare (Driver L298N)
const int motorDrept1 = 6; // was 7;
const int motorDrept2 = 7; // was 6;
const int motorStang1 = 4;
const int motorStang2 = 5;
const int pwmStang = 9;
const int pwmDrept = 10;

// Pini Senzori Linie BFD-1000
const int senzor0 = A0; // Stânga extrem
const int senzor1 = A1; // Stânga
const int senzor2 = A2; // Centru
const int senzor3 = A3; // Dreapta
const int senzor4 = A4; // Dreapta extrem
const int V_INAINTE = 120; // Viteză moderată înainte
const int V_RAPID = 160; // Roata exterioară în viraj
const int V_LENT = 70; // Roata interioară în viraj (suficient cât să se învârtă, nu să se blocheze)

// random generator
unsigned long timer, timeout = 2000; // two seconds

void setup() {
  Serial.begin(115200);
  pinMode(motorDrept1, OUTPUT);
  pinMode(motorDrept2, OUTPUT);
  pinMode(motorStang1, OUTPUT);
  pinMode(motorStang2, OUTPUT);
  pinMode(pwmStang, OUTPUT);
  pinMode(pwmDrept, OUTPUT);
  pinMode(senzor0, INPUT);
  pinMode(senzor1, INPUT);
  pinMode(senzor2, INPUT);
  pinMode(senzor3, INPUT);
  pinMode(senzor4, INPUT);
  // delay(2000); // 2 secunde pauză la pornire
}

void citesteSenzori() {
  // BFD-1000: LOW pe linia neagră, HIGH pe alb
  // S0 = digitalRead(senzor0);
  // S1 = digitalRead(senzor1);
  // S2 = digitalRead(senzor2);
  // S3 = digitalRead(senzor3);
  // S4 = digitalRead(senzor4);

  // random direction overrides previous sensor value
  if (millis() - timer >= timeout) {
    timer = millis();
    switch (int(random(5))) {
      case 0: S0 = 0; S1 = 1; S2 = 1; S3 = 1; S4 = 1; break;
      case 1: S0 = 1; S1 = 0; S2 = 1; S3 = 1; S4 = 1; break;
      case 2: S0 = 1; S1 = 1; S2 = 0; S3 = 1; S4 = 1; break;
      case 3: S0 = 1; S1 = 1; S2 = 1; S3 = 0; S4 = 1; break;
      case 4: S0 = 1; S1 = 1; S2 = 1; S3 = 1; S4 = 0; break;
      default: break;
    }
  }

  // add to setup(): Serial.begin(115200);
  Serial.print(S0);
  Serial.print(" ");
  Serial.print(S1);
  Serial.print(" ");
  Serial.print(S2);
  Serial.print(" ");
  Serial.print(S3);
  Serial.print(" ");
  Serial.print(S4);
  Serial.print(" ");
}

void ajustareTraiectorie() {
  // Verificăm mai întâi senzorul central
  if (S2 == LOW) {
    ajustare = 0; // Centrat pe linie
    ultimaDirectie = 0;
    Serial.print("....S2.... ");
  }
  // Dacă linia se duce spre stânga (S0 sau S1 văd negru/LOW)
  else if (S0 == LOW || S1 == LOW) {
    ajustare = 1; // Corecție stânga
    ultimaDirectie = 1;
    Serial.print("S0S1...... ");
  }
  // Dacă linia se duce spre dreapta (S3 sau S4 văd negru/LOW)
  else if (S3 == LOW || S4 == LOW) {
    ajustare = -1; // Corecție dreapta
    ultimaDirectie = -1;
    Serial.print("......S3A4 ");
  }
  // Dacă a pierdut linia complet, continuă în ultima direcție știută
  else {
    ajustare = ultimaDirectie;
  }
}

void controlMotoare() {
  // Setează sensul înainte pentru ambele motoare
  digitalWrite(motorDrept1, LOW);
  digitalWrite(motorDrept2, HIGH);
  digitalWrite(motorStang1, HIGH);
  digitalWrite(motorStang2, LOW);
  if (ajustare == 0) {
    // DREPT ÎNAINTE: Ambele roți egale
    analogWrite(pwmStang, V_INAINTE);
    analogWrite(pwmDrept, V_INAINTE);
    Serial.print("ÎNAINTE ");
    Serial.print("(");
    Serial.print(V_INAINTE);
    Serial.print(",");
    Serial.print(V_INAINTE);
    Serial.print(")");
  }
  else if (ajustare == 1) {
    // LINIA ESTE ÎN STÂNGA -> Viraj la stânga (Roata stângă lentă, roata dreaptă rapidă)
    analogWrite(pwmStang, V_LENT);
    analogWrite(pwmDrept, V_RAPID);
    Serial.print("STÂNGA  ");
    Serial.print("(");
    Serial.print(V_LENT);
    Serial.print(",");
    Serial.print(V_RAPID);
    Serial.print(")");
  }
  else if (ajustare == -1) {
    // LINIA ESTE ÎN DREAPTA -> Viraj la dreapta (Roata stângă rapidă, roata dreaptă lentă)
    analogWrite(pwmStang, V_RAPID);
    analogWrite(pwmDrept, V_LENT);
    Serial.print("DREAPTA ");
    Serial.print("(");
    Serial.print(V_RAPID);
    Serial.print(",");
    Serial.print(V_LENT);
    Serial.print(")");
  }
  Serial.println();
}

void loop() {
  citesteSenzori();
  ajustareTraiectorie();
  controlMotoare();
  delay(10); // Stabilitate în ciclul de citire
}