from time import sleep
import robot

START_VALUES = (50, 50)
THRESHOLD = 1.01

def test_forward(arlo, l_power, r_power):
    arlo.reset_encoder_counts()
    arlo.go_diff(l_power, r_power, 1, 1)
    sleep(1)
    arlo.stop()
    return (
        int(arlo.read_left_wheel_encoder()),
        int(arlo.read_right_wheel_encoder())
    )

def test_back(arlo, l_power, r_power):
    arlo.reset_encoder_counts()
    arlo.go_diff(l_power, r_power, 0, 0)
    sleep(1)
    arlo.stop()
    return (
        int(arlo.read_left_wheel_encoder()),
        int(arlo.read_right_wheel_encoder())
    )


def test_clockwise(arlo, l_power, r_power):
    arlo.reset_encoder_counts()
    arlo.go_diff(l_power, r_power, 1, 0)
    sleep(1)
    arlo.stop()
    return (
        int(arlo.read_left_wheel_encoder()),
        int(arlo.read_right_wheel_encoder())
    )

def test_anticlockwise(arlo, l_power, r_power):
    arlo.reset_encoder_counts()
    arlo.go_diff(l_power, r_power, 0, 1)
    sleep(1)
    arlo.stop()
    return (
        int(arlo.read_left_wheel_encoder()),
        int(arlo.read_right_wheel_encoder())
    )

def main():
    values = [START_VALUES for _ in range(4)]
    calibrated = [False for _ in range(4)]
    tests = [
        test_forward,
        test_back,
        test_clockwise,
        test_anticlockwise
    ]

    arlo = robot.Robot()

    while not all(calibrated):
        for i, function in enumerate(tests):
            if not calibrated[i]:
                wheels = function(arlo, *values[i])
                fraction = max(wheels)/min(wheels)
                if fraction < THRESHOLD:
                    calibrated[i] = True
                elif wheels[0] < wheels[1]:
                    values[i][0] += 1
                else:
                    values[i][1] += 1

    print(values)



if __name__ == "__main__":
    main()