from machine import Pin, ADC
from button import Button
from time import sleep, time, sleep_us

DEAD_ZONE = 0.125
TIMEOUT = 10
GAMMA = 1
DELAY_MIN_US = 500
DELAY_MAX_US = 100000

class Motor:
    _LUT = [
        (1, 0, 0, 0),
        (1, 1, 0, 0),
        (0, 1, 0, 0),
        (0, 1, 1, 0),
        (0, 0, 1, 0),
        (0, 0, 1, 1),
        (0, 0, 0, 1),
        (1, 0, 0, 1),
    ]

    def __init__(self, pins, phase=0):
        self._pins = pins
        self._phase = phase

    def update_pins(self):
        pattern = self._LUT[self._phase]
        for pin, value in zip(self._pins, pattern):
            pin(value)

    def step(self, dir):
        dir = 1 if dir > 0 else -1 if dir < 0 else 0
        self._phase = (self._phase + dir) % len(self._LUT)
        self.update_pins()

    def inc(self):
        self.step(1)

    def dec(self):
        self.step(-1)

    @property
    def phase(self):
        return self._phase
    
    def off(self):
        for pin in self._pins:
            pin(0)


class Joystick:
    _OFFSET = 30000
    _RANGE = 2**16

    def __init__(self, adcs):
        self._adcs = adcs

    def pos(self):
        return [2 * adc.read_u16() / self._RANGE - 1 for adc in self._adcs]


# motor = Motor([Pin(i, Pin.OUT) for i in range (4)])
motors = [Motor([Pin(2 + i + 4 * j, Pin.OUT) for i in range(4)]) for j in range(3)]
motor_idx = 3
joystick = Joystick([ADC(i) for i in range(2)])
rgb_led = [Pin(20 - i, Pin.OUT) for i in range(3)]
for led in rgb_led:
    led(0)

button = Button(22, thresholds_ms=[1000])


def button_cb(pin_state, threshold):
    global motor_idx
    if pin_state == 0 and threshold > 0:
        motor_idx = (motor_idx + 1) % 4
        for i in range(len(rgb_led)):
            rgb_led[i](1 if i == motor_idx or motor_idx == 3 else 0)


button.callback(button_cb)

for i in range(len(rgb_led)):
    rgb_led[i](1 if i == motor_idx or motor_idx == 3 else 0)

for motor in motors:
    motor.update_pins()

now = time()

while True:
    joystick_pos = joystick.pos()[1]
    abs_pos = abs(joystick_pos)
    step_dir =1 if joystick_pos < 0 else -1
    dir = joystick_pos < 0

    if abs_pos > DEAD_ZONE:
        print(joystick_pos, abs_pos)
        now = time()
        if motor_idx != 3:
            motors[motor_idx].step(step_dir)
        else:
            for motor in motors:
                motor.step(step_dir)

        normalized = (abs_pos - DEAD_ZONE) / (1-DEAD_ZONE)
        delay_us = DELAY_MIN_US + (DELAY_MAX_US- DELAY_MIN_US) * (1-normalized**GAMMA)
        sleep_us(int(delay_us))
    else:
        sleep(0.01)

    if time() - now > TIMEOUT:
        for motor in motors:
            motor.off()

    # sleep(0.0025)