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)

speed = 0.6

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

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

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

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

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

def dgleft():
    m1.throttle = 0
    m2.throttle =speed
    m3.throttle =speed
    m4.throttle = 0

def turnright():
    m1.throttle = speed
    m2.throttle = speed
    m3.throttle = speed
    m4.throttle = 0

def turnleft():
    m1.throttle = speed
    m2.throttle = speed
    m3.throttle = 0
    m4.throttle = speed

def rotright():
    m1.throttle = speed
    m2.throttle = - speed
    m3.throttle = 0
    m4.throttle = 0

def rotleft():
    m1.throttle = - speed
    m2.throttle = speed
    m3.throttle = 0
    m4.throttle = 0

while True:
    print("F=", f"{front.distance:.2f}", "R=", f"{right.distance:.2f}", "L=", f"{left.distance:.2f}")
    try:
        if front.distance <= 35: # 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 front.distance < 10:
            backward()
            time.sleep(0.7)
            if left.distance > right.distance: # Diagonally left
                rotleft()
                time.sleep(0.5)
            if right.distance > left.distance: # Diagonally right
                rotright()
                time.sleep(0.5)

        if left.distance < 50: # Strafe right
            turnright()
            time.sleep(0.4)
        if right.distance < 50: # Strafe left
            turnleft()
            time.sleep(0.4)

        if left.distance < 15: # Strafe right
            #turnright()
            dgright()
            time.sleep(0.5)
            rotright()
            time.sleep(0.4)
        if right.distance < 15: # Strafe left
            #turnleft()
            dgleft()
            time.sleep(0.5)
            rotleft()
            time.sleep(0.4)


        else:
            forward()

    except RuntimeError:
        pass