import time
import board
import adafruit_hcsr04
from adafruit_motorkit import MotorKit

# Set the motors
kit = MotorKit()
m1 = kit.motor1
m2 = kit.motor2
m3 = kit.motor3
m4 = kit.motor4

# Set the sensor pins
front = adafruit_hcsr04.HCSR04(trigger_pin=board.D12, echo_pin=board.D13)
right = adafruit_hcsr04.HCSR04(trigger_pin=board.D10, echo_pin=board.D11)
left = adafruit_hcsr04.HCSR04(trigger_pin=board.D6, echo_pin=board.D9)

def forward():
    m1.throttle = 0.5
    m2.throttle = 0.5
    m3.throttle = 0.5
    m4.throttle = 0.5

def backward():
    m1.throttle = -0.5
    m2.throttle = -0.5
    m3.throttle = -0.5
    m4.throttle = -0.5

def stleft():
    m1.throttle = -0.5
    m2.throttle = 0.5
    m3.throttle = 0.5
    m4.throttle = -0.5
    
def stright():
    m1.throttle = 0.5
    m2.throttle = -0.5
    m3.throttle = -0.5
    m4.throttle = 0.5

def dgright():
    m1.throttle =0.5
    m2.throttle = 0
    m3.throttle = 0
    m4.throttle =0.5

def dgleft():
    m1.throttle = 0
    m2.throttle =0.5
    m3.throttle =0.5
    m4.throttle = 0
    
def turnright():
    m1.throttle = 0.5
    m2.throttle = 0.5
    m3.throttle = 0.5
    m4.throttle = 0
    
def turnleft():
    m1.throttle = 0.5
    m2.throttle = 0.5
    m3.throttle = 0
    m4.throttle = 0.5

while True:
    print("F=", f"{front.distance:.2f}", "R=", f"{right.distance:.2f}", "L=", f"{left.distance:.2f}")
    try:
        if front.distance <= 20: # What to do when there is an obstacle ahead
            if left.distance > right.distance: # Diagonally left
                dgleft()
                time.sleep(0.8)
            if right.distance > left.distance: # Diagonally right
                dgright()
                time.sleep(0.8)
        if left.distance < 30: # Strafe right
            turnright()
            #stright()
            time.sleep(0.5)
        if right.distance < 30: # Strafe left
            turnleft()
            #stleft()
            time.sleep(0.5)
        else:
            forward()

    except RuntimeError:
        pass