import RPi.GPIO as GPIO
import time

GPIO.setmode(GPIO.BCM)

steps_taken = 0

enable_pin = 18
coil_A_1_pin = 4
coil_A_2_pin = 17
coil_B_1_pin = 23
coil_B_2_pin = 24

GPIO.setup(enable_pin, GPIO.OUT)
GPIO.setup(coil_A_1_pin, GPIO.OUT)
GPIO.setup(coil_A_2_pin, GPIO.OUT)
GPIO.setup(coil_B_1_pin, GPIO.OUT)
GPIO.setup(coil_B_2_pin, GPIO.OUT)

GPIO.output(enable_pin, 1)

def forward(delay, steps):
    print("going forward")
    global steps_taken
    for i in range(0, steps):
        setStep(1, 0, 0, 0)
        time.sleep(delay)
        setStep(0, 0, 1, 0)
        time.sleep(delay)
        setStep(0, 1, 0, 0)
        time.sleep(delay)
        setStep(0, 0, 0, 1)
        time.sleep(delay)

    steps_taken += steps

def backward(delay, steps):
    global steps_taken
    for i in range(0, steps):
        setStep(0, 0, 0, 1)
        time.sleep(delay)
        setStep(0, 1, 0, 0)
        time.sleep(delay)
        setStep(0, 0, 1, 0)
        time.sleep(delay)
        setStep(1, 0, 0, 0)
        time.sleep(delay)

    steps_taken -= steps

def release():
    setStep(0, 0, 0, 0)

def get_steps_taken():
    global steps_taken
    return steps_taken

def reset_steps_taken():
    global steps_taken
    steps_taken = 0

# Step until 'steps_taken' == target
def step_to(delay, target):
    if( steps_taken > target ):
        backward(delay, steps_taken - target)
        
    if( steps_taken < target):
        forward(delay, target - steps_taken)

def setStep(w1, w2, w3, w4):
    GPIO.output(coil_A_1_pin, w1)
    GPIO.output(coil_A_2_pin, w2)
    GPIO.output(coil_B_1_pin, w3)
    GPIO.output(coil_B_2_pin, w4)

while True == False:
    delay = raw_input("Delay between steps (milliseconds)? ")
    steps = raw_input("How many steps forward? ")
    forward(int(delay) / 1000.0, int(steps))
    steps = raw_input("How many steps backwards? ")
    backward(int(delay) / 1000.0, int(steps))
    release()
