#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#include <Arduino.h>

#define SCREEN_WIDTH 128 // lebar OLED display, dalam pixel
#define SCREEN_HEIGHT 64 // tinggi OLED display, dalam pixel
#define OLED_RESET     -1 // Reset pin # (atau -1 jika sharing Arduino reset pin)
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

// Servo motor pin
const int servoPin = 17;

// Ultrasonic sensor pins
const int trigPin1 = 16;
const int echoPin1 = 4;
const int trigPin2 = 2;
const int echoPin2 = 15;

// Servo position variables
const int angle1 = 90;    // angle for position 1
const int angle2 = 0;     // angle for position 2
int currentAngle = angle1; // Current angle of the servo

void setup() {
  // Initialize Serial
  Serial.begin(9600);

  // Initialize I2C for OLED
  Wire.begin(18, 19); // SDA di pin 2, SCL di pin 4

  // Initialize OLED display
  if(!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) {
    Serial.println(F("SSD1306 allocation failed"));
    for(;;);
  }
  display.clearDisplay();
  display.setTextSize(1);
  display.setTextColor(SSD1306_WHITE);
  display.setCursor(0,0);
  display.println(F("Selamat Datang Joyo"));
  display.display();
  delay(2000); // Tampilkan selamat datang selama 2 detik

  // Initialize ultrasonic sensors
  pinMode(trigPin1, OUTPUT);
  pinMode(echoPin1, INPUT);
  pinMode(trigPin2, OUTPUT);
  pinMode(echoPin2, INPUT);

  // Initialize servo
  ledcSetup(0, 50, 16); // channel 0, 50 Hz PWM, 16-bit resolution
  ledcAttachPin(servoPin, 0);
}

void loop() {
  // Read distances from ultrasonic sensors
  float distance1 = getDistance(trigPin1, echoPin1);
  float distance2 = getDistance(trigPin2, echoPin2);

  // Check distances and move servo
  if (distance1 <= 10 && distance2 > 10) {
    moveServo(angle2);
    currentAngle = angle2;
  }
  else if (distance2 <= 10 && distance1 > 10) {
    moveServo(angle2);
    currentAngle = angle2;
  } 
  else if (distance2 <= 10 && distance1 <= 10) {
    moveServo(angle2);
    currentAngle = angle2;
  } else {
    moveServo(angle1);
    currentAngle = angle1;
  }

  // Update OLED display with current angle
  display.clearDisplay();
  display.setCursor(0,0);
  display.print(F("Jarak 1 : "));
  display.print(distance1);
  display.println(F(" Cm"));

  display.setCursor(0,16);
  display.print(F("Jarak 2 : "));
  display.print(distance2);
  display.println(F(" Cm"));
  display.display();

   display.setCursor(0,32);
  display.print(F("Sudut: "));
  display.print(currentAngle);
  display.println(F(" Derajat"));
  display.display();

  delay(200); // Tunda untuk stabilitas
}

float getDistance(int trigPin, int echoPin) {
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin,HIGH);
delayMicroseconds(10);
digitalWrite(trigPin, LOW);

long duration = pulseIn(echoPin, HIGH);
float distance = duration * 0.034 / 2;
return distance;
}

void moveServo(int angle) {
int dutyCycle = map(angle, 0, 180, 3277, 8191); // Convert angle to duty cycle
ledcWrite(0, dutyCycle); // Write to servo
}