import machine
from machine import Pin, SoftI2C
from lcd_api import LcdApi
from i2c_lcd import I2cLcd
import time
import network 
from blynkLib import Blynk

# Initialize Blynk
BLYNK_AUTH_TOKEN = 'jZ3oEv1GduQZ5yGzNKpK6KL_Oqw6x7QY'
wifi= network.WLAN(network.STA_IF) 
wifi.active(True) 
wifi.connect("Wokwi-GUEST","")
while not wifi.isconnected(): 
    pass 
print("Wifi Connected Successfully") 

blynk = Blynk(BLYNK_AUTH_TOKEN)

# 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)
    
    # Initialize pulse_end
    pulse_end = 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()
    
    # Send sensor data to Blynk
    #blynk.virtual_write(0, accel_data)  # Accelerometer X
    # blynk.virtual_write(1, accel_data[1])  # Accelerometer Y
    # blynk.virtual_write(2, accel_data[2])  # Accelerometer Z
    # blynk.virtual_write(3, gyro_data[0])   # Gyroscope X
    # blynk.virtual_write(4, gyro_data[1])   # Gyroscope Y
    # blynk.virtual_write(5, gyro_data[2])   # Gyroscope Z
    blynk.virtual_write(7, distance)       # Ultrasonic distance
    blynk.virtual_write(6, vibration_status)  # Vibration sensor status

    # Display data on LCD
    accel_str = "Accel: X={:.2f} Y={:.2f} Z={:.2f}".format(*accel_data)
    gyro_str = "Gyro: X={:.2f} Y={:.2f} Z={:.2f}".format(*gyro_data)
    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 data to console
    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)