// This is only a diagram. The L293D chip is a placeholder.
// Below is the code used in the physical working model.

// Obstacle Avoidance Bot with two DC motors and an ultrasonic sensor

/* MA = Left
   MB = Right */

// Pins
int ma[] = {14, 13, 12}; // Motor A [Enable, Input 1, Input 2]
int mb[] = {27, 26, 25}; // Motor B [Enable, Input 1, Input 2]
const int echo = 5; const int trig = 18; // Ultrasonic Sensor

// Ultrasound
#define SOUND_SPEED 0.034
long duration;
long distanceCm;

// PWM
const int freq = 30000;
const int pwmChannel = 0;
const int resolution = 8;
int dutyCycle = 230;

// Stop
void stop() { digitalWrite(ma[1], LOW); digitalWrite(ma[2], LOW); digitalWrite(mb[1], LOW); digitalWrite(mb[2], LOW); }

// Move Forward Function
void moveForward(int dc) // Args: Duty Cycle
{
  ledcWrite(ma[0], dc); ledcWrite(mb[0], dc);
  Serial.print("Moving Forward");
  digitalWrite(ma[1], HIGH); digitalWrite(ma[2], LOW); // Motor A
  digitalWrite(mb[1], LOW); digitalWrite(mb[2], HIGH); // Motor B
}

// Move Back Function
void moveBack(unsigned long time, int dc) // Args: Time, Duty Cycle
{
  ledcWrite(ma[0], dc); ledcWrite(mb[0], dc);
  Serial.print("Moving Backwards");
  digitalWrite(ma[1], LOW); digitalWrite(ma[2], HIGH); // Motor A
  digitalWrite(mb[1], HIGH); digitalWrite(mb[2], LOW); // Motor B
  delay(time);
  stop();
}

// Turn Function
void turn(unsigned long time, int dc, int direction) // Args: Time, Duty Cycle, Direction (0 = right, 1 = left). 1s at 230 PWM ~ 270 degrees. 
{
  ledcWrite(ma[0], dc); ledcWrite(mb[0], dc);
  if (direction == 1) { // Left 
    Serial.print("Turning right for (seconds): ");  Serial.println(time / 1000);
    digitalWrite(ma[1], LOW); digitalWrite(ma[2], HIGH); // Motor A
    digitalWrite(mb[1], LOW); digitalWrite(mb[2], HIGH); // Motor B
    delay(time);
  }
  else { // Right
    Serial.print("Turning left for (seconds): ");  Serial.println(time / 1000);
    digitalWrite(ma[1], HIGH); digitalWrite(ma[2], LOW); // Motor A
    digitalWrite(mb[1], HIGH); digitalWrite(mb[2], LOW); // Motor B
    delay(time);
  }
  stop();
}

void setup() {
  // Setting Pin Modes
  for (int i = 0; i < 3; i++) { pinMode(ma[i], OUTPUT); }
  for (int i = 0; i < 3; i++) { pinMode(mb[i], OUTPUT); }
  pinMode(trig, OUTPUT);  pinMode(echo, INPUT); 

  // Initating PWMs
  ledcAttachChannel(ma[0], freq, resolution, pwmChannel);
  ledcAttachChannel(mb[0], freq, resolution, pwmChannel);

  // Hello World!
  Serial.begin(115200);
  Serial.println("Hello World!");
}

void loop() {
  // Moving forward
  moveForward(230);

  // Detection Loop
  while (true)
  {
    // Getting distance
    digitalWrite(trig, HIGH);
    delayMicroseconds(10);
    digitalWrite(trig, LOW);
    duration = pulseIn(echo, HIGH);
    float distance = (duration * SOUND_SPEED) / 2;  

    // Checking for if it's less than 40 cm
    if (distance < 40 && distance != 0)
    {
      stop(); delay(500);
      moveBack(750, 230); // Moving back to avoid obstacles
      turn(360, 230, 0); // Turning 90 degrees left
      break; // Back to moving forward
    }
  }
}