// Servo Sweep example for the ESP32
// https://wokwi.com/arduino/projects/323706614646309460

#include <ESP32Servo.h>

#define TRIGGER_PIN_DEPAN 33  // ESP32 pin GIOP23 connected to Ultrasonic Sensor's TRIG pin
#define ECHO_PIN_DEPAN 32  // ESP32 pin GIOP22 connected to Ultrasonic Sensor's ECHO pin
#define TRIGGER_PIN_KIRI 23  // ESP32 pin GIOP23 connected to Ultrasonic Sensor's TRIG pin
#define ECHO_PIN_KIRI 22  // ESP32 pin GIOP22 connected to Ultrasonic Sensor's ECHO pin
#define TRIGGER_PIN_KANAN 13 // ESP32 pin GIOP23 connected to Ultrasonic Sensor's TRIG pin
#define ECHO_PIN_KANAN 12 // ESP32 pin GIOP22 connected to Ultrasonic Sensor's ECHO pin
#define SERVO_PIN 18 // ESP32 pin GIOP26 connected to Servo Motor's pin
#define DISTANCE_THRESHOLD 7 // centimeters

Servo servo; // create servo object to control a servo

// variables will change:
float pulse_depan, pulse_kiri, pulse_kanan,
      distance_depan, distance_kiri, distance_kanan;

void setup() {
  Serial.begin (9600);
  pinMode(TRIGGER_PIN_DEPAN, OUTPUT);
  pinMode(ECHO_PIN_DEPAN, INPUT);
  pinMode(TRIGGER_PIN_KIRI, OUTPUT);
  pinMode(ECHO_PIN_KIRI, INPUT);
  pinMode(TRIGGER_PIN_KANAN, OUTPUT);
  pinMode(ECHO_PIN_KANAN, INPUT);

  servo.attach(SERVO_PIN);
  servo.write(0);
}

void loop() {
  // ambil nilai pulse dari function readPulse
  distance_depan = getDistanceByPulse(TRIGGER_PIN_DEPAN, ECHO_PIN_DEPAN, 10);
  distance_kiri = getDistanceByPulse(TRIGGER_PIN_KIRI, ECHO_PIN_KIRI, 10);
  distance_kanan = getDistanceByPulse(TRIGGER_PIN_KANAN, ECHO_PIN_KANAN, 10);

  // kalau masih jauh, gapapa lurus aj terus gkperlu manuver
  if(distance_depan > DISTANCE_THRESHOLD) {
    Serial.println("lurus...");
    return;
  }
  Serial.println();

  char msg_log[100];
  // kalau masuk sini berarti distance_depan < DISTANCE_TRESHOLD,
  // print : butuh belok nich
  sprintf(msg_log, "## jarak ke depan otw nabrak : %.2f cm\n", distance_depan);
  Serial.print(msg_log);

  // print : detek jarak kiri dan kanan
  sprintf(msg_log, "|kiri  : %.2f\n|kanan : %.2f\n", distance_kiri, distance_kanan);
  Serial.print(msg_log);

  // kalkulasi mana yang lebih jauh jarak tabrakannya,
  // itu yang kita ambil arah beloknya
  int turn = (distance_kiri > distance_kanan) ? 0 : 180;

  // gas belok keun putar kemudi
  servo.write(turn);

  // print : tampilkan sekarang kita belok ke mana
  Serial.print("-> ");
  if(turn == 0) {
    Serial.println("arah kiri menuju tabrakan lebih jauh, belok ke kiri\n");
  }
  else if(turn == 180) {
    Serial.println("arah kanan menuju tabrakan lebih jauh, belok ke kanan\n");
  }

  delay(500);
}

float getDistanceByPulse(int triggerPin, int echoPin, int delayTime) {
  // trigger sensor
  digitalWrite(triggerPin, HIGH);
  delayMicroseconds(delayTime);
  digitalWrite(triggerPin, LOW);

  float duration = pulseIn(echoPin, HIGH); // read pulse
  float distance_cm = duration * 0.017; // distance in cm
  return distance_cm; // return distance
}