from pybricks.hubs import PrimeHub
from pybricks.pupdevices import Motor, ColorSensor, UltrasonicSensor, ForceSensor
from pybricks.parameters import Button, Color, Direction, Port, Side, Stop
from pybricks.robotics import DriveBase
from pybricks.tools import wait, StopWatch
import umath
hub = PrimeHub()

left_motor = Motor(Port.A, Direction.COUNTERCLOCKWISE)
right_motor = Motor(Port.B,Direction.CLOCKWISE) # Specify default direction
left_arm = Motor(Port.C, Direction.CLOCKWISE, [[12,36]],[[12,20,24]] ) # Specify default direction
right_arm = Motor(Port.D, Direction.CLOCKWISE,[[12,36],[12,20,24]]) #Added gear train list for gear ration
lazer_ranger = UltrasonicSensor(Port.E)
color_sensor = ColorSensor(Port.F)
# DriveBase configuration
WHEEL_DIAMETER = 68.8  # mm (adjust for your wheels)
AXLE_TRACK = 180  # mm (distance between wheels)
drive_base = DriveBase(left_motor, right_motor, WHEEL_DIAMETER, AXLE_TRACK)
drive_base.settings(600, 500, 300, 200)
drive_base.use_gyro(True)

transformations = {
    "d": [2, 3],
    "a": [0, 90]
}
current = {
    "x": 0,
    "y": 0,
    "a": 0
}
# Extra useless code
#for transformation in transformations:
#    current.x += transformation.d * umath.cos(umath.radians(transformation.a))
#    current.y += transformation.d * umath.sin(umath.radians(transformation.a))  
def addtoposition(a, r):
    current.x += r*cos(a)
    current.y += r*sin(a)
    current.a = a

drive_base.drive()