#!/usr/bin/python
# algorithm (c) Terry Sturtevant, May 10, 2017

import RPi.GPIO as GPIO
import time
import argparse
import sys

parser = argparse.ArgumentParser("stt")
parser.add_argument("-d","--delay", help="pulse delay time", type=float)
parser.add_argument("-2","--half_step", help="half step")
parser.add_argument("-m","--mode", help="rotation mode: t(ilt), m(ix), (v)-Stepper-Exercise")
parser.add_argument("-t","--type", help="motor type: b..28BYJ, h..14HS5405 with IBT-2 H-bridge driver, s..14HS5405 with Allegro driver")
parser.add_argument("-l","--loops",help="number of loops to perform, defaults to 10")
parser.add_argument("-s","--steps",help="number of steps in each loop")
args = parser.parse_args()

print("args: ")

print(args)

if args.type:
	if args.type=="b":
		type = "b"
	elif args.type=="h":
		type = "h"
	else:
		print('unsupported mode {0}'.format(args.type))
		sys.exit()
else:
	exit -1
	
if args.half_step:
	doHalfStep = True
else:
	doHalfStep = False
	
if args.delay:
	delayTime = args.delay
else:
	delayTime = 0.01
	
if args.mode:
	if args.mode=="m":
		mode = "m"
	elif args.mode=="t":
		mode = "t"
	elif args.mode=="v":
		mode = "v"
	else:
		mode = "m"
else:
	mode = "m"
	
if args.loops:
	loops = int(args.loops)
else:
	loops = 10
	
if args.steps:
	steps = int(args.steps)
else:
	steps = 50
	
print('type: {0}, mode: {1}, doHalfStep: {2}, delayTime: {3}, loops: {4}'.format(type,mode,doHalfStep,delayTime,loops)) 

#
#	mode B
#
def typeBsetup():
	stepper_V_pins	=	[5,6,13,19]
	stepper_H_pins	=	[12,16,20,21]
	stepper_pins = [stepper_V_pins, stepper_H_pins]
	GPIO.setup(stepper_V_pins,GPIO.OUT)
	GPIO.setup(stepper_H_pins,GPIO.OUT)
	stepper_phase_010 = [GPIO.HIGH, GPIO.LOW,  GPIO.LOW,  GPIO.LOW ]	# -...
	stepper_phase_015 = [GPIO.HIGH, GPIO.HIGH, GPIO.LOW,  GPIO.LOW ]	# --..
	stepper_phase_020 = [GPIO.LOW,  GPIO.HIGH, GPIO.LOW,  GPIO.LOW ]	# ._..
	stepper_phase_025 = [GPIO.LOW,  GPIO.HIGH, GPIO.HIGH, GPIO.LOW ]	# .--.
	stepper_phase_030 = [GPIO.LOW,  GPIO.LOW,  GPIO.HIGH, GPIO.LOW ]	# ..-.
	stepper_phase_035 = [GPIO.LOW,  GPIO.LOW,  GPIO.HIGH, GPIO.HIGH]	# ..--
	stepper_phase_040 = [GPIO.LOW,  GPIO.LOW,  GPIO.HIGH, GPIO.HIGH]	# ..--
	stepper_phase_045 = [GPIO.HIGH, GPIO.LOW,  GPIO.LOW,  GPIO.HIGH]	# -..-

	stepper_sequence_mix = []
	if doHalfStep:
		stepper_sequence_mix.append([stepper_phase_010,stepper_phase_045])
		stepper_sequence_mix.append([stepper_phase_015,stepper_phase_040])
		stepper_sequence_mix.append([stepper_phase_020,stepper_phase_035])
		stepper_sequence_mix.append([stepper_phase_025,stepper_phase_030])
		stepper_sequence_mix.append([stepper_phase_030,stepper_phase_025])
		stepper_sequence_mix.append([stepper_phase_035,stepper_phase_020])
		stepper_sequence_mix.append([stepper_phase_040,stepper_phase_015])
		stepper_sequence_mix.append([stepper_phase_045,stepper_phase_010])
	else:
		stepper_sequence_mix.append([stepper_phase_010,stepper_phase_040])
		stepper_sequence_mix.append([stepper_phase_020,stepper_phase_030])
		stepper_sequence_mix.append([stepper_phase_030,stepper_phase_020])
		stepper_sequence_mix.append([stepper_phase_040,stepper_phase_010])

	stepper_sequence_tilt = []
	if doHalfStep:
		stepper_sequence_tilt.append([stepper_phase_010,stepper_phase_010])
		stepper_sequence_tilt.append([stepper_phase_015,stepper_phase_015])
		stepper_sequence_tilt.append([stepper_phase_020,stepper_phase_020])
		stepper_sequence_tilt.append([stepper_phase_025,stepper_phase_025])
		stepper_sequence_tilt.append([stepper_phase_030,stepper_phase_030])
		stepper_sequence_tilt.append([stepper_phase_035,stepper_phase_035])
		stepper_sequence_tilt.append([stepper_phase_040,stepper_phase_040])
		stepper_sequence_tilt.append([stepper_phase_045,stepper_phase_045])
	else:
		stepper_sequence_tilt.append([stepper_phase_010,stepper_phase_010])
		stepper_sequence_tilt.append([stepper_phase_020,stepper_phase_020])
		stepper_sequence_tilt.append([stepper_phase_030,stepper_phase_030])
		stepper_sequence_tilt.append([stepper_phase_040,stepper_phase_040])
	def sequenceLoop(mod):
		if mod=="m":
			seq = stepper_sequence_mix
		elif mod=="t":
			seq = stepper_sequence_tilt
		else:
			seq = stepper_sequence_mix
		try:
			while True:
				for row in seq:
					GPIO.output(stepper_V_pins,row[0])
					GPIO.output(stepper_H_pins,row[1])
					time.sleep(delayTime)
		except KeyboardInterrupt:
			pass
	def typeBexerciseMix():
		sequenceLoop(mode)
	def typeBexerciseTilt():
		sequenceLoop(mode)

#
# type H
#
def typeHsetup():
	# each motor has:   controlled by 
	# coil A-C          module x-A
	# coil B-D          module x-B
	#
	#
	motor_v_pin_a_l_pwm = 19		# module V-A left  H-bridge half
	motor_v_pin_a_r_pwm = 13		# module V-A right H-bridge half
	motor_v_pin_b_l_pwm =  6		# module V-B left  H-bridge half
	motor_v_pin_b_r_pwm =  5		# module V-B left  H-bridge half
	
	motor_h_pin_a_l_pwm = 12		# module H-A left  H-bridge half
	motor_h_pin_a_r_pwm = 16		# module H-A right H-bridge half
	motor_h_pin_b_l_pwm = 20		# module H-B left  H-bridge half
	motor_h_pin_b_r_pwm = 21		# module H-B right H-bridge half
	
	motor_general_enable= 26
	motor_pins = [motor_v_pin_a_l_pwm, motor_v_pin_a_r_pwm, motor_v_pin_b_l_pwm, motor_v_pin_b_r_pwm, motor_h_pin_a_l_pwm, motor_h_pin_a_r_pwm, motor_h_pin_b_l_pwm, motor_h_pin_b_r_pwm, motor_general_enable]
	motor_v_control_pins = [motor_v_pin_a_l_pwm, motor_v_pin_a_r_pwm, motor_v_pin_b_l_pwm, motor_v_pin_b_r_pwm]
	motor_h_control_pins = [motor_h_pin_a_l_pwm, motor_h_pin_a_r_pwm, motor_h_pin_b_l_pwm, motor_h_pin_b_r_pwm]
	# motor wants:    this means:
	# STEP A B C D    A-L-PWM A-R-PWM B-L-PWM B-R-PWM
	# 1    + + - -       HIGH     LOW    HIGH     LOW
	# 2    - + + -        LOW    HIGH    HIGH     LOW
	# 3    - - + +        LOW    HIGH     LOW    HIGH
	# 4    + - - +       HIGH     LOW     LOW    HIGH
	motor_phase_1 = [GPIO.HIGH, GPIO.LOW,  GPIO.HIGH, GPIO.LOW ]
	motor_phase_15= [GPIO.LOW,  GPIO.LOW,  GPIO.LOW,  GPIO.LOW ]
	motor_phase_2 = [GPIO.LOW,  GPIO.HIGH, GPIO.HIGH, GPIO.LOW ]
	motor_phase_25= [GPIO.LOW,  GPIO.LOW,  GPIO.LOW,  GPIO.LOW ]
	motor_phase_3 = [GPIO.LOW,  GPIO.HIGH, GPIO.LOW,  GPIO.HIGH]
	motor_phase_35= [GPIO.LOW,  GPIO.LOW,  GPIO.LOW,  GPIO.LOW ]
	motor_phase_4 = [GPIO.HIGH, GPIO.LOW,  GPIO.LOW,  GPIO.HIGH]
	motor_phase_45= [GPIO.LOW,  GPIO.LOW,  GPIO.LOW,  GPIO.LOW ]
	motor_phases = [motor_phase_1, motor_phase_2, motor_phase_3, motor_phase_4]
	motor_reverse_phases = [motor_phase_4, motor_phase_3, motor_phase_2, motor_phase_1]
	# motor_halfstep_phases = [motor_phase_1, motor_phase_15, motor_phase_2, motor_phase_25, motor_phase_3, motor_phase_35, motor_phase_4, motor_phase_45]
	GPIO.setup(motor_pins,GPIO.OUT)
	GPIO.output(motor_general_enable, GPIO.HIGH)
	def do_phases_steps(phasepat,num):
		for stepnum in range(1,num):
			for phase in phasepat:
				GPIO.output(motor_v_control_pins, phase)
				time.sleep(delayTime)
	def do_steps(num):
		do_phases_steps(motor_phases, num)
	def do_reverse_steps(num):
		do_phases_steps(motor_reverse_phases, num)
		
def exerciseStepperV():
	try:
		GPIO.output(motor_general_enable,GPIO.HIGH)
		for repnum in range(1,loops):
			do_steps(steps)
			time.sleep(1)
			do_reverse_steps(steps)
			time.sleep(1)
	except KeyboardInterrupt:
		print("caught KeyboardInterrupt")
		pass
	except Exception as ue:
		print("caught unknown exception:")
		print(ue)
		# print('An exception of type {0} occurred. Arguments:\n{1!r}'.format(type(ue).__name__, ue.args))
		pass
	finally:
		GPIO.output(motor_general_enable, GPIO.LOW)
		print("motor disabled")
		GPIO.output(motor_v_control_pins,GPIO.LOW)
		GPIO.output(motor_general_enable,GPIO.LOW)
		print("GPIO V motor controls reset")
		


def setup():
	GPIO.setmode(GPIO.BCM)
	if type=="b":
		typeBsetup()
	elif type=="h":
		typeHsetup()
	else:
		print("unrecognized type {0}".format([type]));
	
def execute():
	try:
		if mode=="m":
			exerciseMix()
		elif mode=="t":
			exerciseTilt()
		elif mode=="v":
			exerciseStepperV()
		else:
			print("unrecognized mode {0} for type {1}".format([mode,type]));
	except Exception as ue:
		print("caught unknown exception:")
		print(ue)
		# print('An exception of type {0} occurred. Arguments:\n{1!r}'.format(type(ue).__name__, ue.args))
		pass
	finally:
		GPIO.cleanup()
		print("GPIO cleaned up in execution")

setup()
execute()	
GPIO.cleanup()
print("GPIO cleaned up at end")