import RPi.GPIO as GPIO
import threading
import time
import queue

mq = queue.Queue(10)

button =22
red =17
green =27
blue =23
white =24
yellow =18
servo =19

cnt = 0
def buttonPressed(channel):
	global cnt
	cnt = cnt + 1
	if cnt == 4:
		cnt = 0
	mq.put(cnt)

GPIO.setmode(GPIO.BCM)

GPIO.setup(button, GPIO.IN)
GPIO.add_event_detect(button, GPIO.RISING)
GPIO.add_event_callback(button, buttonPressed)

GPIO.setup(red, GPIO.OUT)
GPIO.setup(green, GPIO.OUT)
GPIO.setup(blue, GPIO.OUT)
GPIO.setup(white, GPIO.OUT)
GPIO.setup(yellow, GPIO.OUT)
GPIO.setup(servo, GPIO.OUT)

GPIO.output(red, False)
GPIO.output(green, False)
GPIO.output(blue, False)
GPIO.output(white, False)

yellow_p = GPIO.PWM(yellow, 1000.0)
yellow_p.start(0.0)

servo_p = GPIO.PWM(servo_pin, 50)
servo_p.start(3.0)

flag = False
def t1():
	while True:
		GPIO.output(white, 1)
		time.sleep(0.6)	
		GPIO.output(white, 0)
		time.sleep(0.6)
		if flag: break

def t2():
	while True:
		for h in range(0, 1024):
			yellow_p.ChangeDutyCycle(h*100//1024)
			time.sleep(0.001)
		for h in range(1024, 0, -1):
			yellow_p.ChangeDutyCycle(h*100//1024)
			time.sleep(0.001)
		if flag: break

def t3():
	while True:
		Input = input()
		
		if Input == "q":
			servo_p.ChangeDutyCycle(3.0)
		elif Input == "w":
			servo_p.ChangeDutyCycle(7.75)
		elif Input == "e":
			servo_p.ChangeDutyCycle(12)
    servo_p.ChangeDutyCycle(0.0)
		time.sleep(1.0)
		if flag: break
		
T1 = threading.Thread(target=t1)
T1.start()
T2 = threading.Thread(target=t2)
T2.start()
T3 = threading.Thread(target=t3)
T3.start()

try:
	while True:
		Cnt = mq.get()
		
		if Cnt == 0:
			GPIO.output(red, True)
		elif Cnt == 1:
			GPIO.output(green, True)
		elif Cnt == 2:
			GPIO.output(blue, True)
		elif Cnt == 3:
			GPIO.output(red, False)
			GPIO.output(green, False)
			GPIO.output(blue, False)

except KeyboardInterrupt:
	pass

flag = True
T1.join()
T2.join()
T3.join()

servo_p.ChangeDutyCycle(3.0)
time.sleep(1.0)
servo_p.ChangeDutyCycle(0.0)

yellow_p.stop()
servo_p.stop()
GPIO.cleanup()