from machine import Pin, I2C
from ssd1306 import SSD1306_I2C
import utime
from hcsr04 import HCSR04

# Inisialisasi pin untuk sensor jarak HC-SR04
trigger_pin = 8  # Sesuaikan dengan pin yang digunakan
echo_pin = 9     # Sesuaikan dengan pin yang digunakan

# Inisialisasi pin untuk I2C pada SSD1306
scl_pin = 27     # Sesuaikan dengan pin yang digunakan
sda_pin = 26     # Sesuaikan dengan pin yang digunakan

# Inisialisasi OLED display
pix_res_x = 128
pix_res_y = 64
i2c_dev = I2C(1, scl=Pin(scl_pin), sda=Pin(sda_pin), freq=200000)
oled = SSD1306_I2C(pix_res_x, pix_res_y, i2c_dev)

# Inisialisasi sensor jarak HC-SR04
sensor = HCSR04(trigger_pin, echo_pin)

# Inisialisasi pin untuk lampu LED
red_led_pin = 5      # Sesuaikan dengan pin yang digunakan
yellow_led_pin = 6   # Sesuaikan dengan pin yang digunakan
green_led_pin = 7    # Sesuaikan dengan pin yang digunakan

# Konfigurasi pin sebagai output
red_led = Pin(red_led_pin, Pin.OUT)
yellow_led = Pin(yellow_led_pin, Pin.OUT)
green_led = Pin(green_led_pin, Pin.OUT)

# Inisialisasi pin untuk LED RGB
red_pin = 10    # Sesuaikan dengan pin yang digunakan
green_pin = 11  # Sesuaikan dengan pin yang digunakan
blue_pin = 12   # Sesuaikan dengan pin yang digunakan
common_pin = 13 # Sesuaikan dengan pin yang digunakan

# Konfigurasi pin sebagai output untuk LED RGB
red_rgb_led = Pin(red_pin, Pin.OUT)
green_rgb_led = Pin(green_pin, Pin.OUT)
blue_rgb_led = Pin(blue_pin, Pin.OUT)
common_rgb_led = Pin(common_pin, Pin.OUT)

def display_distance(distance):
    oled.fill(0)
    oled.text("Distance:", 5, 5)
    oled.text("{:.2f} cm".format(distance), 5, 15)
    oled.show()

def control_leds(distance):
    if distance < 100:
        # Kontrol RGB LED
        red_rgb_led.on()
        green_rgb_led.off()
        blue_rgb_led.off()
    elif 100 <= distance <= 200:
        # Kontrol RGB LED
        red_rgb_led.off()
        green_rgb_led.on()
        blue_rgb_led.off()
    elif 200 < distance <= 400:
        # Kontrol RGB LED
        red_rgb_led.off()
        green_rgb_led.off()
        blue_rgb_led.on()
    else:
        # Matikan RGB LED jika jarak tidak sesuai dengan kondisi di atas
        red_rgb_led.off()
        green_rgb_led.off()
        blue_rgb_led.off()

    # Kontrol LED warna lainnya seperti sebelumnya
    if distance < 100:
        red_led.on()
        yellow_led.off()
        green_led.off()
    elif 100 <= distance <= 200:
        red_led.off()
        yellow_led.on()
        green_led.off()
    elif 200 < distance <= 400:
        red_led.off()
        yellow_led.off()
        green_led.on()
    else:
        red_led.off()
        yellow_led.off()
        green_led.off()

def main():
    try:
        while True:
            # Baca jarak dari sensor HC-SR04
            distance = sensor.distance_cm()

            # Tampilkan jarak di OLED display
            display_distance(distance)

            # Kontrol LED berdasarkan jarak
            control_leds(distance)

            # Tunggu sejenak sebelum membaca kembali
            utime.sleep(1)

    except KeyboardInterrupt:
        # Tangkap exception jika pengguna menekan Ctrl+C
        pass

if __name__ == '__main__':
    main()