# APM mini car: stop motors on completion or interruption
import drive as _mini_safety_drive
_mini_resources = []
_mini_safety_drive.stop_rpm(0)
try:
    import drive as _mini_drive_api
    import time as _mini_time
    
    def _mini_number(value, low, high, label):
        value = float(value)
        if not low <= value <= high:
            raise ValueError(label + ' out of range')
        return value
    
    def _mini_motor(channel):
        if channel not in (1, 2):
            raise ValueError('Motor must be 1 or 2')
        return channel
    
    def _mini_power(channel, power, brake=False):
        channel = _mini_motor(channel)
        power = int(_mini_number(power, -100, 100, 'Power'))
        _mini_drive_api.stop_rpm(channel)
        # This firmware treats soft_break=True as braking even at nonzero power.
        _mini_drive_api.control_motor(channel, power, soft_break=bool(brake) and power == 0)
    
    def _mini_differential(power, steering):
        power = _mini_number(power, -100, 100, 'Power')
        steering = _mini_number(steering, -100, 100, 'Steering')
        _mini_drive_api.stop_rpm(0)
        _mini_drive_api.drive(power, steering)
    
    def _mini_rpm(left, right):
        left = _mini_number(left, -300, 300, 'Left RPM')
        right = _mini_number(right, -300, 300, 'Right RPM')
        _mini_drive_api.drive_rpm(left, right)
    
    def _mini_single_rpm(channel, rpm):
        _mini_drive_api.set_rpm(_mini_motor(channel), _mini_number(rpm, -300, 300, 'RPM'))
    
    def _mini_wait(milliseconds):
        _mini_time.sleep_ms(int(_mini_number(milliseconds, 0, 60000, 'Delay')))
    
    def _mini_resource(obj, method):
        _mini_resources.append((obj, method))
        return obj
    
    def _mini_remote_drive(mode, maximum, deadzone):
        import remote_controller as rc
        maximum = _mini_number(maximum, 0, 150, 'Maximum RPM')
        deadzone = _mini_number(deadzone, 0, 50, 'Joystick deadzone')
        rc.scan()
        if not rc.is_connected():
            _mini_drive_api.stop_rpm(0)
            return False
        def axis(key):
            value = rc.get_axis(key)
            return 0 if abs(value) < deadzone else value * maximum / 100
        if mode == 'tank':
            left, right = axis(rc.LEFT_Y), axis(rc.RIGHT_Y)
        else:
            throttle, steering = axis(rc.LEFT_Y), axis(rc.LEFT_X)
            left, right = throttle + steering, throttle - steering
        scale = max(1, max(abs(left), abs(right)) / max(1, maximum))
        _mini_rpm(left / scale, right / scale)
        return True
    
    
    _mini_rpm(30, 30)
    _mini_wait(2000)
    _mini_drive_api.stop_rpm(0)
finally:
    _mini_safety_drive.stop_rpm(0)
    for _mini_obj, _mini_method in _mini_resources:
        try:
            getattr(_mini_obj, _mini_method)()
        except Exception as _mini_error:
            print("Cleanup:", _mini_error)
    _mini_safety_drive.stop_rpm(0)