# #LCD Screen_____________________________________________________________________________________
# from machine import I2C, Pin    # since I2C communication would be used, I2C class is imported
# from time import sleep
# from pico_i2c_lcd import I2cLcd # this nust be saved in the Raspberry Pi Pico

# # creating an I2C object, specifying the data (SDA) and clock (SCL) pins used in the Raspberry Pi Pico
# # any SDA and SCL pins in the Raspberry Pi Pico can be used (check documentation for SDA and SCL pins)
# i2c = I2C(0, sda=Pin(0), scl=Pin(1), freq=400000)

# # getting I2C address
# I2C_ADDR = i2c.scan()[0]

# # Creating LCD object using I2C and specify number of rows and columns in LCD
# # LCD number of rows = 2, number of columns = 16
# lcd = I2cLcd(i2c, I2C_ADDR, 2, 16)


# #Set Up Servo Motor________________________________________________________________________________
# from Servo import set_angle


# #PID Controller from class function I 'built'______________________________________________________
# from PID import *
# # Initialize the PID-controlled servo to respond to how close an object is
# pid = PID(kp= 50, ki= 5, kd=15)#1.0, 0.05, 0.25 for ultrasonic sensor
# target_value = 0.5  # Desired distance from the ultrasonic sensor
# target_angle = 90   # Desired angle the servo should initially be at


# #Initiate Accelermometer_____________________________________________________________________________
# from Accel import * #Imports class function for the acceleramometer
# from machine import I2C, Pin
# import utime

# # Setup I2C
# i2c_acc = I2C(0, sda=Pin(20), scl=Pin(21), freq=400000)  # Initialize I2C bus

# # Create MPU6050 object
# mpu = MPU6050(i2c_acc)


# # Run for Accelerometer ____________________________________________________________________________________
# while True:
#     accel_x, accel_y, accel_z = mpu.read_accel()
#     gyro_x, gyro_y, gyro_z = mpu.read_gyro()
#     print("Acceleration: X:{0} Y:{1} Z:{2}".format(accel_x, accel_y, accel_z))
#     # print("Gyroscope: X:{0} Y:{1} Z:{2}".format(gyro_x, gyro_y, gyro_z))

#     # Calculate the PID output to adjust the servo
#     pid_output = pid.compute(target_value, accel_z)
#     new_angle = min(max(int(target_angle + pid_output), 0), 180)
#     set_angle(angle=new_angle,pin=16)

#     # lcd.putstr(f"{str(accel_z)} cm")
#     sleep(0.25)
#     # lcd.clear()



# # #Run for ultrasonic sensor_______________________________________________________________________________
# # From Ultra import ultra #This is function for ultra sonic sensor I wrote

# # # Initialize the servo to the target angle
# # set_angle(target_angle)

# # # Main control loop
# # while True:
# #     distance = ultra(trig=14,echo=15)
# #     lcd.putstr(f"{str(distance)} cm")

# #     # Calculate the PID output to adjust the servo
# #     pid_output = pid.compute(target_value, distance)
# #     new_angle = min(max(int(target_angle + pid_output), 0), 180)
# #     set_angle(new_angle)
# #     sleep(0.25)
# #     lcd.clear()





#########################    THIS IS CLEANER CODE FROM GPT   ####################################################

from machine import I2C, Pin, PWM
from pico_i2c_lcd import I2cLcd
from utime import sleep, sleep_us, ticks_us, ticks_diff
from PID import PID  # Assuming PID.py is properly defined
from Accel import MPU6050  # Assuming Accel.py correctly defines MPU6050

# Constants
LCD_I2C_ID = 0
ACCEL_I2C_ID = 0  # Share the bus if necessary, or use different buses
SERVO_PIN = 16
TRIG_PIN = 14
ECHO_PIN = 15

# Initialize I2C for LCD and Accelerometer
i2c_lcd = I2C(LCD_I2C_ID, sda=Pin(0), scl=Pin(1), freq=400000)
lcd_address = i2c_lcd.scan()[0]  # Automatically find LCD address
lcd = I2cLcd(i2c_lcd, lcd_address, 2, 16)  # Assume 16x2 LCD

i2c_acc = I2C(ACCEL_I2C_ID, sda=Pin(20), scl=Pin(21), freq=400000)  # MPU6050 I2C
mpu = MPU6050(i2c_acc)  # Initialize MPU6050

# Initialize Ultrasonic Sensor
trig = Pin(TRIG_PIN, Pin.OUT)
echo = Pin(ECHO_PIN, Pin.IN)

def read_ultrasonic():
    trig.low()
    sleep_us(2)
    trig.high()
    sleep_us(10)
    trig.low()
    while echo.value() == 0:
        signal_off = ticks_us()
    while echo.value() == 1:
        signal_on = ticks_us()
    time_passed = ticks_diff(signal_on, signal_off)
    distance = (time_passed * 0.034) / 2
    return round(distance, 1)

# Initialize Servo
servo = PWM(Pin(SERVO_PIN))
servo.freq(50)

def set_angle(angle):
    min_duty = 1550
    max_duty = 8150
    duty = min_duty + (max_duty - min_duty) * angle // 180
    servo.duty_u16(duty)

# Initialize PID Controller
pid = PID(kp=50, ki=5, kd=15)
target_value = 0.5  # Desired distance or acceleration impact
target_angle = 90




# Combined Main Loop
while True:
    # Read Accelerometer and Gyroscope
    accel_x, accel_y, accel_z = mpu.read_accel()
    gyro_x, gyro_y, gyro_z = mpu.read_gyro()
    
    # # Update LCD with Accelerometer Data
    # lcd.clear()
    # lcd.move_to(0, 0)
    # lcd.putstr(f"Ax:{accel_x:.2f}g Ay:{accel_y:.2f}g")
    # lcd.move_to(0, 1)
    # lcd.putstr(f"Az:{accel_z:.2f}g Gz:{gyro_z:.2f}dps")
    print(f"Ax:{accel_x:.2f}g Ay:{accel_y:.2f}g")
    # Update Servo based on PID response to Accelerometer Z
    pid_output = pid.compute(target_value, accel_z)
    new_angle = min(max(int(target_angle + pid_output), 0), 180)
    set_angle(new_angle)

    sleep(0.25)

#     # Uncomment to include ultrasonic sensor reading
#     # distance = read_ultrasonic()
#     # lcd.clear()
#     # lcd.putstr(f"Distance: {distance}cm")
#     # pid_output = pid.compute(target_value, distance)
#     # new_angle = min(max(int(target_angle + pid_output), 0), 180)
#     # set_angle(new_angle)
#     # sleep(0.25)
#     # lcd.clear()