# The library file
import cv2
import numpy as np
from picamera.array import PiRGBArray
from picamera import PiCamera
import time
import redis
import os 
import RPi.GPIO as GPIO

button_press = 0 
button_pause = 0 

# Buttons for start, pause, and stop.
GPIO.setmode(GPIO.BCM)

GPIO.setup(17,GPIO.IN,pull_up_down=GPIO.PUD_UP)
GPIO.setup(22,GPIO.IN,pull_up_down=GPIO.PUD_UP)
GPIO.setup(23,GPIO.IN,pull_up_down=GPIO.PUD_UP)

def GPIO23_callback(channel):
	print "we are in here"
	quit()

def GPIO22_callback(channel):
	global button_pause
	print button_pause
	if  button_pause == 1:
		cm_move(cm_pwm)
		button_pause = 0 
	else:
		cm_stop(cm_pwm)
		button_pause = 1

def GPIO17_callback(channel):
	global button_press
	button_press = 1

GPIO.add_event_detect(17,GPIO.FALLING,callback=GPIO17_callback,bouncetime=300)
GPIO.add_event_detect(22,GPIO.FALLING,callback=GPIO22_callback,bouncetime=300)
GPIO.add_event_detect(23,GPIO.FALLING,callback=GPIO23_callback,bouncetime=300)

# Pin 26 as the convey_belt motor controller.
# Pin 4 as rotation of the sort plate.
# Color Reference map 
Color_map = {-2:'background',-1:'Golden',0:'Black',1:'Brown',2:'Red',3:'Orange',4:'Yellow',5:'Green',6:'Blue',7:'Violet',8:'Gray',9:'White'}
#update the reference value
Color_lower = {-2:[0,150,0],-1:[180,255,255],0:[0,0,0],1:[0,150,0],2:[0,200,45],3:[4, 100, 100],4:[20,200,100],5:[30,100,0],6:[80, 50, 50],7:[130, 40, 50],8:[0,0, 50],9:[0, 0, 90]}
Color_upper = {-2:[180,255,255],-1:[180,255,255],0:[3,5,50],1:[10,255,50],2:[8,255,100],3:[9, 250, 150],4:[30,255,150],5:[100,255,100],6:[106, 250, 150],7:[155, 250, 150],8:[180, 50, 80],9:[180, 15, 140]}

Color_lower_2 = {2:[140,235,50]}
Color_upper_2 = {2:[180,255,100]}

Thresh_hold = 120

# Map from color to degree
# 0 clock-wise is the direction we hope to map.
Plate_map_dutycycle = {0:7.2,1:6.9}
Plate_map_forward = {0:0, 1:0.34 , 2:0.82, 3:1.29, 4:0.82, 5:0.34} # map to duty_cycle 0	(7.2)
Plate_map_backward = {0:0, 1:0.53 , 2:1.1 ,3:1.72, 4:1.1, 5:0.53} # map to duty_cycle 1 (6.9)

def database_initial():
	# Initilize redis database
	db = redis.StrictRedis(host='127.0.0.1', port=6379, db=5)
	db.flushall()
	db.set('0', 0)
	return db

def camera_intilize():
	# Camera Configuration
	camera=PiCamera()
	camera.resolution=(640,480)
	camera.framerate=32
	#rawCapture = PiRGBArray(camera,size=(640,480))
	time.sleep(0.1)	# Wait time for camera to warm up
	return camera

def order_redis(db):
	# key order check the first valid value in redis.
	int_array = []
	for i in db.keys():
		int_array.append(int(i))
	int_array = sorted(int_array)
	#print "the value of the key is " + str(int_array) + 'the last value'  +str(int_array[-1])
	return int_array[-1]

def add_to_db(value,db):
	# This function is used to add the resistor value to redis database.
	key = int(order_redis(db)) + 1
	data = str(value) + "," + time.strftime('%X %x %Z')
	#print "try to add to database" + str(key)
	db.set(key,data)

def edge_detection(camera):
	# Detect the edge of the resistor
	rawCapture = PiRGBArray(camera,size=(640,480))
	for frame in camera.capture_continuous(rawCapture, format = "bgr", use_video_port = True):
		while (button_pause == 1):
			pass
		image = frame.array
		edges = cv2.Canny(image,80,120)
		edge = cv2.resize(edges,(480,480),interpolation = cv2.INTER_CUBIC) 
		temp = np.array(edge)
		number = zip(*np.where(temp==255))
		if(len(number)>30):
			#print len(number)
			return True
		else:
			print "idel"
		rawCapture.truncate(0)	
		key = cv2.waitKey(1) & 0xFF
		if key == ord("q"):
			break

def value_calculation(initial_location,value_recorder):
	# This function is used to detect the order of resistor.
	# The initial_location should be a list map from color id to color x location.
	reverse_map = {}
	i = 0
	while i<4:
		if(initial_location[i][0]) == -2:
			bg = initial_location[i][1]
		else:
			reverse_map[initial_location[i][1]]= initial_location[i][0]
		i += 1
	Reverse_map_order = sorted(reverse_map.items())
	if abs(Reverse_map_order[0][0]-bg) < abs(Reverse_map_order[-1][0]-bg):
		final_map = list(reversed(Reverse_map_order)) 
	else:
		final_map = Reverse_map_order
	value = (final_map[0][1] *10 +final_map[1][1]) * pow(10,final_map[2][1])
	if(value_recorder[0] == value):
		value_recorder[1] += 1
	else:
		value_recorder[0] = value
		value_recorder[1] = 0
	return value_recorder

def algrithom_lines(y1,y2):
	# Detect the center line
	k = (y2-y1)/(479.0-0.0)
	x = 0
	y_array = []
	while x<480: 
		y = k*(x-0) + y1
		y_array.append(int(y))
		x += 1
	return y_array

def edge_detector(image):
	# Detect the edge of resistor
	median = cv2.medianBlur(image,5)
	image_gray = cv2.cvtColor(median, cv2.COLOR_BGR2GRAY)
	edges = cv2.Canny(image_gray,20,80)
	edge = cv2.resize(edges,(640,480),interpolation = cv2.INTER_CUBIC) 
	temp = np.array(edge)
	i = 0
	total_upper = 0
	count_upper = 0
	total_down = 0
	count_down = 0
	while i<600:
		if ( (temp[0][i]) == 255 and i<600):
			total_upper += i
			count_upper += 1
		if ( (temp[-1][i]) == 255 and i<600):
			total_down += i
			count_down += 1
		i+=1
	if(count_upper != 0 and count_down != 0):
		y1 = total_upper/count_upper
		y2 = total_down/count_down
		cal_y = algrithom_lines(y1,y2)
		return cal_y
	return 0
		
def bit_calculator(cal_y,image_bgr):
	# Calculate the center line location
	i = 0
	color_array = []
	while i < len(image_bgr):
		color_array.append(image_bgr[i][cal_y[i]])
		i += 1
	np_array = np.mat(color_array)
	np_new = np_array[np.newaxis,:,:]
	return np_new

def color_detection(camera):
	# Detect the color ring of the resistor
	error_count = 0
	rawCapture = PiRGBArray(camera,size=(640,480))
	valid_count = 0
	value_recorder = [0,valid_count]
	for frame in camera.capture_continuous(rawCapture, format = "bgr", use_video_port = True):
		image = frame.array	
		gaussian_3 = cv2.GaussianBlur(image,(9,9),10.0)
		unsharp_image = cv2.addWeighted(image, 1.5, gaussian_3, -0.5, 0, image)
		hsv = cv2.cvtColor(unsharp_image,cv2.COLOR_BGR2HSV)	
		count = 0
		Color_string =""
		dic_list=[]
		bg = 0
		error_count += 1

		if(error_count >= 15):
			print "I cannot tell the color of the resistor"
			print "Let's Continue"
			return -1
		for i in Color_map:
			kernel = np.ones((3,3), np.uint8)	
			if i == -2:
				kernel = np.ones((8,8), np.uint8)
			if i == 2:
				lower_bound = np.array(Color_lower_2[i])	
				upper_bound = np.array(Color_upper_2[i])
				mask = cv2.inRange(hsv,lower_bound,upper_bound)
				res2 = cv2.erode(mask,kernel,iterations=1)
				mask =cv2.bitwise_or(mask,res2,mask)
			
			lower_bound = np.array(Color_lower[i]) 
			upper_bound = np.array(Color_upper[i])
			mask = cv2.inRange(hsv,lower_bound,upper_bound)		#erosion filetering after mask.			
			mask = cv2.erode(mask,kernel,iterations=1)
			res = cv2.bitwise_and(image,image,mask=mask)
			
			temp = np.array(mask)
			number = zip(*np.where(temp==255))
			if (len(number) > Thresh_hold and len(number)<18000):
				temp = []
				x=0
				y=0
				for j in number:
					x += j[0]
					y += j[1]
				location = "locate:" + str(x/len(number)) + "," +str(y/len(number))
				valid_loc = x/len(number)
				#print 'The Color ' + ' ' + Color_map[i] + ' is  ' +str(len(number)) + ' '+ location
				Color_string = Color_string + '\n' + 'The Color ' + ' ' + Color_map[i] + ' is  ' +str(len(number)) + ' '+ location
				count += 1
				temp.append(i)
				temp.append(valid_loc)
				dic_list.append(temp)
				if (i == -2):
					bg =1

		if((count == 4) and bg == 1):
			value_recorder = value_calculation(dic_list,value_recorder)
			if(value_recorder[1] == 2):
				print "the resistor is valid"
				return value_recorder[0]
		else:
			print "idel"

		rawCapture.truncate(0)	
		# Destory the windows
		key = cv2.waitKey(1) & 0xFF
		if key == ord("q"):
			break


def resistor_capture(camera,cm_pwm,db):
	# Capture the videostream to charge if there is a resistor.
	if(edge_detection(camera)):
		print "I detect the resistor"
		time.sleep(0.3)
		cm_stop(cm_pwm)
	resistor_value = color_detection(camera)
	add_to_db(resistor_value,db)
	print resistor_value
	return resistor_value
	

def cm_initialize():
	# Convey belt movement initialize
	duration = 20
	duty_cycle = 7.3
	frequency = (1-duty_cycle*0.01)*50
	GPIO.setmode(GPIO.BCM)
	GPIO.setup(26,GPIO.OUT)
	pwm = GPIO.PWM(26,frequency)
	pwm.start(duty_cycle)
	pwm.ChangeDutyCycle(0)
	return pwm

def cm_move(cm_pwm):
	# Convey Belt move
	duty_cycle = 7.3
	cm_pwm.ChangeDutyCycle(duty_cycle)
	
def cm_stop(cm_pwm):
	# Convey Belt stop
	print "stop"
	duty_cycle = 0
	cm_pwm.ChangeDutyCycle(duty_cycle)

def pm_initialize():
	# Sorted plate initialize
	duration = 20
	duty_cycle = 7.3
	frequency = (1-duty_cycle*0.01)*50
	GPIO.setmode(GPIO.BCM)
	GPIO.setup(4,GPIO.OUT)
	pwm = GPIO.PWM(4,frequency)
	pwm.start(duty_cycle)
	pwm.ChangeDutyCycle(0)
	return pwm

def pm_forward(time_forward):
	# Sorted plate move clockwise
	pm_pwm.ChangeDutyCycle(7.2)
	start_time = time.time()
	while time.time() - start_time < time_forward:
		pass
	pm_pwm.ChangeDutyCycle(0)
	time.sleep(1)

def pm_backward(time_backward):
	# Sorted plate move count-clockwise
	pm_pwm.ChangeDutyCycle(6.9)
	start_time = time.time()
	while time.time() - start_time < time_backward:
		pass
	pm_pwm.ChangeDutyCycle(0)
	time.sleep(1)
	
def result_handler(pm_pwm,key,cm_pwm):
	# Control the movement of sorted plate
	time_forward = Plate_map_forward[key]
	time_backward = Plate_map_backward[key]
	stay_time = 3
	if(key<3):
		pm_forward(time_forward)
		cm_move(cm_pwm)
		time.sleep(stay_time)
		pm_backward(time_backward)
	else:	
		pm_backward(time_backward)
		cm_move(cm_pwm)
		time.sleep(stay_time)
		pm_forward(time_forward)

if __name__ == "__main__":
	print "game start"
	global cm_pwm
	global pw_pwm
	cm_pwm=cm_initialize()
	pm_pwm=pm_initialize()
	camera = camera_intilize()	
	db = database_initial()
	while (button_press == 0):
		pass
	cm_move(cm_pwm)
	while True:
		value=resistor_capture(camera,cm_pwm,db)
		#print "the value of here is " + str(value)
		if value == -1:
			key = 0
		elif value == 51:
			key = 4
		elif value == 1000:
			key = 1
		elif value == 5100:
			key = 3
		elif value == 1000000:
			key = 2 
		else:
			key = 5
		#print "the value of the key is " + str(key)
		result_handler(pm_pwm,key,cm_pwm)