import time
from _thread import start_new_thread
from machine import Pin, PWM


class Motor:
    def __init__(self, direction, speed, position):
        self.position = 0
        self.thread = None
        
        self.direction_pin = Pin(direction, mode=Pin.OUT, value=1)
        self.speed_pin = PWM(Pin(speed), freq=30_000, duty_u16=65535)
        self.position_pin = Pin(position, mode=Pin.IN, pull=Pin.PULL_UP)
        self.position_pin.irq(handler=self.edge, trigger=Pin.IRQ_FALLING)

    def edge(self, pin):
        self.position += 1 if self.direction_pin() else -1

    @property
    def speed(self):
        return (65535 - self.speed_pin.duty_u16()) / 65535 * 100
    
    @speed.setter
    def speed(self, speed):
        duty_u16 = 65535 - int(speed / 100 * 65535)
        self.speed_pin.duty_u16(duty_u16)


    @property
    def direction(self):
        return 1 if self.direction_pin.value() else -1

    @direction.setter
    def direction(self, value):
        """
        -1 := CCW
         1 := CW
        """
        if value == -1:
            self.direction_pin.value(0)
        elif value == 1:
            self.direction_pin.value(1)
    
    def stop(self):
        self.speed = 0
    
    def stop_ramp(self):
        self.thread = None
    
    def wait_ramp(self):
        while self.thread is not None:
            time.sleep_ms(1)

    def ramp(self, start, end, duration):
        if duration == 0:
            self.speed = end
            return
        
        if self.thread is None:
            self.thread = True
            start_new_thread(self._ramp, (start, end, duration))
        
    def _ramp(self, start, end, duration):
        steps = int(min(256, duration * 50))
        delay = int(duration / steps * 1000)
        increment = (end - start) / steps
        current_speed = start
        self.speed = int(current_speed)
        
        for _ in range(steps):
            if self.thread is None:
                return
            
            current_speed += increment
            self.speed = int(current_speed)
            time.sleep_ms(delay)
        
        self.thread = None

DURATION = 0
MAX_SPEED = 50
MAX_POSITION = 500

motor = Motor(2, 3, 4)
motor.ramp(0, MAX_SPEED, DURATION)
while motor.thread is not None:
    time.sleep_ms(1)

while True:
    while motor.position < MAX_POSITION:
        time.sleep_ms(1)
        
    print(motor.position)

    motor.ramp(MAX_SPEED, 0, DURATION)
    motor.wait_ramp()
    print(motor.position)
    
    motor.direction = -1
    motor.ramp(0, MAX_SPEED, DURATION)
    motor.wait_ramp()
    print(motor.position)

    while motor.position > 0:
        time.sleep_ms(1)

    print(motor.position)

    motor.ramp(MAX_SPEED, 0, DURATION)
    motor.wait_ramp()
    print(motor.position)
    motor.direction = 1

    motor.ramp(0, MAX_SPEED, DURATION)
    motor.wait_ramp()
    print(motor.position)