from machine import Pin, PWM, ADC
import time
import math

# Constants
MOTOR1_RPM = 10  # Initial motor speed in RPM
PROJECTILE_MASS = 0.05  # Example mass in kg
MAX_VELOCITY = 20  # Max launch velocity in m/s (adjust as needed)
G = 9.81  # Acceleration due to gravity

# Motor and Servo PWM Setup
motor1_pin = Pin(15, Pin.OUT)
motor2_pin = Pin(16, Pin.OUT)
servo_pin = PWM(Pin(17))  # Servo motor PWM pin
servo_pin.freq(50)  # 50 Hz frequency for servo control

# Ultrasonic Sensor Pins
trigger_pin = Pin(18, Pin.OUT)
echo_pin = Pin(19, Pin.IN)

# LDR Setup
ldr_pin = ADC(26)

# Function to convert angle to PWM duty for servo motor (0-180 degrees)
def angle_to_duty(angle):
    return int(500 + (angle / 180) * 2000)

# Function to read distance from ultrasonic sensor
def read_distance():
    # Send a 10us pulse to the trigger pin
    trigger_pin.low()
    time.sleep_us(2)  # Wait for 2 microseconds to ensure low state
    trigger_pin.high()  # Send high pulse
    time.sleep_us(10)  # Hold for 10 microseconds
    trigger_pin.low()  # End the pulse

    # Measure the echo pulse duration
    pulse_duration = time.time_pulse_us(echo_pin, 1)
    if pulse_duration < 0:
        return float('inf')  # Return infinity if timeout occurs
    
    distance = (pulse_duration * 0.0343) / 2  # cm
    return distance

# Function to calculate launch angle for projectile motion
def calculate_launch_angle(distance):
    try:
        # Ensure the argument to arcsin is within [-1, 1]
        arg = (G * distance) / (MAX_VELOCITY ** 2)
        if arg > 1:
            return 45  # Default to 45 degrees if distance requires impossible trajectory
        angle = 0.5 * math.asin(arg)
        return math.degrees(angle)
    except ValueError:
        # In case of math domain error
        return 45  # Default angle

# Motor control function - adjust based on your motor driver requirements
def control_motor(motor_pin, state, speed=None):
    if state:
        motor_pin.high()
    else:
        motor_pin.low()

# Main control loop
def main():
    # Start motor 1
    print("Starting motor 1 at 10 RPM...")
    control_motor(motor1_pin, True)

    # Check for light input using the LDR
    ldr_value = ldr_pin.read_u16()
    light_threshold = 30000  # Define a threshold for light detection (adjust as needed)

    while ldr_value < light_threshold:
        ldr_value = ldr_pin.read_u16()  # Wait until light source is detected
        print(f"Current light level: {ldr_value}")
        time.sleep(0.1)

    print("Light source detected, stopping motor 1...")
    control_motor(motor1_pin, False)  # Stop motor 1

    # Measure distance using ultrasonic sensor
    distance = read_distance()
    print("Distance to target:", distance, "cm")

    # Calculate required launch angle based on the distance
    angle = calculate_launch_angle(distance / 100)  # Convert cm to m
    print("Calculated launch angle:", angle)

    # Set angle on servo motor
    duty = angle_to_duty(angle)
    servo_pin.duty_u16(duty)
    time.sleep(1)  # Allow servo time to reach position

    # Start motor 2
    n_rpm = 30  # Define required RPM for second motor
    print(f"Starting motor 2 at {n_rpm} RPM...")
    control_motor(motor2_pin, True)
    
    # Keep motor 2 running for some time (adjust as needed)
    time.sleep(5)
    
    # Stop motor 2 after launch
    control_motor(motor2_pin, False)
    print("Launch complete")

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