from machine import Pin, PWM
from time import sleep
from motor import MotorController
from neopixel import NeoPixel

print("Clank controller coming online")

onboard_led = Pin(2, Pin.OUT)
neo_pixel_pin = Pin(23, Pin.OUT)

np = NeoPixel(neo_pixel_pin, 1)

# Motor A
pin_12 = Pin(12, Pin.OUT)
pin_14 = Pin(14, Pin.OUT)
pwm_13 = PWM(Pin(13), freq=1000, duty=0)

# Motor B
pin_25 = Pin(25, Pin.OUT)
pin_26 = Pin(26, Pin.OUT)
pwm_27 = PWM(Pin(27), freq=1000, duty=0)

front_motors = MotorController(pin_12, pin_14, pwm_13, False, pin_26, pin_25, pwm_27)


def blink_three_times(pin):
    for i in range(0, 3):
        pin.on()
        sleep(.5)
        pin.off()
        sleep(.5)


while True:
    blink_three_times(onboard_led)
    np[0] = (255, 255, 255)
    np.write()
    # print("Setting the motors up!")
    # onboard_led.on()
    # front_motors.motor_a.forward = True
    # front_motors.motor_b.forward = True
    # front_motors.motor_a.speed = 50
    # front_motors.motor_b.speed = 50
    # sleep(2)
    # onboard_led.off()
    # front_motors.motor_a.speed = 100
    # front_motors.motor_b.speed = 100
    # sleep(2)
    # onboard_led.on()
    # front_motors.motor_a.speed = 0
    # front_motors.motor_b.speed = 0
    # front_motors.motor_a.forward = False
    # front_motors.motor_b.forward = False
    # front_motors.motor_a.speed = 50
    # front_motors.motor_b.speed = 50
    # sleep(2)
    # onboard_led.off()
    # front_motors.motor_a.speed = 100
    # front_motors.motor_b.speed = 100
    # sleep(2)
    # onboard_led.on()
    # front_motors.motor_a.speed = 0
    # front_motors.motor_b.speed = 0
    # front_motors.motor_a.forward = True
    # front_motors.motor_b.forward = True
    # onboard_led.off()
    # sleep(.3)