import RPi.GPIO as gpio
import time
import math

# Clean up GPIOs in case they have been preactivated
gpio.cleanup()

# Use the BCM GPIO pin numbers rather than the physical pin numbers for
# portability.
gpio.setmode(gpio.BCM)

gpio.setwarnings(False)

# Set which pins we're using for the stepper control
# The order in the list is:
# [ blue, pink, yellow, orange ]
stepper_pins = [ 6, 13, 19, 26]

# Set the pins as outputs
for pin in stepper_pins:
    gpio.setup(pin,gpio.OUT)
    gpio.output(pin,False)

# Quick pause for things to finish
time.sleep(0.5)

# Basic variables
wait_time = 0.002

# Single step sequence
single_steps = 4
# Define single_seq as a list
single_seq = []
# Temporarily populate it to make sure it has the right length
single_seq = range(single_steps)
# Set the sequence
single_seq[0] = [1,0,0,0]
single_seq[1] = [0,1,0,0]
single_seq[2] = [0,0,1,0]
single_seq[3] = [0,0,0,1]

def setStep(stepper_pins_states):
    # setStep takes in a list of states (True/False, or 1/0) and sets the GPIO
    # pins to high/low, accordingly.

    for i in range(len(stepper_pins)):
        gpio.output(stepper_pins[i],stepper_pins_states[i])

def turnSteps(num):
    # turnSteps takes in the number of steps the stepper motor should take,
    # with the sign determining the direction. The gearing ratio results in
    # there being 2048 steps in a full circle.

    count = 0
    print('In turnSteps')
    while abs(count) < abs(num):
        print(single_seq[count%len(single_seq)])
        setStep(single_seq[count%len(single_seq)])
        time.sleep(wait_time)
        print(count)
        print(math.copysign(1,num))
        count += int(math.copysign(1,num))
        print(count)

def turnCircles(circ):
    # turnCircles is just a shortcut to define the turn by a fraction of a
    # circle.

    turnSteps(circ*2048)

def stepperPowerdown():
    # Sets all the pins to low so that the stepper motor coils aren't
    # unnecessarily being powered.

    for pin in stepper_pins:
        gpio.setmode(gpio.BCM)
        gpio.setup(pin,gpio.OUT)
        gpio.output(pin,False)

gpio.setup(21,gpio.IN)

# Main loop: Run the stepper motor
try:
    print('trying')
    while True:
        print(gpio.input(21))
        if not gpio.input(21):
            turnSteps(-1024)
        else:
            stepperPowerdown()

        time.sleep(0.1)
except:
    print('exception called!!')
    gpio.cleanup()
finally:
    print('Finally ...')
    gpio.cleanup()
    stepperPowerdown()