#include <Wire.h>
#include <RTClib.h>
#include <Servo.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>

#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 32

const int TRIG_PIN_DISPLAY = 4;
const int ECHO_PIN_DISPLAY = 5;
const int TRIG_PIN_SERVO = 12;
const int ECHO_PIN_SERVO = 11;
const int servoPin = 3;

#define OLED_RESET -1
#define SCREEN_ADDRESS 0x3C
#define MAX_DISTANCE 6
#define MIN_DISTANCE 30

Adafruit_SSD1306 oled(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);
Servo myservo;
RTC_DS3231 rtc; // Change to DS3231 for better accuracy

void setup() {
  Serial.begin(115200);
  pinMode(ECHO_PIN_DISPLAY, INPUT);
  pinMode(TRIG_PIN_DISPLAY, OUTPUT);
  pinMode(ECHO_PIN_SERVO, INPUT);
  pinMode(TRIG_PIN_SERVO, OUTPUT);
  myservo.attach(servoPin);
  oled.begin(SSD1306_SWITCHCAPVCC, 0x3C);
  oled.clearDisplay();
  oled.setTextSize(4);
  oled.setTextColor(SSD1306_WHITE);

  if (!rtc.begin()) {
    Serial.println("Couldn't find RTC");
    while (1);
  }

  if (rtc.lostPower()) {
    Serial.println("RTC lost power, let's set the time!");
    rtc.adjust(DateTime(F(__DATE__), F(__TIME__)));
  }

  if (!oled.begin(SSD1306_SWITCHCAPVCC, 0x3C)) {
    Serial.println(F("Pengalokasian SSD1306 gagal"));
    while (true);
  }
}

void loop() {
  oled.clearDisplay();
  
  DateTime now = rtc.now();
  int currentHour = now.hour();
  int currentMinute = now.minute();

  Serial.print("Current Time: ");
  Serial.print(currentHour);
  Serial.print(":");
  Serial.println(currentMinute);

  if ((currentHour >= 21) && (currentHour <= 4)) {
    digitalWrite(ECHO_PIN_SERVO, LOW);
    digitalWrite(TRIG_PIN_SERVO, LOW);
    digitalWrite(ECHO_PIN_DISPLAY, LOW);
    digitalWrite(TRIG_PIN_DISPLAY, LOW);
    myservo.detach();
    oled.ssd1306_command(SSD1306_DISPLAYOFF);
    Serial.println("Mode Tidur");
  } else {
    float distance_cm_display = readDistanceCM_Display(ECHO_PIN_DISPLAY, TRIG_PIN_DISPLAY);
    float distance_cm_servo = readDistanceCM_Servo(ECHO_PIN_SERVO, TRIG_PIN_SERVO);
    
    int bars = map(distance_cm_display, MIN_DISTANCE, MAX_DISTANCE, 0, 6); // Ubah nilai 5 sesuai dengan jumlah bar yang diinginkan
    drawBatteryBars(bars);

    Serial.print("Distance (Display): ");
    Serial.print(distance_cm_display);
    Serial.println(" cm");

    Serial.print("Distance (Servo): ");
    Serial.print(distance_cm_servo);
    Serial.println(" cm");
    delay(600);
    
    if (distance_cm_servo <= 8) {
      myservo.write(0);
      delay(4000); 
    } else if (distance_cm_servo > 8) {
      myservo.write(40);
    }

    oled.ssd1306_command(SSD1306_DISPLAYON);
  }
}

void drawBatteryBars(int bars) {
  int barWidth = 20;  // Width of each battery bar
  int barHeight = SCREEN_HEIGHT - 2;  // Height of the battery icon
  int gap = 2;  // Gap between bars

  int batteryWidth = bars * (barWidth + gap) - gap;  // Total width of the battery icon

  // Calculate the starting X position to align the battery icon to the left side of the screen
  int startX = 0;

  // Draw border
  oled.drawRect(0, 0, SCREEN_WIDTH, SCREEN_HEIGHT, SSD1306_WHITE);

  for (int i = 0; i < bars; i++) {
    int x = startX + i * (barWidth + gap);
    int y = 1;  // Adjust the Y position to center the battery icon vertically

    // Draw each battery bar
    oled.fillRect(x, y, barWidth, barHeight, SSD1306_WHITE);
  }

  oled.display();
}
float readDistanceCM_Servo(int echoPin, int trigPin) {
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
  unsigned long duration = pulseIn(echoPin, HIGH);
  return duration * 0.034 / 2;
}

float readDistanceCM_Display(int echoPin, int trigPin) {
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
  
  unsigned long duration = pulseIn(echoPin, HIGH);
  float distance_cm = duration * 0.034 / 2;

  return distance_cm;
}