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

# define strafing
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

# define diagonal movement
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

# define turning/steering
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

#define rotating
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

# Main control loop
while True:
    print("F=", f"{front.distance:.2f}", "R=", f"{right.distance:.2f}", "L=", f"{left.distance:.2f}") 
    # The obstacle distance thresholds can be set however you want. I set them with the values I found to be optimal in folkrace
    try:
        if front.distance <= 35: 
            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: # Robot gets dangerously close to an obstacle or even hits it
            # The robot will move backwards and rotare to the direction with the most space
            backward()
            time.sleep(0.7)
            if left.distance > right.distance:
                rotleft()
                time.sleep(0.5)
            if right.distance > left.distance:
                rotright()
                time.sleep(0.5)

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

            # When the robot's side gets dangerously close it moves diagonally and the rotates a bit to escape
        if left.distance < 15:
            dgright()
            time.sleep(0.5)
            rotright()
            time.sleep(0.4)
        if right.distance < 15:
            dgleft()
            time.sleep(0.5)
            rotleft()
            time.sleep(0.4)


        else: # If no obstacle is seen, keep going forward
            forward()

    except RuntimeError:
        pass