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

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, ponteiro=0, chave1=0, chave2=0):
        self.m1 = motor1
        self.m2 = motor2
        self.ponteiro = PWM(Pin(ponteiro), freq = 50, duty = 40)
        self.posicao = [0,0]
        self.chave1 = Pin(chave1)
        self.chave2 = Pin(chave2)

    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)

    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=[0,0], ponteiro=()):
        if ponteiro: self.ponteiro.duty((bool(ponteiro)) * 1023)
        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]

    def curva(self, coordenadas_ini=[0,0], coordenadas_fim=[0,0], sentido=1):
       #passar para classe Todo 
        x_centro, y_centro = coordenadas[0], coordenadas[1]
        x_ini, y_ini = x_centro + coordenadas_ini[0], y_centro + coordenadas_ini[1]
        x_fim, y_fim = coordenadas_fim[0], coordenadas_fim[1]
        try:
            x1, y1 = x_ini - x_centro, y_ini - y_centro
            x2, y2 = x_fim - x_centro, y_fim - y_centro
            r1 = (x1**2 + y1**2) ** 1/2
            r2 = (x2**2 + y2**2) ** 1/2
            if int(r1 - r2): raise KeyboardInterrupt
        except KeyboardInterrupt as e:
       #

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)