"""
Project:    Defender
Type:       spike, word-blocks, slot 0
Last saved: 2025-02-22T03:09:42.580Z
"""

from pybricks.hubs import PrimeHub
from pybricks.parameters import Direction, Port
from pybricks.pupdevices import Motor, UltrasonicSensor
import umath

def convert_ussensor_distance_back(value, unit):
    if unit == "cm": return value / 10
    elif unit == "inches": return value / 25.4
    elif unit == "%": return value * 100 / 2000
    else: return value
def float_safe(value, default=0):
    try: return float(value)
    except: return default
def convert_speed(pct):
    return float_safe(pct) * 10

hub = PrimeHub()
motor_a = Motor(Port.A)
motor_b = Motor(Port.B)
motor_c = Motor(Port.C)
motor_d = Motor(Port.D)
ultrasonicsensor_e = UltrasonicSensor(Port.E)
default_speeds = {motor_a: 500, motor_b: 500, motor_c: 500, motor_d: 500}

g_direction = None
g_speed = None
g_rotation = None
g_distance = None
g_values = []

# ------------------------------- GROUP: START ------------------------------- #
def stack1_whenprogramstarts_fn():
    global g_direction, g_rotation, g_distance, g_speed
    hub.imu.reset_heading(0)
    while True:
        g_direction = None
        g_rotation = hub.imu.heading()
        g_distance = convert_ussensor_distance_back(ultrasonicsensor_e.distance(), "cm")
        g_speed = 200 - float_safe(None)
        move(g_direction, g_speed, g_rotation)
# ------------------------------ GROUP: MYBLOCK ------------------------------ #
def move(direction: string, speed: string, rotation: string):
    default_speeds[motor_a] = convert_speed(umath.cos(float_safe(direction)) * float_safe(speed) + float_safe(rotation))
    default_speeds[motor_b] = convert_speed(umath.cos(float_safe(direction) + 90) * float_safe(speed) + float_safe(rotation))
    default_speeds[motor_c] = convert_speed(umath.cos(float_safe(direction) + 180) * float_safe(speed) + float_safe(rotation))
    default_speeds[motor_d] = convert_speed(umath.cos(float_safe(direction) + 270) * float_safe(speed) + float_safe(rotation))
    motor_a.run(default_speeds[motor_a])
    motor_b.run(default_speeds[motor_b])
    motor_c.run(default_speeds[motor_c])
    motor_d.run(default_speeds[motor_d])

stack1_whenprogramstarts_fn()