#include <Ultrasonic.h>

// Pins des capteurs ultrasoniques
#define LEFT_TRIGGER_PIN 13
#define LEFT_ECHO_PIN 12
#define RIGHT_TRIGGER_PIN 6
#define RIGHT_ECHO_PIN 8


// Pins des moteurs
const int motorAForwardPin = 2;
const int motorAReversePin = 3;
const int motorBForwardPin = 4;
const int motorBReversePin = 5;
const int motorAEnablePin = 9;
const int motorBEnablePin = 10;
const int boutton = 1;
bool turnedLeft = false;

unsigned long startMillis = 0;

// Capteurs ultrasoniques
Ultrasonic leftSensor(LEFT_TRIGGER_PIN, LEFT_ECHO_PIN);
Ultrasonic rightSensor(RIGHT_TRIGGER_PIN, RIGHT_ECHO_PIN);

void setup() {
  pinMode(motorAForwardPin, OUTPUT);
  pinMode(motorAReversePin, OUTPUT);
  pinMode(motorBForwardPin, OUTPUT);
  pinMode(motorBReversePin, OUTPUT);
  pinMode(motorAEnablePin, OUTPUT);
  pinMode(motorBEnablePin, OUTPUT);
  pinMode(boutton, INPUT);

  Serial.begin(9600);
}

unsigned long leftDetectionTime = 0; // Adjusted detection time for left sensor
unsigned long rightDetectionTime = 0; // Adjusted detection time for right sensor

void loop() {
  //while (true) {
  unsigned long currentMillis = millis() - startMillis; // Simulated millis reset

  // the robot keeps going forward until it detects an obstacle
  while (rightSensor.read() > 40 && leftSensor.read() > 40) {
    moveForward();
  }

  // Obstacle detection and handling
  if (rightSensor.read() <= 40) {
    rightDetectionTime = currentMillis;
    turnedLeft = false;
  }
  if (leftSensor.read() <= 40) {
    leftDetectionTime = currentMillis;
    turnedLeft = true;
  }

  // Handle individual sensor conditions first
  if ((currentMillis - rightDetectionTime <= 1500) && (currentMillis - leftDetectionTime <= 1500)) {


    while ((currentMillis - rightDetectionTime <= 1500) && (currentMillis - leftDetectionTime <= 1500)) {
      if (turnedLeft) {
        turnRight(1500);
        moveForward();
        delay(600);
      }
      else {
        turnLeft(1500);
        moveForward();
        delay(600);
      }
    }
    // Reset detection times after handling both sensors
    rightDetectionTime = 0;
    leftDetectionTime = 0;
    startMillis = millis();

  }
  if (rightSensor.read() <= 40 && leftSensor.read() > 70) {
    turnRight(800);
    delay(100);
    moveForward();
    delay(200);
    turnedLeft = false;
  }
  else if (leftSensor.read() <= 40 && rightSensor.read() > 70) {
    turnLeft(600);
    delay(100);
    moveForward();
    delay(200);
    turnedLeft = true;
  }
  // Handle both sensors detecting obstacles within 500 ms
  //else
  //}
}

void moveForward() {
  digitalWrite(motorAForwardPin, HIGH);
  digitalWrite(motorAReversePin, LOW);
  digitalWrite(motorBForwardPin, HIGH);
  digitalWrite(motorBReversePin, LOW);
  analogWrite(motorAEnablePin, 255);
  analogWrite(motorBEnablePin, 255);
}

void turnRight(int duration) {
  digitalWrite(motorAForwardPin, HIGH);
  digitalWrite(motorAReversePin, LOW);
  digitalWrite(motorBForwardPin, LOW);
  digitalWrite(motorBReversePin, HIGH);
  analogWrite(motorAEnablePin, 255);
  analogWrite(motorBEnablePin, 200);
  delay(duration);
}

void turnLeft(int duration) {
  digitalWrite(motorAForwardPin, LOW);
  digitalWrite(motorAReversePin, HIGH);
  digitalWrite(motorBForwardPin, HIGH);
  digitalWrite(motorBReversePin, LOW);
  analogWrite(motorAEnablePin, 200);
  analogWrite(motorBEnablePin, 255);
  delay(duration);
}