from machine import Pin, PWM
import utime
from hcsr04 import HCSR04
import machine
import math
import mpu6050  # Import the MPU6050 module

# Initialize I2C for MPU6050 (adjust with your pins)
i2c = machine.I2C(0, scl=machine.Pin(22), sda=machine.Pin(21))
mpu = mpu6050.MPU6050(i2c)

# HC-SR04 Sensor for obstacle detection
sensor = HCSR04(trigger_pin=5, echo_pin=18)

# Buzzer for alerts
buzzer = PWM(Pin(19))

# Vibration motor for orientation alert (connected to PWM pin)
vibration_motor = PWM(Pin(16))  # Adjust the pin number as per your setup

# Function to read distance from HC-SR04 sensor
def read_distance():
    try:
        distance = sensor.distance_cm()  # Get the distance in cm
        return distance
    except Exception as e:
        print("Error reading distance:", e)
        return None

# Function to alert based on distance
def obstacle_alert(distance):
    if distance is not None and 0 < distance < 100:  # Check if obstacle is within range
        frequency = int(2000 - distance * 15) * 4  # Closer object → Higher frequency
        duty = int((100 - distance) * 10)  # Closer object → Higher duty cycle
        duty = min(duty, 1023)  # Limit duty cycle to max 1023
        
        buzzer.freq(frequency)
        buzzer.duty(duty)
        print(f"Obstacle detected! Distance: {distance:.2f} cm, Frequency: {frequency} Hz, Duty: {duty}")
    else:
        buzzer.duty(0)  # Turn off the buzzer if no obstacle

# Calculate tilt angles (pitch and roll)
def calculate_tilt(accel_data):
    x = accel_data['x']
    y = accel_data['y']
    z = accel_data['z']
    pitch = math.atan2(x, math.sqrt(y**2 + z**2)) * 180 / math.pi
    roll = math.atan2(y, math.sqrt(x**2 + z**2)) * 180 / math.pi
    return pitch, roll

# Function to activate vibrational alert
def vibrational_alert():
    vibration_motor.freq(1000)  # Set a vibration frequency (e.g., 1000Hz)
    vibration_motor.duty(512)  # Set the motor to 50% duty cycle (moderate vibration)
    print("Vibrational Alert: Orientation out of bounds!")
    utime.sleep(0.5)  # Vibration for 0.5 seconds
    vibration_motor.duty(0)  # Stop the vibration after the alert

# Main function to integrate both functionalities
def main():
    tilt_threshold = 30  # Tilt threshold in degrees
    
    while True:
        # Read accelerometer data from MPU6050
        accel = mpu.get_accel()
        pitch, roll = calculate_tilt(accel)
        
        # Print tilt angles
        print(f"Pitch: {pitch:.2f}, Roll: {roll:.2f}")
        
        # Read distance from HC-SR04 sensor
        distance = read_distance()
        
        # Check for potential fall
        if distance is not None and distance < 10 and (abs(pitch) > tilt_threshold or abs(roll) > tilt_threshold):
            print("Fall Detected: Distance < 10 cm and Orientation out of bounds!")
            buzzer.freq(1000)
            buzzer.duty(512)
            vibrational_alert()
        else:
            # Handle normal alerts if no fall is detected
            if abs(pitch) > tilt_threshold or abs(roll) > tilt_threshold:
                print("Warning: Device orientation out of bounds!")
                vibrational_alert()  # Trigger the vibrational alert
            
            # Alert if an obstacle is detected
            obstacle_alert(distance)
        
        # Sleep for a while to avoid overloading the CPU
        utime.sleep(0.1)

# Run the main function
if __name__ == "__main__":
    main()