#!/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, (c)hute-vibrator")
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.half_step:
	doHalfStep = True
else:
	doHalfStep = False
	
if args.delay:
	delayTime = args.delay
else:
	delayTime = 0.01
	
if args.mode:
	if args.mode=="c":
		mode = "c"
	elif args.mode=="m":
		mode = "m"
	elif args.mode=="t":
		mode = "t"
	elif args.mode=="v":
		mode = "v"
	elif args.mode=="h":
		mode = "h"
	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)) 

GPIO.setmode(GPIO.BCM)

#
# solenoids
#
feed_solenoid = 17
switch_solenoid = 18

#
# chute vibration motor
#
vib_motor = 23

#
# buttons
#
red_button = 22
	
# 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]
tilt_phases = [[motor_phase_1,motor_phase_4],[motor_phase_2,motor_phase_3],[motor_phase_3,motor_phase_2],[motor_phase_4,motor_phase_1]]
mix_phases = [[motor_phase_1,motor_phase_1],[motor_phase_2,motor_phase_2],[motor_phase_3,motor_phase_3],[motor_phase_4,motor_phase_4]]
# 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)
GPIO.setup(vib_motor,GPIO.OUT)
GPIO.setup(feed_solenoid,GPIO.OUT)
GPIO.setup(switch_solenoid,GPIO.OUT)
GPIO.setup(red_button,GPIO.IN)
def drum_phases_steps(phasespat,num):
	for n in (range(1,num)):
		for phase in phasespat:
			try:
				GPIO.output(motor_v_control_pins,phase[0])
				GPIO.output(motor_h_control_pins,phase[1])
				time.sleep(delayTime)
			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))
				GPIO.output(motor_v_control_pins,GPIO.LOW)
				GPIO.output(motor_h_control_pins,GPIO.LOW)
				GPIO.output(motor_general_enable,GPIO.LOW)
				pass
def mix_steps(num):
	GPIO.output(motor_general_enable,GPIO.HIGH)
	drum_phases_steps(mix_phases,num)
	GPIO.output(motor_general_enable,GPIO.LOW)
def tilt_steps(num):
	GPIO.output(motor_general_enable,GPIO.HIGH)
	drum_phases_steps(tilt_phases,num)
	GPIO.output(motor_general_enable,GPIO.LOW)
def do_phases_steps(motorid,phasepat,num):
	for stepnum in range(1,num):
		for phase in phasepat:
			if (motorid=="V"):
				GPIO.output(motor_v_control_pins, phase)
			elif (motorid=="H"):
				GPIO.output(motor_h_control_pins, phase)
			else:
				print("unrecognized motor id {0}".format([motorid]))
				raise Exception("unrecognized motor id")
			time.sleep(delayTime)
def do_steps(motorid,num):
	do_phases_steps(motorid,motor_phases, num)
def do_reverse_steps(motorid,num):
	do_phases_steps(motorid,motor_reverse_phases, num)
	
def exerciseStepper(motorid):
	print("will exercise stepper {0}".format([motorid]))
	try:
		GPIO.output(motor_general_enable,GPIO.HIGH)
		for repnum in range(1,loops):
			do_steps(motorid,steps)
			time.sleep(1)
			do_reverse_steps(motorid,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")
		if (motorid=="V"): 
			GPIO.output(motor_v_control_pins,GPIO.LOW)
		elif (motorid=="H"):
			GPIO.output(motor_h_control_pins,GPIO.LOW)
		else:
			print("unrecognized motorid {0} for switching off at end of exerciseStepper".format([motorid]))
			raise Exception("unrecognized motor id")
		GPIO.output(motor_general_enable,GPIO.LOW)
		print("GPIO {0} motor controls reset".format([motorid]))

def exerciseVib():	
	try:
		GPIO.output(vib_motor,GPIO.HIGH)
		while (GPIO.input(red_button)!=GPIO.HIGH):
			time.sleep(0.1)
	except KeyboardInterrupt:
		print("keyboard interrupt")
		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(vib_motor,GPIO.LOW)
		print("vibration motor off")
	
try:
	if mode=="c":
		exerciseVib()
	if mode=="m":
		mix_steps(steps)
	elif mode=="t":
		tilt_steps(steps)
	elif mode=="h":
		exerciseStepper("H")
	elif mode=="v":
		exerciseStepper("V")
	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:
	print("execution terminated")

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