from motorbridge import Mode

class Mode(IntEnum):
    MIT = 1
    POS_VEL = 2
    VEL = 3
    FORCE_POS = 4

motor.ensure_mode(Mode.MIT, 1000)
motor.send_mit(
    pos=1.0,    # Target position (rad)
    vel=0.0,    # Target velocity (rad/s)
    kp=30.0,    # Position stiffness (Nm/rad)
    kd=1.0,     # Velocity damping (Nm·s/rad)
    tau=0.0     # Feedforward torque (Nm)
)

motor.ensure_mode(Mode.POS_VEL, 1000)
motor.send_pos_vel(
    pos=2.0,    # Target position (rad)
    vlim=1.5    # Velocity limit (rad/s)
)

motor.ensure_mode(Mode.VEL, 1000)
motor.send_vel(vel=2.0)  # 2 rad/s

# Stop motor
motor.send_vel(vel=0.0)

motor.ensure_mode(Mode.FORCE_POS, 1000)
motor.send_force_pos(
    pos=1.0,    # Target position (rad)
    vlim=1.0,   # Velocity limit (rad/s)
    ratio=0.3   # Force ratio (0.0-1.0)
)

from motorbridge import MotorState

@dataclass(frozen=True)
class MotorState:
    can_id: int
    arbitration_id: int
    status_code: int
    pos: float
    vel: float
    torq: float
    t_mos: float
    t_rotor: float

from motorbridge import Controller, Mode

with Controller("can0") as ctrl:
    motor = ctrl.add_damiao_motor(0x01, 0x11, "4340P")
    ctrl.enable_all()

    # Request feedback
    motor.request_feedback()

    # Get state (may be None if no feedback yet)
    state = motor.get_state()

    if state is not None:
        # Access fields
        print(f"Position: {state.pos:.3f} rad")
        print(f"Velocity: {state.vel:.3f} rad/s")
        print(f"Torque: {state.torq:.3f} Nm")
        print(f"MOS Temp: {state.t_mos:.1f} °C")
        print(f"Rotor Temp: {state.t_rotor:.1f} °C")
        print(f"Status: 0x{state.status_code:02X}")

        # Check for temperature warning
        if state.t_mos > 80.0 or state.t_rotor > 80.0:
            print("WARNING: High temperature!")
    else:
        print("No feedback received")

motor.request_feedback()
state = motor.get_state()
if state is not None:
    # status_code is the canonical machine-readable status value
    print(state.status_code, state.pos, state.vel)

state = motor.get_state()
if state and state.status_code != 0:
    print(f"Motor error: status=0x{state.status_code:02X}")
    motor.clear_error()

import time

def get_state_with_retry(motor, max_attempts=10, delay=0.05):
    """Get motor state with retry on None."""
    for attempt in range(max_attempts):
        motor.request_feedback()
        state = motor.get_state()
        if state is not None:
            return state
        time.sleep(delay)
    return None

# Usage
state = get_state_with_retry(motor)
if state:
    print(f"Got state: pos={state.pos:.3f}")
else:
    print("Failed to get state after retries")

import time
from motorbridge import Controller, Mode, MotorState

def print_state(state: MotorState, prefix: str = ""):
    """Pretty print motor state."""
    print(f"{prefix}pos={state.pos:+7.3f} rad | "
          f"vel={state.vel:+6.3f} rad/s | "
          f"torq={state.torq:+5.2f} Nm | "
          f"t_mos={state.t_mos:5.1f}°C | "
          f"status=0x{state.status_code:02X}")

with Controller("can0") as ctrl:
    motor = ctrl.add_damiao_motor(0x01, 0x11, "4340P")

    # Enable motor
    ctrl.enable_all()
    motor.ensure_mode(Mode.POS_VEL, 1000)

    # Control loop with state monitoring
    target_pos = 0.0

    for i in range(100):
        # Sinusoidal position target
        target_pos = 1.0 * (1.0 - (i / 50.0) if i < 50 else (i - 50) / 50.0)

        # Send command
        motor.send_pos_vel(target_pos, vlim=2.0)

        # Read state
        motor.request_feedback()
        state = motor.get_state()

        if state:
            print_state(state, prefix=f"#{i:03d} ")

            # Safety check
            if state.t_mos > 85.0:
                print("WARNING: MOSFET overheating!")
            if state.status_code != 0:
                print(f"ERROR: status=0x{state.status_code:02X}")

        time.sleep(0.02)

    motor.disable()