"""MicroPython on Pololu 3pi+ 2040. API-checked, not physically piloted.
Run explicitly with wheels raised first. A arms; C or bumper stops.
"""
from pololu_3pi_2040_robot import robot
import time

COMMAND = 300  # device units; deliberately small, not a universal safe speed
DURATION_MS = 500
motors = robot.Motors()
button_a, button_c = robot.ButtonA(), robot.ButtonC()
bumps = robot.BumpSensors()
encoders = robot.Encoders()
motors.off()
try:
    bumps.calibrate()  # keep both bumpers released during calibration
    print("Press A to arm one pulse; C cancels. Arming expires in 30 s.")
    start = time.ticks_ms()
    armed = False
    while time.ticks_diff(time.ticks_ms(), start) < 30000:
        if button_c.is_pressed():
            break
        if button_a.is_pressed():
            armed = True
            break
        time.sleep_ms(10)
    if armed:
        before = encoders.get_counts()
        start = time.ticks_ms()
        reason = "duration"
        while time.ticks_diff(time.ticks_ms(), start) < DURATION_MS:
            bumps.read()
            if button_c.is_pressed():
                reason = "button_c"
                break
            if bumps.left_is_pressed() or bumps.right_is_pressed():
                reason = "bump"
                break
            motors.set_speeds(COMMAND, COMMAND)
            time.sleep_ms(10)
        motors.off()
        after = encoders.get_counts()
        print("stop_reason", reason, "count_delta", after[0] - before[0], after[1] - before[1])
    else:
        print("not_armed")
finally:
    motors.off()