// ================= MOTOR DRIVER PINS =================
#define LEFT_MOTOR_FORWARD  18
#define LEFT_MOTOR_REVERSE  19
#define RIGHT_MOTOR_FORWARD 25
#define RIGHT_MOTOR_REVERSE 26

// ================= ULTRASONIC SENSOR =================
#define TRIG_PIN 4
#define ECHO_PIN 15

// ================= CONSTANTS =================
const int SAFE_DISTANCE = 20;
const int TURN_DELAY = 450;

void setup() {

  Serial.begin(115200);

  pinMode(TRIG_PIN, OUTPUT);
  pinMode(ECHO_PIN, INPUT);

  pinMode(LEFT_MOTOR_FORWARD, OUTPUT);
  pinMode(LEFT_MOTOR_REVERSE, OUTPUT);
  pinMode(RIGHT_MOTOR_FORWARD, OUTPUT);
  pinMode(RIGHT_MOTOR_REVERSE, OUTPUT);

  stopMotors();
}

void loop() {

  int distance = getDistance();

  Serial.print("Distance: ");
  Serial.print(distance);
  Serial.println(" cm");

  if (distance > SAFE_DISTANCE || distance == 0) {
    moveForward();
  }
  else {
    stopMotors();
    delay(200);

    reverse();
    delay(350);

    stopMotors();
    delay(200);

    turnRight90();
    delay(TURN_DELAY);

    stopMotors();
    delay(200);
  }
}

// ================= ULTRASONIC =================
int getDistance() {

  long duration;
  int distance;

  digitalWrite(TRIG_PIN, LOW);
  delayMicroseconds(2);

  digitalWrite(TRIG_PIN, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG_PIN, LOW);

  duration = pulseIn(ECHO_PIN, HIGH, 30000);

  if (duration == 0) return 100;

  distance = duration * 0.034 / 2;
  return distance;
}

// ================= MOTOR CONTROL =================

void moveForward() {
  digitalWrite(LEFT_MOTOR_FORWARD, HIGH);
  digitalWrite(LEFT_MOTOR_REVERSE, LOW);
  digitalWrite(RIGHT_MOTOR_FORWARD, HIGH);
  digitalWrite(RIGHT_MOTOR_REVERSE, LOW);
}

void reverse() {
  digitalWrite(LEFT_MOTOR_FORWARD, LOW);
  digitalWrite(LEFT_MOTOR_REVERSE, HIGH);
  digitalWrite(RIGHT_MOTOR_FORWARD, LOW);
  digitalWrite(RIGHT_MOTOR_REVERSE, HIGH);
}

void turnRight90() {
  digitalWrite(LEFT_MOTOR_FORWARD, HIGH);
  digitalWrite(LEFT_MOTOR_REVERSE, LOW);
  digitalWrite(RIGHT_MOTOR_FORWARD, LOW);
  digitalWrite(RIGHT_MOTOR_REVERSE, HIGH);
}

void stopMotors() {
  digitalWrite(LEFT_MOTOR_FORWARD, LOW);
  digitalWrite(LEFT_MOTOR_REVERSE, LOW);
  digitalWrite(RIGHT_MOTOR_FORWARD, LOW);
  digitalWrite(RIGHT_MOTOR_REVERSE, LOW);
}