import machine
from machine import Pin, SoftI2C
from lcd_api import LcdApi
from i2c_lcd import I2cLcd
import time
# MPU6050 I2C address
MPU6050_ADDR = 0x68

# MPU6050 register addresses
MPU6050_REG_PWR_MGMT_1 = 0x6B
MPU6050_REG_ACCEL_XOUT_H = 0x3B
MPU6050_REG_GYRO_XOUT_H = 0x43

# Vibration sensor pin
VIBRATION_SENSOR_PIN = 23
buzzer_pin = Pin(2, Pin.OUT)
# Ultrasonic sensor pins
ULTRASONIC_TRIGGER_PIN = 19
ULTRASONIC_ECHO_PIN = 18

# I2C LCD address and dimensions
I2C_LCD_ADDR = 0x27
I2C_LCD_COLS = 16
I2C_LCD_ROWS = 2

# Initialize I2C
i2c = machine.I2C(scl=machine.Pin(22), sda=machine.Pin(21))
vibration_sensor = machine.Pin(VIBRATION_SENSOR_PIN, machine.Pin.IN)
ultrasonic_trigger = machine.Pin(ULTRASONIC_TRIGGER_PIN, machine.Pin.OUT)
ultrasonic_echo = machine.Pin(ULTRASONIC_ECHO_PIN, machine.Pin.IN)

# Initialize I2C LCD
lcd = I2cLcd(i2c, I2C_LCD_ADDR, I2C_LCD_ROWS, I2C_LCD_COLS)  # Swap rows and cols

def MPU6050_init():
    # Wake up MPU6050
    i2c.writeto_mem(MPU6050_ADDR, MPU6050_REG_PWR_MGMT_1, b'\x00')

def read_raw_data(addr):
    # Read raw 16-bit big-endian data from the specified address
    high = i2c.readfrom_mem(MPU6050_ADDR, addr, 1)[0]
    low = i2c.readfrom_mem(MPU6050_ADDR, addr + 1, 1)[0]
    value = (high << 8) | low
    if value >= 0x8000:
        return -((65535 - value) + 1)
    else:
        return value

def read_accel_data():
    x = read_raw_data(MPU6050_REG_ACCEL_XOUT_H)
    y = read_raw_data(MPU6050_REG_ACCEL_XOUT_H + 2)
    z = read_raw_data(MPU6050_REG_ACCEL_XOUT_H + 4)
    return (x, y, z)

def read_gyro_data():
    x = read_raw_data(MPU6050_REG_GYRO_XOUT_H)
    y = read_raw_data(MPU6050_REG_GYRO_XOUT_H + 2)
    z = read_raw_data(MPU6050_REG_GYRO_XOUT_H + 4) 
    return (x, y, z)

def measure_distance():
    # Send a 10us pulse to trigger the ultrasonic sensor
    ultrasonic_trigger.value(1)
    time.sleep_us(10)
    ultrasonic_trigger.value(0)
    
    # Measure the duration of the echo pulse
    while ultrasonic_echo.value() == 0:
        pulse_start = time.ticks_us()
    
    while ultrasonic_echo.value() == 1:
        pulse_end = time.ticks_us()
    
    # Calculate distance in centimeters
    pulse_duration = pulse_end - pulse_start
    distance = pulse_duration * 0.0343 / 2  # Speed of sound is approximately 343 m/s
    return distance

MPU6050_init()

while True:
    accel_data = read_accel_data()
    gyro_data = read_gyro_data()
    vibration_status = vibration_sensor.value()
    distance = measure_distance()
    accel_str = "Accel: X={:.2f} Y={:.2f} Z={:.2f}".format(*accel_data)
    gyro_str = "Gyro: X={:.2f} Y={:.2f} Z={:.2f}".format(*gyro_data)

    if distance<16 or vibration_status==True:
        print("Warning..")
        buzzer_pin.on()
        time.sleep(1)
        buzzer_pin.off()

    # Display distance on I2C LCD
    lcd.clear()
    lcd.putstr("Distance: " + "{:.2f} cm".format(distance))
    time.sleep(1)
    lcd.clear()
    lcd.putstr(accel_str)
    time.sleep(1)
    lcd.clear()
    lcd.putstr(gyro_str)
    time.sleep(1)
    
    print("Accelerometer (x,y,z):", accel_data)
    print("Gyroscope (x,y,z):", gyro_data)
    print("Vibration sensor:", "HIGH" if vibration_status else "LOW")
    print("Distance:", "{:.2f} cm".format(distance))

    time.sleep(1)