# Project objective: Swing the servo arm from min, mid, to max positions
#
# Hardware and connections used:
#   Servo GND to Raspberry Pi Pico GND
#   Servo V+ to Raspberry Pi Pico 3.3 V
#   Servo PWM pin to GPIO Pin 15
#   Servo PWM pin to GPIO Pin 13
# 
# Programmer: Adrian Josele G. Quional

# modules
from picozero import Servo
from time import sleep
from machine import Pin, time_pulse_us
from utime import sleep_us, sleep_ms
import math

# creating a Servo object
servo_left_right = Servo(15)
servo_up_down = Servo(13)


SOUND_SPEED=340 # Vitesse du son dans l'air
TRIG_PULSE_DURATION_US=10
DISTANCE_BETWEEN_SENSORS = 20 # in cm
RAD_TO_DEGREE = 360/(2*3.14)

trig_pin1 = Pin(16, Pin.OUT) # Broche GP15 de la Pico
echo_pin1 = Pin(17, Pin.IN)  # Broche GP14 de la Pico

trig_pin2 = Pin(20, Pin.OUT) # Broche GP15 de la Pico
echo_pin2 = Pin(19, Pin.IN)  # Broche GP14 de la Pico

def calculate_distance(time_in_us):
     return (SOUND_SPEED * ultrason_duration / 20000)
     
def calculate_angle(distance1, distance2):
    cos_alpha = (pow(DISTANCE_BETWEEN_SENSORS, 2)+pow(distance_cm1, 2)-pow(distance_cm2, 2))/(2*DISTANCE_BETWEEN_SENSORS*distance_cm1)
    print("  ", cos_alpha)
    beta = 0
    if (cos_alpha <=1 and cos_alpha > 0):
        alpha = math.acos(cos_alpha)
        print(alpha*RAD_TO_DEGREE)
        
        distance_to_eye = math.sqrt((pow(DISTANCE_BETWEEN_SENSORS/2, 2)+pow(distance_cm1, 2)-2*distance_cm1*DISTANCE_BETWEEN_SENSORS/2*cos_alpha))
        print("distance_to_eye:", distance_to_eye)

        sin_beta = distance_cm1*math.sin(alpha)/distance_to_eye
        beta = math.asin(sin_beta)
        print(beta*RAD_TO_DEGREE)
    return beta

while True:
    # Prepare le signal
    trig_pin1.value(0)
    sleep_us(5)
    # Créer une impulsion de 10 µs
    trig_pin1.value(1)
    sleep_us(TRIG_PULSE_DURATION_US)
    trig_pin1.value(0)

    ultrason_duration = time_pulse_us(echo_pin1, 1, 30000) # Renvoie le temps de propagation de l'onde (en µs)
    distance_cm1 = calculate_distance(ultrason_duration)
 

    # Prepare le signal
    trig_pin2.value(0)
    sleep_us(5)
    # Créer une impulsion de 10 µs
    trig_pin2.value(1)
    sleep_us(TRIG_PULSE_DURATION_US)
    trig_pin2.value(0)

    ultrason_duration = time_pulse_us(echo_pin2, 1, 30000) # Renvoie le temps de propagation de l'onde (en µs)
    distance_cm2 = calculate_distance(ultrason_duration)

    print("Distance sensor 1:", distance_cm1, "Distance sensor 2:", distance_cm2)

    # calculate the object location
    angle = calculate_angle(distance_cm1, distance_cm2)
    servo_left_right.value = angle
    
    print("angle is ", angle)
    # when sound sensor detecs sound
    if (False):
        servo_up_down.min()
        sleep_ms(100)
        servo_up_down.max()
        sleep_ms(100)
        servo_up_down.mid()
    
    sleep_ms(500)