#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#include <Servo.h>
#define LCD_I2C_ADDRESS 0x3C
#define LEBAR_LCD 128
#define TINGGI_LCD 64
#define TOMBOL_T1 2
#define TOMBOL_T2 3
#define LAMPU_L1 9
#define LAMPU_L2 10
#define LAMPU_L3 11
Adafruit_SSD1306 lcd (LEBAR_LCD, TINGGI_LCD, &Wire);

Servo servo1;
byte sudut_servo = 0;
byte step_servo = 0;
long timer_millis =0;
#define PIN_SERVO1 12

void setup() {
  pinMode(TOMBOL_T1, INPUT_PULLUP); 
  pinMode(TOMBOL_T2, INPUT_PULLUP);
  pinMode(LAMPU_L1, OUTPUT);
  pinMode(LAMPU_L2, OUTPUT);
  pinMode(LAMPU_L3, OUTPUT);

lcd.begin(SSD1306_SWITCHCAPVCC, LCD_I2C_ADDRESS); 
servo1.attach(PIN_SERVO1);
lcd.clearDisplay();
lcd.setTextSize(1);
lcd.setTextColor(WHITE);
lcd.setCursor(0,0);
lcd.print("BLPT DIY");
lcd.setTextColor(BLACK, WHITE);
lcd.setCursor(0,10);
lcd.print("Embedded System");
lcd.setTextColor(WHITE);
lcd.setCursor(0,20);
lcd.print("2024");
lcd.display();
delay(2000);

}

void loop() {
  // Membaca status tombol T1 dan T2
  bool tombol_T1 = digitalRead(TOMBOL_T1);
  bool tombol_T2 = digitalRead(TOMBOL_T2);

  // Mengubah posisi servo sesuai dengan tombol yang ditekan
  if (tombol_T1 == LOW) { // Jika tombol T1 ditekan
    sudut_servo += 1; // Servo bertambah 1 derajat
    if (sudut_servo > 180) {
      sudut_servo = 180; // Maksimum posisi servo 180 derajat
    }
  }
  else if (tombol_T2 == LOW) { // Jika tombol T2 ditekan
    sudut_servo -= 1; // Servo berkurang 10 derajat
    if (sudut_servo < 0) {
      sudut_servo = 0; // Minimum posisi servo 0 derajat
    }
  }

  // Menggerakkan servo sesuai dengan posisi yang diinginkan
  servo1.write(sudut_servo);

  // Memperbarui status lampu sesuai dengan posisi servo
  updateLampu();

  // Menampilkan informasi di LCD
  lcd.clearDisplay();
  lcd.setTextSize(1);
  lcd.setTextColor(WHITE);
  lcd.setCursor(0,0);
  lcd.print("Sudut servo:");
  lcd.print(sudut_servo);

  lcd.display();

  delay(100); // Delay untuk mencegah bouncing pada tombol
}

void updateLampu() {
  if (sudut_servo == 0) {
    digitalWrite(LAMPU_L1, HIGH);
    digitalWrite(LAMPU_L2, LOW);
    digitalWrite(LAMPU_L3, LOW);
  } else if (sudut_servo == 90) {
    digitalWrite(LAMPU_L1, LOW);
    digitalWrite(LAMPU_L2, HIGH);
    digitalWrite(LAMPU_L3, LOW);
  } else if (sudut_servo == 180) {
    digitalWrite(LAMPU_L1, LOW);
    digitalWrite(LAMPU_L2, LOW);
    digitalWrite(LAMPU_L3, HIGH);
  }
}