from time import sleep
import robot

START_VALUES = [60, 60]
THRESHOLD = 1.05
SLEEP_TIME = 2

def test_forward(arlo, l_power, r_power):
    arlo.reset_encoder_counts()
    arlo.go_diff(l_power, r_power, 1, 1)
    sleep(SLEEP_TIME)
    arlo.stop()
    return (
        abs(int(arlo.read_left_wheel_encoder())),
        abs(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(SLEEP_TIME)
    arlo.stop()
    return (
        abs(int(arlo.read_left_wheel_encoder())),
        abs(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(SLEEP_TIME)
    arlo.stop()
    return (
        abs(int(arlo.read_left_wheel_encoder())),
        abs(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(SLEEP_TIME)
    arlo.stop()
    return (
        abs(int(arlo.read_left_wheel_encoder())),
        abs(int(arlo.read_right_wheel_encoder()))
    )

def main():
    values = [START_VALUES.copy() for _ in range(4)]
    cps = [0 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):
        print(calibrated, values)
        for i, function in enumerate(tests):
            calibrated[i] = False
            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

            cps[i] = wheels[0]/SLEEP_TIME

    time = [0 for _ in range(4)]

    cpc = 144/(3.14159*15) # wheel counts per cm

    # milliseconds per 10cm forward
    time[0] = int((1000 * 10 * cpc)/cps[0])
    # milliseconds per 10cm backwards
    time[1] = int((1000 * 10 * cpc)/cps[1])

    cpr = 3.1415*38 # 1 rotation in cm
    # milliseconds per 10 degrees clockwise
    time[2] = int((1000 * cpr * cpc)/(cps[2] * 36))
    # milliseconds per 10 degrees anticlockwise
    time[3] = int((1000 * cpr * cpc)/(cps[3] * 36))

    values_hex = "-".join(
        [".".join([format(i, "x") for i in v]) for v in values]
    )
    time_hex = "-".join([format(i, "x") for i in time])
    print(f"{values_hex}_{time_hex}")



if __name__ == "__main__":
    main()