from time import sleep
import board 
import neopixel
import pwmio

# GPIO pin mappings
motor_left = board.D12
motor_right = board.D13
scissor_motor = board.D26

strip = neopixel.NeoPixel(board.D10, 30)
strip.fill((0, 255, 100))

pwm_left = pwmio.PWMOut(motor_left, frequency = 1000, duty_cycle=0)
pwm_right = pwmio.PWMOut(motor_right, frequency = 1000, duty_cycle=0)
pwm_scissor = pwmio.PWMOut(scissor_motor, frequency = 1000, duty_cycle=0)

def drive(angle, speed):
    """
    minimal drive train: forward, left, right
    no reverse functionality (limitation of chosen motor driver hardware)
    speed in range of PWM signal [0, 100] - may add a min/max speed 
    """
    if(angle >= 315 or angle <= 45):
        #forward 
        pwm_left.duty_cycle = int(speed * 65535/100)
        pwm_right.duty_cycle = int(speed * 65535/100)
        sleep(0.01)
    elif(45 < angle < 135):
        # turn right (spin right wheels 4 times slower - current hardware does not allow for reverse direction)
        pwm_left.duty_cycle = int(speed * 65535/100)
        pwm_right.duty_cycle = int((speed/4)* 65535/100)
        sleep(0.01)
    elif(225 < angle < 315):
        # turn left 
        pwm_left.duty_cycle = int((speed/4) * 65535/100)
        pwm_right.duty_cycle = int(speed * 65535/100)
        sleep(0.01)
    else:
        pwm_left.duty_cycle = 0
        pwm_right.duty_cycle = 0
        sleep(0.01)

def cut(speed): 
    pwm_scissor.duty_cycle = int(speed * 65535/100)
    sleep(0.01)