from pybricks.hubs import PrimeHub
from pybricks.parameters import Axis, Direction, Port, Stop
from pybricks.pupdevices import Motor
from pybricks.robotics import DriveBase
from pybricks.tools import wait

from assi import A
from robot_class import (
    DEFAULT_GYRO_SETTLE_MS,
    DEFAULT_STRAIGHT_ACCELERATION,
    DEFAULT_STRAIGHT_SPEED,
    DEFAULT_TURN_ACCELERATION,
    DEFAULT_TURN_RATE,
    LazyRobot,
    Robot,
)

# robot.py (estratto rilevante)


leorobot = LazyRobot(
    name="leohub",
    top_side=A.UP,
    front_side=A.BACKWARD,
    left_port=Port.B,
    right_port=Port.A,
    left_direction=Direction.COUNTERCLOCKWISE,
    right_direction=Direction.CLOCKWISE,
    wheel_diameter=56,
    axle_track=105,
    use_gyro=True,
    straight_speed=DEFAULT_STRAIGHT_SPEED,
    straight_acceleration=DEFAULT_STRAIGHT_ACCELERATION,
    turn_rate=DEFAULT_TURN_RATE,
    turn_acceleration=DEFAULT_TURN_ACCELERATION,
    settle_ms=DEFAULT_GYRO_SETTLE_MS,
    extra_R_port=Port.E,
    extra_R_direction=Direction.CLOCKWISE,
    extra_L_port=Port.D,
    extra_L_direction=Direction.CLOCKWISE,
)

cbrobot = LazyRobot(
    name="cbhub",
    top_side=A.UP,
    front_side=A.RIGHT,
    left_port=Port.B,
    right_port=Port.A,
    left_direction=Direction.COUNTERCLOCKWISE,
    right_direction=Direction.CLOCKWISE,
    wheel_diameter=49.5,
    axle_track=88,
    use_gyro=True,
    straight_speed=DEFAULT_STRAIGHT_SPEED,
    straight_acceleration=DEFAULT_STRAIGHT_ACCELERATION,
    turn_rate=DEFAULT_TURN_RATE,
    turn_acceleration=DEFAULT_TURN_ACCELERATION,
    settle_ms=DEFAULT_GYRO_SETTLE_MS,
    extra_R_port=Port.E,
    extra_R_direction=Direction.CLOCKWISE,
    extra_L_port=Port.D,
    extra_L_direction=Direction.CLOCKWISE,
)


giorobot = LazyRobot(
    name="favhub",
    top_side=A.UP,
    front_side=A.RIGHT,
    left_port=Port.F,
    right_port=Port.B,
    left_direction=Direction.COUNTERCLOCKWISE,
    right_direction=Direction.CLOCKWISE,
    wheel_diameter=43.2,
    axle_track=80,
    use_gyro=True,
    straight_speed=DEFAULT_STRAIGHT_SPEED,
    straight_acceleration=DEFAULT_STRAIGHT_ACCELERATION,
    turn_rate=DEFAULT_TURN_RATE,
    turn_acceleration=DEFAULT_TURN_ACCELERATION,
    settle_ms=DEFAULT_GYRO_SETTLE_MS,
    extra_R_port=Port.E,
    extra_R_direction=Direction.CLOCKWISE,
    extra_L_port=Port.D,
    extra_L_direction=Direction.CLOCKWISE,
)