// Alocare pini driver motoare L298N
const int ENA = 6; const int IN1 = 7; const int IN2 = 8;
const int ENB = 5; const int IN3 = 9; const int IN4 = 10;

// Alocare pini senzori optici reflectivi
const int IRSensorLeft = 12; const int IRSensorRight = 11;

// Alocare pini senzor ultrasonic si alarma acustica
const int trigPin = 3; const int echoPin = 4;
const int buzzerPin = 2;

// Setare constante viteza prin factor de umplere PWM
#define s 80 
#define t 80 

void setup() {
  pinMode(ENA, OUTPUT); pinMode(IN1, OUTPUT); pinMode(IN2, OUTPUT);
  pinMode(ENB, OUTPUT); pinMode(IN3, OUTPUT); pinMode(IN4, OUTPUT);
  
  pinMode(IRSensorLeft, INPUT); pinMode(IRSensorRight, INPUT);
  
  pinMode(trigPin, OUTPUT); pinMode(echoPin, INPUT);
  pinMode(buzzerPin, OUTPUT);
  
  Serial.begin(9600);
}

void loop() {
  // 1. Prioritate Siguranta: Verificare obstacole
  long distanta = citesteDistanta();

  if (distanta > 0 && distanta < 20) { 
    stopMotors();
    // Alarma sacadata de proximitate
    digitalWrite(buzzerPin, HIGH); delay(100);
    digitalWrite(buzzerPin, LOW);  delay(100);
    return; // Blocam restul executiei din loop
  } else {
    digitalWrite(buzzerPin, LOW);
  }

  // 2. Algoritm urmarire linie neagra
  bool leftSensor = digitalRead(IRSensorLeft);
  bool rightSensor = digitalRead(IRSensorRight);

  if (leftSensor == HIGH && rightSensor == HIGH)      { moveForward(); }
  else if (leftSensor == HIGH && rightSensor == LOW)  { turnRight(); }
  else if (leftSensor == LOW && rightSensor == HIGH) { turnLeft(); }
  else                                                { stopMotors(); }

  delay(50); 
}

long citesteDistanta() {
  digitalWrite(trigPin, LOW); delayMicroseconds(2);
  digitalWrite(trigPin, HIGH); delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
  long durata = pulseIn(echoPin, HIGH, 30000); // Timeout la 30ms
  return durata * 0.034 / 2;
}

void moveForward() {
  digitalWrite(IN1, HIGH); digitalWrite(IN2, LOW); analogWrite(ENA, s);
  digitalWrite(IN3, HIGH); digitalWrite(IN4, LOW); analogWrite(ENB, s);
}

void turnRight() {
  digitalWrite(IN1, HIGH); digitalWrite(IN2, LOW); analogWrite(ENA, t);
  digitalWrite(IN3, LOW);  digitalWrite(IN4, LOW);  analogWrite(ENB, 0);
}

void turnLeft() {
  digitalWrite(IN1, LOW);  digitalWrite(IN2, LOW);  analogWrite(ENA, 0);
  digitalWrite(IN3, HIGH); digitalWrite(IN4, LOW); analogWrite(ENB, t);
}

void stopMotors() {
  digitalWrite(IN1, LOW); digitalWrite(IN2, LOW); analogWrite(ENA, 0);
  digitalWrite(IN3, LOW); digitalWrite(IN4, LOW); analogWrite(ENB, 0);
}