from machine import Pin, ADC, PWM, I2C
from time import sleep
from motor import Class_Motor

def mdc(a, b):
    try:
        while b:
            a, b = b, a % b
    except ZeroDivisionError:
        a = max(a, b)
    return abs(a)

class Conjunto():
    def __init__(self, motor1, motor2, ponteira=0):
        self.m1 = motor1
        self.m2 = motor2
        self.ponteiro = PWM(Pin(ponteira), freq = 50, duty = 40)
        self.posicao = [0,0] #{X:0, Y:0}

    def direção(self, d=(0,0)):
        m_left, m_right = d
        self.m1.dire.value(int((m_left+1)/2))
        self.m1.enable.value(not m_left)
        self.m2.dire.value(int((m_right+1)/2))
        self.m2.enable.value(not m_right)
        return (m_left*-1 + m_right*-1)/2, (m_left*-1 - m_right*-1)/2

    def movimento(self, a, b, c, d):
        self.direção((c,d))
        for i in range(abs(b/a)):
            self.m1.move()
            self.m2.move()
            sleep(0.001)

    def mover(self, x, y):
        a = mdc(x, y)
        for i in range(a):
            try:
                self.movimento(a, x, x/abs(x), x/abs(x))
            except ZeroDivisionError:
                self.movimento(a, x, 0, 0)
            try:    
                self.movimento(a, y, y/abs(y)*-1, y/abs(y))
            except ZeroDivisionError:
                self.movimento(a, y, 0, 0)

    def absoluto(self, coordenadas, ponteiro=0):
        self.ponteiro.duty(40+(75*ponteiro))
        coordenadas[1] *= -1
        x = coordenadas[0] - self.posicao[0]
        y = coordenadas[1] - self.posicao[1]
        self.mover(x, y)
        self.posicao[0] = coordenadas[0]
        self.posicao[1] = coordenadas[1]

if __name__ == '__main__':
    right_motor = Class_Motor(17, 16, m=[22, 21, 19], enable=23)
    left_motor = Class_Motor(33, 32, enable=13)
    cortadora = Conjunto(right_motor, left_motor, 2)