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

#
# ------------------------------------------------------------------------
#
#	PUCON_RK_MIX	stepper and solenoid test program
#
#	Version			0.7		-	A4988 stepper driver

import RPi.GPIO as GPIO
import pigpio

import time
import argparse
import sys
import math
import traceback



parser = argparse.ArgumentParser("stt")
parser.add_argument("-d","--delay", help="pulse delay time", type=float)
parser.add_argument("-m","--mode", help="rotation mode: t(ilt), m(ix), (v)-stepper-exercise, (h)-stepper-exercise, (c)hute-vibrator, (s)olenoid-exercise, play M(u)sic")
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")
parser.add_argument("-p","--pause",help="pause between loops in seconds, defaults to 0.5")
args = parser.parse_args()

# print("args: ")
# print(args)
		

#
# solenoids
#
feed_solenoid = 17
switch_solenoid = 18

#
# chute vibration motor
#
vib_motor = 23

#
# each motor has one A4988 and has
#
#	enable	
#	direction
#	step
#
motor_v_pin_ena		= 19
motor_v_pin_dir		= 13
motor_v_pin_step	=  6
motor_h_pin_ena		= 21
motor_h_pin_dir		= 20
motor_h_pin_step	= 16

delayTime = 0.05
steps = 100
loops = 1
pause = 0.5

CW  = GPIO.LOW
CCW = GPIO.HIGH

output_pins = [	feed_solenoid, switch_solenoid, vib_motor, 
				motor_v_pin_ena, motor_v_pin_dir, motor_v_pin_step, 
				motor_h_pin_ena, motor_h_pin_dir, motor_h_pin_step]
	
GPIO.setmode(GPIO.BCM)

GPIO.setup(output_pins,GPIO.OUT)
GPIO.output(motor_v_pin_ena, GPIO.HIGH)
GPIO.output(motor_h_pin_ena, GPIO.HIGH)
GPIO.output(switch_solenoid, GPIO.LOW)
GPIO.output(feed_solenoid, GPIO.LOW)
GPIO.output(vib_motor, GPIO.LOW)

# ------------------------------------------------------------------------------
#	FUNCTION:		crange
#	INPUT:			maxval	-	upper limit of range to be created
#	DESCRIPTION:	create a range object 1..maxval

def crange(maxval):
	return range(1,maxval+1)

# ------------------------------------------------------------------------------
#	FUNCTION:		both_steps
#	INPUT:			num		-	number of steps to be executed
#	DESCRIPTION:	perform <num> steps on both V and H stepper motors
#
def both_steps(num):
	for stp in crange(num):
		GPIO.output(motor_v_pin_step,GPIO.HIGH)
		GPIO.output(motor_h_pin_step,GPIO.HIGH)
		if stp>50 and stp%100==0:
			print("both_steps, step {0} of {1}".format(stp,num))
		time.sleep(delayTime)
		GPIO.output(motor_v_pin_step,GPIO.LOW)
		GPIO.output(motor_h_pin_step,GPIO.LOW)
		time.sleep(delayTime)

# ------------------------------------------------------------------------------
#	FUNCTION:		mix_steps
#	INPUT:			num		-	number of steps to be executed
#					rev		-	if true, steps are performed in reverse direction
#	DESCRIPTION:	perform <num> steps on both V and V motors in MIXing mode:
#					both motors rotate in same direction ,thereby rotating the mix drum
#
def mix_steps(num,rev=False):
	if rev:
		GPIO.output(motor_h_pin_dir,CW)
		GPIO.output(motor_v_pin_dir,CCW)
	else:
		GPIO.output(motor_h_pin_dir,CCW)
		GPIO.output(motor_v_pin_dir,CW)
	GPIO.output(motor_v_pin_ena,GPIO.LOW)
	GPIO.output(motor_h_pin_ena,GPIO.LOW)
	both_steps(num)
	GPIO.output(motor_h_pin_ena,GPIO.HIGH)
	GPIO.output(motor_v_pin_ena,GPIO.HIGH)
	
def tilt_steps(num,rev=False,stay=False):
	if rev:
		GPIO.output(motor_h_pin_dir,CCW)
		GPIO.output(motor_v_pin_dir,CCW)
	else:
		GPIO.output(motor_h_pin_dir,CW)
		GPIO.output(motor_v_pin_dir,CW)
	GPIO.output(motor_v_pin_ena,GPIO.LOW)
	GPIO.output(motor_h_pin_ena,GPIO.LOW)
	both_steps(num)
	if not stay:
		GPIO.output(motor_h_pin_ena,GPIO.HIGH)
		GPIO.output(motor_v_pin_ena,GPIO.HIGH)

# ------------------------------------------------------------------------------
#	FUNCTION:		mix_exercise
#	INPUT:			stps		-	number of steps to be exercised
#					reps		-	number of repetitions
#	DESCRIPTION:	performs <reps> repetitions of:
#					1) <num> mix steps in forward direction
#					2) <<pause>> delay
#					3) <num> mix steps in reverse direction
#					4) <<pause>> delay
#
def mix_exercise(num,reps):
	for cloop in crange(reps):
		print("mix exercise loop {0} of {1} - forward".format(cloop,reps))
		mix_steps(num,False)
		time.sleep(pause)
		print("mix exercise loop {0} of {1} - reverse".format(cloop,reps))
		mix_steps(num,True)
		time.sleep(pause)
		
# ------------------------------------------------------------------------------
#	FUNCTION:		tilt_exercise
#	INPUT:			stps		-	number of steps to be exercised
#					reps		-	number of repetitions
#	DESCRIPTION:	performs <reps> repetitions of:
#					1) <num> tilt steps in forward direction
#					2) <<pause>> delay
#					3) <num> tilt steps in reverse direction
#					4) <<pause>> delay
#
def tilt_exercise(num,reps):
	for cloop in crange(reps):
		print("tilt exercise loop {0} of {1} - forward".format(cloop,reps))
		tilt_steps(num,False,True)
		time.sleep(pause)
		print("tilt exercise loop {0} of {1} - reverse".format(cloop,reps))
		tilt_steps(num,True,False)
		time.sleep(pause)
	
def pulse_steps(pulses,pin,tim=delayTime):
	for pulsenum in crange(pulses):
		GPIO.output(pin,GPIO.HIGH)
		time.sleep(tim)
		GPIO.output(pin,GPIO.LOW)
		time.sleep(tim)
		if (pulsenum>50) and ((pulsenum%100)==0):
			print(" step {0} of {1}".format(pulsenum,pulses))

# ------------------------------------------------------------------------------
#	FUNCTION:		exerciseStepper
#	INPUT:			mot		-	select motor to exercise: "V" .. front, "H" .. back
#	DESCRIPTION:	exercise the motor by performing <loop> repetitions of:
#					1) perform <<steps>> steps in CW direction
#					2) pause for <<pause>> seconds
#					3) perform <<steps>> steps in CCW direction
#					4) pause for <<pause>> seconds
#
def exerciseStepper(mot):
	if mot=="V":
		ena = motor_v_pin_ena
		dir = motor_v_pin_dir
		stp = motor_v_pin_step
	elif mot=="H":
		ena = motor_h_pin_ena
		dir = motor_h_pin_dir
		stp = motor_h_pin_step
	GPIO.output(ena,GPIO.LOW)
	GPIO.output(dir,GPIO.HIGH)
	GPIO.output(stp,GPIO.LOW)
	for curloop in crange(loops):
		print("exerciseStepper - stepper {0} - loop {1} of {2} - CW".format(mot,curloop,loops))
		GPIO.output(dir,CW)
		pulse_steps(steps,stp)
		time.sleep(pause)
		print("exerciseStepper - stepper {0} - loop {1} of {2} - CCW".format(mot,curloop,loops))
		GPIO.output(dir,CCW)
		pulse_steps(steps,stp)
		time.sleep(pause)
	GPIO.output(ena,GPIO.HIGH)
	GPIO.output(dir,GPIO.HIGH)
	GPIO.output(stp,GPIO.LOW)

# ------------------------------------------------------------------------------
#	musical note generation structures
#
#	<<scale>> has delay time values for tempered scale notes, starting from a1
# 
scale = []
cnote = 220
for note in range(1,130):
	ndel = 1/cnote
	scale.append([cnote,ndel])
	cnote *= 2**(1/12)

plain_scale = [0,2,4,5,7,9,11,12]

a1  = 0
as1 = 1
h1  = 2
c1  = 3
cs1 = 4
d1  = 5
ds1 = 6
e1  = 7
f1  = 8
fs1 = 9
g1  = 10
gs1 = 11
a2  = 12
as2 = 13
h2  = 14
c2  = 15
cs2 = 16
d2  = 17
ds2 = 18
e2  = 19
f2  = 20
fs2 = 21
g2  = 22
gs2 = 23
a3  = 24

ducks = [c1,d1,e1,f1,g1,g1,a2,a2,a2,a2,g1,a2,a2,a2,a2,g1,f1,f1,f1,f1,e1,e1,d1,d1,d1,d1,c1]

# ------------------------------------------------------------------------------
#	FUNCTION:		playMusic
#	INPUT:			mot		-	motor to use for playing notes: "V" or "H"
#	DESCRIPTION: 	plays frequences from plain scale a1..a2
#
def playMusic(mot):
	if mot=="V":
		ena = motor_v_pin_ena
		dir = motor_v_pin_dir
		stp = motor_v_pin_step
	elif mot=="H":
		ena = motor_h_pin_ena
		dir = motor_h_pin_dir
		stp = motor_h_pin_step
	GPIO.output(ena,GPIO.LOW)
	GPIO.output(dir,GPIO.HIGH)
	GPIO.output(stp,GPIO.LOW)
	for curtone in plain_scale:
		curnote = scale[curtone]
		print("note {0} Hz".format(curnote[0]))
		GPIO.output(dir,CW)
		pulse_steps(int(pause/curnote[1]),stp,curnote[1])
		time.sleep(delayTime)
	GPIO.output(ena,GPIO.HIGH)
	GPIO.output(dir,GPIO.HIGH)
	GPIO.output(stp,GPIO.LOW)

def exerciseSolenoids():
	try:
		for loop in crange(loops):
			GPIO.output(switch_solenoid,GPIO.LOW)
			GPIO.output(feed_solenoid,GPIO.HIGH)
			time.sleep(0.5)
			GPIO.output(feed_solenoid,GPIO.LOW)
			time.sleep(0.5)
			GPIO.output(switch_solenoid,GPIO.HIGH)
			time.sleep(0.5)
			GPIO.output(feed_solenoid,GPIO.HIGH)
			time.sleep(0.5)
			GPIO.output(feed_solenoid,GPIO.LOW)
			GPIO.output(switch_solenoid,GPIO.LOW)
	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))
	finally:
		GPIO.output(switch_solenoid,GPIO.LOW)
		GPIO.output(feed_solenoid,GPIO.LOW)
		print("solenoid exercise ended")

def exerciseVib():	
	GPIO.output(vib_motor,GPIO.HIGH)
	while True:
		time.sleep(0.1)
	GPIO.output(vib_motor,GPIO.LOW)
	print("vibration motor off")
	
def exerciseFeed(stps,reps):
	try:
		GPIO.output(vib_motor,GPIO.HIGH)
		for loop in crange(reps):
			GPIO.output(feed_solenoid,GPIO.HIGH)
			time.sleep(pause)
			GPIO.output(feed_solenoid,GPIO.LOW)
			time.sleep(pause)
	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))
	finally:
		GPIO.output(vib_motor,GPIO.LOW)
		GPIO.output(switch_solenoid,GPIO.LOW)
		GPIO.output(feed_solenoid,GPIO.LOW)
		print("solenoid exercise ended")

def exerciseMotorH(s,l):
	exerciseStepper("H")

def exerciseMotorV(s,l):
	exerciseStepper("V")

def playMusicH(s,l):
	playMusic("H")
	
def playMusicV(s,l):
	playMusic("V")

modes = {	"c": {"modechar": "c", "modefunc": exerciseVib,		"defaults": {"d": 0.001, "s": 100, }}, 
			"f": {"modechar": "f", "modefunc": exerciseFeed},
			"h": {"modechar": "h", "modefunc": exerciseMotorH},
			"m": {"modechar": "m", "modefunc": mix_steps,		"defaults": {"d": 0.01, "s": 100, "p": 0.5, "l": 1}},
			"t": {"modechar": "t", "modefunc": tilt_steps,		"defaults": {"d": 0.01, "s": 100, "p": 0.5, "l": 1}},
			"u": {"modechar": "u", "modefunc": playMusicV},
			"v": {"modechar": "v", "modefunc": exerciseMotorV},
			"w": {"modechar": "w", "modefunc": playMusicH} }

if args.mode:
	moderec = modes[args.mode]
	if moderec:
		mode = moderec["modechar"]
		if "defaults" in moderec:
			defs = moderec["defaults"]
			if not args.delay:
				if "d" in defs:
					args.delay = defs["d"]
			if not args.loops:
				if "l" in defs:
					args.loops = defs["l"]
			if not args.pause:
				if "p" in defs:
					args.pause = defs["p"]
			if not args.steps:
				if "s" in defs:
					args.steps = defs["s"]
	else:
		mode = "y"
else:
	mode ="x"
	

if args.delay:
	delayTime = args.delay
else:
	delayTime = 0.01
	
if args.loops:
	loops = int(args.loops)
else:
	loops = 10

if args.pause:
	pause = float(args.pause)
else:
	pause = 0.5
	
if args.steps:
	steps = int(args.steps)
else:
	steps = 50
	
print('mode: {0}, steps: {1}, delayTime: {2}, loops: {3}, pause: {4}'.format(mode,steps,delayTime,loops,pause)) 

try:
	cmode = modes[mode]
	if cmode:
		cmode["modefunc"](steps,loops)
	else:
		print("unrecognized mode {0}".format(mode));
except KeyboardInterrupt:
	print("caught KeyboardInterrupt")
except Exception as ue:
	print("caught unknown exception:")
	print(traceback.format_exc())
	print(ue)
	print('An exception {2} of type {0} occurred. Arguments:\n{1!r}'.format(type(ue).__name__, ue.args, ue))
	pass
finally:
	GPIO.output(motor_h_pin_ena,GPIO.HIGH)
	GPIO.output(motor_h_pin_ena,GPIO.HIGH)
	GPIO.output(motor_v_pin_step,GPIO.LOW)
	GPIO.output(motor_h_pin_step,GPIO.LOW)
	GPIO.output(vib_motor,GPIO.LOW)
	GPIO.output(feed_solenoid,GPIO.LOW)
	GPIO.output(switch_solenoid,GPIO.LOW)
	print("execution terminated")

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