from motorbridge import Motor  # Usually obtained via Controller.add_*_motor()

class Motor:
    def __init__(self, ptr: int) -> None: ...
    def close(self) -> None: ...

motor = ctrl.add_damiao_motor(0x01, 0x11, "4340P")
try:
    # ... use motor
finally:
    motor.close()

motor.enable()
print("Motor enabled")

motor.disable()
print("Motor disabled")

try:
    motor.send_mit(0.5, 0.0, 30.0, 1.0, 0.0)
except CallError:
    motor.clear_error()
    motor.enable()

# Calibrate zero position
motor.set_zero_position()
print("Zero position set")

from motorbridge import Mode

motor.ensure_mode(Mode.MIT, timeout_ms=1000)
print("Switched to MIT mode")

# Position control with stiffness and damping
motor.send_mit(
    pos=1.0,      # Target: 1.0 radian
    vel=0.0,      # Zero target velocity
    kp=30.0,      # Position stiffness
    kd=1.0,       # Velocity damping
    tau=0.0       # No feedforward torque
)

# Move to position with speed limit
motor.send_pos_vel(pos=2.0, vlim=1.5)

motor.robstride_send_pos_vel_pp(pos=1.0, vel_max=2.0, acc_set=10.0)

motor.robstride_send_pos_vel_csp(pos=1.0, vlim=2.0)

# Spin at 2 rad/s
motor.send_vel(vel=2.0)

# Compliant position control
motor.send_force_pos(pos=1.0, vlim=1.0, ratio=0.3)

motor.request_feedback()
# Feedback will be available via get_state() after poll

state = motor.get_state()
if state:
    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")
else:
    print("No feedback available")

motor.set_can_timeout_ms(100)  # 100ms timeout

from motorbridge import RID_PMAX

# Set position mapping range
motor.write_register_f32(RID_PMAX, 12.0)

from motorbridge import RID_MST_ID

# Set feedback ID
motor.write_register_u32(RID_MST_ID, 0x12)

from motorbridge import RID_KP_ASR

kp = motor.get_register_f32(RID_KP_ASR, timeout_ms=500)
print(f"Speed loop Kp: {kp}")

from motorbridge import RID_CTRL_MODE

mode = motor.get_register_u32(RID_CTRL_MODE)
print(f"Current mode: {mode}")

# Modify parameters
motor.write_register_f32(RID_PMAX, 12.0)
motor.write_register_f32(RID_VMAX, 30.0)

# Save to flash
motor.store_parameters()
print("Parameters saved")

device_id, responder_id = motor.robstride_ping()
print(f"Device: {device_id}, Responder: {responder_id}")

device_id, responder_id = motor.robstride_ping_host_id(0xFD, timeout_ms=80)
print(f"Device: {device_id}, Responder: {responder_id}")

value = motor.robstride_get_param_f32_host_id(0x7019, 0xFD, timeout_ms=120)
print(value)

motor.robstride_set_device_id(42)

# Write float parameter
motor.robstride_write_param_f32(0x7019, 1.5)

# Read float parameter
value = motor.robstride_get_param_f32(0x7019, timeout_ms=500)
print(f"Parameter value: {value}")

import time
from motorbridge import Controller, Mode

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

    # Enable and set mode
    ctrl.enable_all()
    motor.ensure_mode(Mode.MIT, timeout_ms=1000)

    # Control loop
    for i in range(100):
        # Send command
        motor.send_mit(pos=0.5, vel=0.0, kp=30.0, kd=1.0, tau=0.0)

        # Request and read feedback
        motor.request_feedback()
        state = motor.get_state()

        if state:
            print(f"#{i:03d} pos={state.pos:+.3f} vel={state.vel:+.3f}")

        time.sleep(0.02)

    # Clean disable
    motor.disable()