/*
  Arduino | coding-help
  Obstacle avoiding car

  November 29, 2024 3:24am
  عماد الدين

  Hi, I’m making an arduino robot car that avoids obstacles.
  I’m using an H-bridge L293D and an arduino UNO card.
  However, when the car is between two bottles, I want it to
  detect both at the same period thanks to millis, but idk how
  to reset millis everytime.
  My robot has to go around 4 consecutive bottles,
  so idk if it works this way or not.

  L293D control pins
  IN1	    IN2	    Spinning Direction
  Low(0)	Low(0)	Motor OFF
  High(1)	Low(0)	Forward
  Low(0)	High(1)	Backward
  High(1)	High(1)	Motor OFF
*/

#include <Ultrasonic.h>

const int THRESHOLD = 100; // set minimum distance to 40cm

// Pins des capteurs ultrasoniques
#define LEFT_TRIGGER_PIN 11
#define LEFT_ECHO_PIN 10
#define RIGHT_TRIGGER_PIN 3
#define RIGHT_ECHO_PIN 2

// Pins des moteurs
const int motorAForwardPin = 8;
const int motorAReversePin = 7;
const int motorBForwardPin = 5;
const int motorBReversePin = 4;
const int motorAEnablePin = 9;
const int motorBEnablePin = 6;

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

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

void moveForward() {
  Serial.println("Forward");
  digitalWrite(motorAForwardPin, HIGH);
  digitalWrite(motorAReversePin, LOW);
  digitalWrite(motorBForwardPin, HIGH);
  digitalWrite(motorBReversePin, LOW);
  analogWrite(motorAEnablePin, 255);
  analogWrite(motorBEnablePin, 255);
}

void turnRight() {
  Serial.println("Turning right");
  digitalWrite(motorAForwardPin, HIGH);
  digitalWrite(motorAReversePin, LOW);
  digitalWrite(motorBForwardPin, LOW);
  digitalWrite(motorBReversePin, HIGH);
  analogWrite(motorAEnablePin, 255);
  analogWrite(motorBEnablePin, 0);
}

void turnLeft() {
  Serial.println("Turning left");
  digitalWrite(motorAForwardPin, LOW);
  digitalWrite(motorAReversePin, HIGH);
  digitalWrite(motorBForwardPin, HIGH);
  digitalWrite(motorBReversePin, LOW);
  analogWrite(motorAEnablePin, 0);
  analogWrite(motorBEnablePin, 255);
}

void stop(int duration) {
  Serial.println("Stop");
  digitalWrite(motorAForwardPin, LOW);
  digitalWrite(motorAReversePin, LOW);
  digitalWrite(motorBForwardPin, LOW);
  digitalWrite(motorBReversePin, LOW);
  analogWrite(motorAEnablePin, 0);
  analogWrite(motorBEnablePin, 0);
  delay(duration);
}

void setup() {
  Serial.begin(9600);
  pinMode(motorAForwardPin, OUTPUT);
  pinMode(motorAReversePin, OUTPUT);
  pinMode(motorBForwardPin, OUTPUT);
  pinMode(motorBReversePin, OUTPUT);
  pinMode(motorAEnablePin, OUTPUT);
  pinMode(motorBEnablePin, OUTPUT);
}

void loop() {
  // get distances
  int lDistance = leftSensor.read();
  int rDistance = rightSensor.read();

  // basic obstacle avoidance
  if (lDistance >= THRESHOLD && rDistance >= THRESHOLD) {
    moveForward();
  }

  /*
    if (lDistance < THRESHOLD)  {
    turnRight();
    }
    if (rDistance < THRESHOLD)  {
    turnLeft();
    }
  */

  // navigate pylons
  if (lDistance < THRESHOLD && rDistance < THRESHOLD) { // obstacle on both sides
    
    delay(3000);
    
    // between the pylons, turn left first
    do  {
      turnLeft();
    } while (rightSensor.read() < THRESHOLD);
    while (rightSensor.read() > THRESHOLD)  {}

    stop(1000);

    // now back between the pylons, turn right
    do  {
      turnRight();
    } while (leftSensor.read() < THRESHOLD);
    while (leftSensor.read() > THRESHOLD) {}

    stop(1000);

    // now back between the pylons, move forward
    moveForward();
  }

}

/*
  unsigned long currentMillis = millis(); // 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);
      }
      currentMillis = millis();
    }
    // Reset detection times after handling both sensors
    rightDetectionTime = 0;
    leftDetectionTime = 0;


  }
  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
  //}
  }
*/