import RPi.GPIO as GPIO
import time
import signal, os

m_signum=0
def handler(signum, frame):
	global m_signum
	print('Signal handler called with signal', signum)
#	print('signal.SIGUSR1='+str(signal.SIGUSR1))
#	print('Signal handler called with signal', signal.strsignal(signum))
	m_signum=signum
	if m_signum==10:
		print('m_signum==10')


signal.signal(signal.SIGUSR1, handler)
signal.signal(signal.SIGUSR2, handler)

PIN = 18
PIN_GPIO=[0]*4
PIN_GPIO[0] = 24
PIN_GPIO[1] = 25
PIN_GPIO[2] = 5
PIN_GPIO[3] = 16
keys=[0]*4
LED_BUILTIN = 17

PWM1=[0]*2
PWM2=[0]*2
PWM1[0] = 6
PWM2[0] = 13
PWM1[1] = 20
PWM2[1] = 21
D=[0]*2
D[0] = 12
D[1] = 26
p=[0]*2

PWM = 75

GPIO.setmode(GPIO.BCM)
GPIO.setwarnings(False)
GPIO.setup(PIN,GPIO.IN,GPIO.PUD_UP)
GPIO.setup(LED_BUILTIN,GPIO.OUT)
for i in range(2):
	GPIO.setup(PIN_GPIO[i  ],GPIO.IN,GPIO.PUD_UP)
	GPIO.setup(PIN_GPIO[i+2],GPIO.IN,GPIO.PUD_UP)
	GPIO.setup(PWM1[i],GPIO.OUT)
	GPIO.setup(PWM2[i],GPIO.OUT)
	GPIO.setup(D[i],GPIO.OUT)
	p[i] = GPIO.PWM(D[i],2*1000)
	p[i].start(PWM)

def set_motor(no, p1, p2):
	p[no].ChangeDutyCycle(0)
	GPIO.output(PWM1[no],p1)
	GPIO.output(PWM2[no],p2)
	for i in range(PWM, 100+1):
		p[no].ChangeDutyCycle(i)
		time.sleep(0.01)

def stop_motor(no):
	for i in range(100, PWM-1, -1):
		p[no].ChangeDutyCycle(i)
		time.sleep(0.01)

def forward():
	GPIO.output(PWMA1,1)
	GPIO.output(PWMA2,0)
	GPIO.output(PWMB1,1)
	GPIO.output(PWMB2,0)

def stop():
#	set_motor(0,0,0)
	GPIO.output(PWM1[0],0)
	GPIO.output(PWM2[0],0)
#	set_motor(1,0,0)
	GPIO.output(PWM1[1],0)
	GPIO.output(PWM2[1],0)

def reverse():
	set_motor(0,1,0,1)

def left():
	set_motor(1,0,0,0)

def right():
	set_motor(0,0,0,1)

def getkey():
	if GPIO.input(PIN) == 0:
		count = 0
		while GPIO.input(PIN) == 0 and count < 200:  #9ms
			count += 1
			time.sleep(0.00006)

		count = 0
		while GPIO.input(PIN) == 1 and count < 80:  #4.5ms
			count += 1
			time.sleep(0.00006)

		idx = 0
		cnt = 0
		data = [0,0,0,0]
		for i in range(0,32):
			count = 0
			while GPIO.input(PIN) == 0 and count < 15:    #0.56ms
				count += 1
				time.sleep(0.00006)

			count = 0
			while GPIO.input(PIN) == 1 and count < 40:   #0: 0.56ms
				count += 1                               #1: 1.69ms
				time.sleep(0.00006)

			if count > 8:
				data[idx] |= 1<<cnt
			if cnt == 7:
				cnt = 0
				idx += 1
			else:
				cnt += 1
		if data[0]+data[1] == 0xFF and data[2]+data[3] == 0xFF:  #check
			return data[2]

def getkey_gpio(no):
	return GPIO.input(PIN_GPIO[no])


print('IRM Test Start ...')
stop()
try:
	while True:
		print('. ', end='', flush=True)
		modus=0
		for i in range(4):
			keys[i] = getkey_gpio(i)
			if keys[i]:
				print(str(i))
		if keys[1] and False:
			m_signum=0
#			set_motor(0,0,1)
			set_motor(1,1,0)
			time.sleep(1)
#			set_motor(0,1,0)
			time.sleep(12)
		if m_signum==10:
			m_signum=0
#                       set_motor(0,0,1)
#                        set_motor(1,1,0)
#                        time.sleep(1)
			set_motor(0,1,0)
			time.sleep(1)
		if (keys[2] and False) or m_signum==signal.SIGUSR2:
			m_signum=0
#			set_motor(0,1,0)
			set_motor(1,0,1)
			time.sleep(1)
			set_motor(0,0,1)
			time.sleep(12)
		if keys[1]:
			set_motor(1,1,0)
			while getkey_gpio(1):
				print('o ', end='', flush=True)
				time.sleep(.1)
			stop_motor(1)
		if keys[2]:
			set_motor(1,0,1)
			while getkey_gpio(2):
				print('o ', end='', flush=True)
				time.sleep(.1)
			stop_motor(1)
		if keys[3]:
			set_motor(modus,1,0)
			while getkey_gpio(3):
				print('o ', end='', flush=True)
				time.sleep(.1)
			stop_motor(modus)
		if keys[0]:
			set_motor(modus,0,1)
			while getkey_gpio(0):
				print('o ', end='', flush=True)
				time.sleep(.1)
			stop_motor(modus)

#			if key == "stretch":
#				set_motor(1,1,0)
#				time.sleep(1)
#				set_motor(0,1,0)
#				time.sleep(11)
#			if key == "shrink":
#				set_motor(1,0,1)
#				time.sleep(1)
#				set_motor(0,0,1)
#				time.sleep(11)

		GPIO.output(LED_BUILTIN, not GPIO.input(LED_BUILTIN))
		time.sleep(.1)
		stop()

except KeyboardInterrupt:
	print("KeyboardInterrupt")
	stop()
	GPIO.cleanup();