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

DEFAULT_STRAIGHT_SPEED: int = 100  # mm/s
DEFAULT_STRAIGHT_ACCELERATION: int = 100  # mm/s²
DEFAULT_TURN_RATE: int = 90  # deg/s
DEFAULT_TURN_ACCELERATION: int = 50  # deg/s²
DEFAULT_GYRO_SETTLE_MS: int = 300  # ms


class Robot:
    """Inizializza Hub, Motori e DriveBase con impostazioni coerenti.

    Parametri:
        name: Nome logico del robot (per logging/diagnostica).
        top_side: Faccia dell'hub che punta verso l'alto (Axis).
        front_side: Faccia dell'hub che definisce l'“avanti” del robot (Axis).
        left_port: Porta del motore sinistro (guardando verso l'“avanti”).
        right_port: Porta del motore destro.
        left_direction: Verso del motore sinistro (Direction).
        right_direction: Verso del motore destro (Direction).
        wheel_diameter: Diametro ruote in millimetri (mm).
        axle_track: Distanza tra i punti di contatto a terra delle due ruote (mm).
        use_gyro: Se True abilita il giroscopio per turn/straight.
        straight_speed: Velocità lineare predefinita (mm/s).
        straight_acceleration: Accelerazione lineare predefinita (mm/s²).
        turn_rate: Velocità di rotazione predefinita (deg/s).
        turn_acceleration: Accelerazione di rotazione predefinita (deg/s²).
        settle_ms: Millisecondi di attesa dopo l’abilitazione del gyro.

    Attributi:
        hub: Istanza di PrimeHub configurata con l’orientamento dato.
        left: Motore sinistro.
        right: Motore destro.
        db: DriveBase pronta all’uso (settings applicati).
    """

    name: str
    hub: PrimeHub
    left: Motor
    right: Motor
    db: DriveBase

    def __init__(
        self,
        name: str,
        top_side: Axis,
        front_side: Axis,
        left_port: Port,
        right_port: Port,
        left_direction: Direction,
        right_direction: Direction,
        wheel_diameter: float,
        axle_track: int,
        use_gyro: bool,
        straight_speed: int = DEFAULT_STRAIGHT_SPEED,
        straight_acceleration: int = DEFAULT_STRAIGHT_ACCELERATION,
        turn_rate: int = DEFAULT_TURN_RATE,
        turn_acceleration: int = DEFAULT_TURN_ACCELERATION,
        settle_ms: int = DEFAULT_GYRO_SETTLE_MS,
    ) -> None:
        self.name = name

        self.hub = PrimeHub(top_side=top_side, front_side=front_side)

        self.left = Motor(left_port, left_direction)
        self.right = Motor(right_port, right_direction)

        self.db = DriveBase(
            self.left,
            self.right,
            wheel_diameter=wheel_diameter,
            axle_track=axle_track,
        )

        self.db.settings(
            straight_speed,
            straight_acceleration,
            turn_rate,
            turn_acceleration,
        )

        if use_gyro:
            self.db.use_gyro(True)
            wait(settle_ms)
            self.db.reset(angle=0)
        else:
            self.db.reset()


LEOHUB: Robot = Robot(
    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,
)

CBHUB: Robot = Robot(
    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,
)