from machine import Pin, I2C
import time
import pca9685
import servo
import motor

# ultrasonic setup
trigPin=Pin(12,Pin.OUT,0)
echoPin=Pin(15,Pin.IN,0)
soundVelocity=340
distance=0

# servos setup
freq = 50
sdaPIN=Pin(13)
sclPIN=Pin(14)
i2c=I2C(0,sda=sdaPIN, scl=sclPIN)
pwm = pca9685.PCA9685(address=0x5f, i2c = i2c)
pwm.freq(freq)
#pan servo
servo1 = servo.Servos(address=0x5f, i2c = i2c)
#tilt servo
servo2 = servo.Servos(address=0x5f, i2c = i2c)

# motors setup
motors = motor.DCMotors(address=0x5f, i2c = i2c, freq=freq)

# car setup
OBSTACLE_DISTANCE = 40
OBSTACLE_DISTANCE_LOW = 20
d = 0.1
# d = 1

# delay to unplug car
time.sleep(10)

def motor_move(motors, m1_speed=0, m2_speed=0, m3_speed=0, m4_speed=0):
    sc = 0.5
    motors.speed(index=0, value= int(sc * m1_speed))
    motors.speed(index=1, value= int(sc * m2_speed))
    motors.speed(index=2, value= int(sc * m3_speed))
    motors.speed(index=3, value= int(sc * m4_speed))

def getSonar():
    trigPin.value(1)
    time.sleep_us(10)
    trigPin.value(0)
    while not echoPin.value():
        pass
    pingStart=time.ticks_us()
    while echoPin.value():
        pass
    pingStop=time.ticks_us()
    pingTime=time.ticks_diff(pingStop,pingStart)
    distance=pingTime*soundVelocity//2//10000
    return int(distance)

def get_distance(carmode = 1):
    add_angle = 30
    if carmode==1:
        add_angle = 0
    else:
        add_angle = 30
        
    servo2.position(index=1, degrees=90)
    servo1.position(index=0, degrees=120+add_angle)
    time.sleep(d)
    distance1 = getSonar()   # Get the data on the left
    
    servo1.position(index=0, degrees=90)
    time.sleep(d)
    distance2 = getSonar()   # Get the data in the middle
    
    servo1.position(index=0, degrees=60-add_angle)
    time.sleep(d)
    distance3 = getSonar()   # Get the data in the right
    
    servo1.position(index=0, degrees=90)
    time.sleep(d)
    return distance1, distance2, distance3
    
# while True:
for n in range(50):
    servo1.position(index=0, degrees=90)
    servo2.position(index=1, degrees=90)
    distance1, distance2, distance3 = get_distance(carmode=1)
    print(distance1, distance2, distance3)
    # distance1 = left
    # distance2 = middle
    # distance3 = right
    if (distance2 > OBSTACLE_DISTANCE):    #There is no obstacle within 40cm of the front of the car
        if (distance1 >= distance3 and distance3 < OBSTACLE_DISTANCE_LOW):   #There is an obstacle on the right
            print(' There is obstacle on the right')
            motor_move(motors, -1000, -1000, 2000, 2000)
        elif (distance1 < distance3 and distance1 < OBSTACLE_DISTANCE_LOW):  #There is an obstacle on the left
            print(' There is obstacle on the left')
            motor_move(motors, 2000, 2000, -1000, -1000)
        else:
            print(' There on no obstacles around')
            motor_move(motors, 1000, 1000, 1000, 1000)
        time.sleep(.02)
    
    elif distance2 < OBSTACLE_DISTANCE:  #There is an obstacle ahead
         motor_move(motors)
         distance1, distance2, distance3 = get_distance(carmode=2)
         if distance2 < OBSTACLE_DISTANCE_LOW:   #The car got too close to the obstacle
             print('The car got too close to the obstacle')
             motor_move(motors, -1000, -1000, -1000, -1000)
         elif distance1 < distance3:     #There is also an obstacle on the left
             print('There is also an obstacle on the left')
             motor_move(motors, 2000, 2000, -2000, -2000)
         elif distance1 > distance3:     #There is also an obstacle on the right
             print('There is also an obstacle on the right')
             motor_move(motors, -2000, -2000, 2000, 2000)
         time.sleep(.2)

# stop the car         
motor_move(motors)