from machine import Pin
from time import sleep

step_pin = Pin(19, Pin.OUT)
dir_pin = Pin(18, Pin.OUT)

ms_pin_numbers = [2,0,4]
ms_pins = [Pin(x,Pin.OUT) for x in ms_pin_numbers]


micro_steps =  [[0,0,0], # 0
                [1,0,0], # 1
                [0,1,0], # 2
                [1,1,0], # 3
                [1,1,1]] # 4

micro_step_angle = [1.8, 1.8/2, 1.8/4, 1.8/8, 1.8/16]
angle_increment = 1.8
step_angle = 1.8


def choose_micro_stepping(selection):
    global angle_increment
    for i,level in enumerate(micro_steps[selection]):
        ms_pins[1].value(level)
        angle_increment = -1 * micro_step_angle[selection]



def step():
    step_pin.on()
    sleep(0.05)
    step_pin.off()
    sleep(0.05)


def change_dir():
    global angle_increment
    dir_pin.value(0 if dir_pin.value() == 1 else 1)
    angle_increment = -1 * angle_increment

#
def move_steps(num_steps):
    for i in range(num_steps):
        step()

#
def move_to_angle(new_position):
    angle_to_move = abs(motor_position - new_position)
    step_to_move = angle_to_move / step_angle
    if step_to_move < 0:
        change_dir()
        move_steps(abs(step_to_move))
        change_dir()
    else:
        move_steps(abs(step_to_move))


motor_position = 0
print("Hello Stepper")
choose_micro_stepping(3)
change_dir()

move_to_angle(10)
sleep(1)
move_steps(50)

while True:
    pass

    # for i in range(100):
    #     step()
    #     sleep(0.05)
    #     motor_position += angle_increment
    #     print("Current Angle", round(motor_position, 2))
    # change_dir()