from machine import Pin, SoftI2C
import ssd1306
import time

# Inisialisasi I2C untuk OLED dan MPU6050
i2c = SoftI2C(scl=Pin(22), sda=Pin(21))
oled_width = 128
oled_height = 64
oled = ssd1306.SSD1306_I2C(oled_width, oled_height, i2c)

# Inisialisasi Ultrasonik HC-SR04
trigger_pin = Pin(5, Pin.OUT)
echo_pin = Pin(18, Pin.IN)

# MPU6050 Address
MPU6050_ADDR = 0x68

# MPU6050 Register
MPU6050_ACCEL_XOUT_H = 0x3B
MPU6050_PWR_MGMT_1 = 0x6B

# Wake up MPU6050
i2c.writeto_mem(MPU6050_ADDR, MPU6050_PWR_MGMT_1, b'\x00')

def read_mpu6050():
    accel_data = i2c.readfrom_mem(MPU6050_ADDR, MPU6050_ACCEL_XOUT_H, 6)
    accel_x = int.from_bytes(accel_data[0:2], 'big')
    accel_y = int.from_bytes(accel_data[2:4], 'big')
    accel_z = int.from_bytes(accel_data[4:6], 'big')

    # MPU6050 memberikan nilai 16-bit signed, konversi jika perlu
    if accel_x > 32767:
        accel_x -= 65536
    if accel_y > 32767:
        accel_y -= 65536
    if accel_z > 32767:
        accel_z -= 65536

    return accel_x, accel_y, accel_z

def get_distance():
    # Kirimkan sinyal trigger
    trigger_pin.off()
    time.sleep_us(2)
    trigger_pin.on()
    time.sleep_us(10)
    trigger_pin.off()

    # Tunggu sinyal echo
    signal_off = 0
    signal_on = 0

    # Tangkap waktu saat echo pin low (start)
    while echo_pin.value() == 0:
        signal_off = time.ticks_us()
    # Tangkap waktu saat echo pin high (end)
    while echo_pin.value() == 1:
        signal_on = time.ticks_us()

    # Hitung waktu yang berlalu (time_passed)
    time_passed = time.ticks_diff(signal_on, signal_off)
    
    # Kecepatan suara di udara = 34300 cm/s, hitung jarak
    distance = (time_passed * 0.0343) / 2
    return distance

while True:
    # Baca jarak dari Ultrasonik
    distance = get_distance()  # Mendapatkan jarak dalam cm
    
    # Baca nilai akselerasi dari MPU6050
    accel_x, accel_y, accel_z = read_mpu6050()

    # Tampilkan data di OLED
    oled.fill(0)
    oled.text("Distance:", 0, 0)
    oled.text("{:.2f} cm".format(distance), 0, 10)
    
    # Tampilkan nilai akselerasi
    oled.text("Accel X: {}".format(accel_x), 0, 20)
    oled.text("Accel Y: {}".format(accel_y), 0, 30)
    oled.text("Accel Z: {}".format(accel_z), 0, 40)

    oled.show()
    
    time.sleep(1)