def getSMin(img_copy, num_tools, smin_range):
	HSVValFile = open('HSVValFile.dat', "r");
	hsvcutoffs = cPickle.load(HSVValFile);
	HSVValFile.close()
	initial_smin = hsvcutoffs[2]
	
	timgObj = ToolImageProcessHelp()
	timgObj.setImage(img_copy.copy(), 'Original')
	timgObj.setSMin(initial_smin)
	timgObj.processImage()
	
	cv2.waitKey(0)
	
	smin_list = []
	search_flag = True
	
	for smin_val in range(initial_smin - smin_range, initial_smin + smin_range + 1):
		timgObj.setImage(img_copy.copy(), 'Original')
		timgObj.setSMin(smin_val)
		timgObj.processImage()
		num = timgObj.getNumTools()
		h = timgObj.getHuMomentsList()
		c = timgObj.getCentroidsList()
		if num == num_tools:
			smin_list.append(smin_val)
		
	smin_list_errors = map(lambda x: (x - initial_smin) ** 2, smin_list)
	return smin_list[smin_list_errors.index(min(smin_list_errors))]
def main():
		
	rospy.init_node("Surf_2014_Final")
	
	fo = open('left_del_cam_pos.dat', "r")
	del_limb = cPickle.load(fo)
	#del_limb = 'left'
	left_del_pos = cPickle.load(fo)
	cam_right_pos = cPickle.load(fo)
	fo.close()
	
	posFile = open("BaxterCamArmJointPosFile.dat", "r")
	cam_arm_joint_pos = cPickle.load(posFile)
	posFile.close()
	
	mvObj = moveGetCamArmImage('right')
	mvObj.moveCamArm()
	img = mvObj.getCamArmImage()
			
	ppObj = PixelToBaxterPosTransformer()
	ppObj.setOffsets(0.0, 0.017, -0.035)
	
	timgObj = ToolImageProcessHelp()
	timgObj.setImage(img, 'Original')
	timgObj.processImage_BackProject()
	
	cv2.imshow('Contours', timgObj.getContourImage())
	cv2.waitKey(0)
	cv2.destroyAllWindows()

	num = timgObj.getNumTools()
	h = timgObj.getHuMomentsList()
	c = timgObj.getCentroidsList()
	
	ToolList = []
	
	for i in range(num):
		tool = ToolIdentify()
		tool.setHuMoments([ h[0,i], h[1,i], h[2,i], h[3,i], h[4,i], h[5,i], h[6,i], h[7,i] ])
		tool.setCentroid(c[0, i], c[1, i])
		tool.identifyTool()
		ToolList.append(tool)
		print "Tool #%s: %s" % (i+1, tool.getToolName())

	
	
	
	index = raw_input("Enter index of tool to deliver:")
	index = int(index) - 1 # Because tools are printed from 1 onwards but indexing starts from 0
		
	ppObj.calcBaxterPos(ToolList[index]._centroidX, ToolList[index]._centroidY)
	(joints_loc, inter_joints_loc1, inter_joints_loc2) = ppObj.getBaxterJointSpace(del_limb)
		
	print "Delivering %s..." % (ToolList[index].getToolName())
	#############Chaneg right to left in cam_arm_joint_pos################ 
	#dummy = []
	#for i in cam_arm_joint_pos.keys():
	#	new_key = i.replace('right', 'left')
	#	dummy.append(new_key)
	#cam_right_pos = dict(zip(dummy, cam_arm_joint_pos.values()))
	########################################################################
	
	# Move Delivery Arm
	control_arm = baxter_interface.limb.Limb(del_limb)
	control_arm.set_joint_position_speed(0.5) # Joint movement speed
	control_arm.move_to_neutral(5)
	control_arm.move_to_joint_positions(inter_joints_loc1, 7.0)
	time.sleep(1)
	control_arm.move_to_joint_positions(joints_loc, 7.0)
	time.sleep(1)
	control_arm.move_to_joint_positions(inter_joints_loc2, 7.0)
	time.sleep(1)
	control_arm.move_to_joint_positions(left_del_pos, 7.0)
	time.sleep(3)
	control_arm.move_to_neutral(5)
	
	return 0
def main():
		
	rospy.init_node("Surf_2014_Final")
	lhObj = LeapHelp()
	initial_num_tools = int(raw_input("Enter number of tools on table:"))
	left_untuck = {"left_w0":-0.08, "left_w1":-1.0, "left_w2":-1.19, "left_e0":1.94,  "left_e1":0.67, "left_s0":1.03, "left_s1":-0.50}
	
	fo = open('left_del_cam_pos.dat', "r")
	del_limb = cPickle.load(fo)
	#del_limb = 'left'
	left_del_pos = cPickle.load(fo)
	cam_right_pos = cPickle.load(fo)
	fo.close()
	
	fo = open('left_high_pos.dat', "r")
	del_limb = cPickle.load(fo)
	left_high_pos = cPickle.load(fo)
	fo.close()
	
	fo = open('left_ret_pos.dat', "r")
	ret_limb = cPickle.load(fo)
	left_ret_del_pos = cPickle.load(fo)
	fo.close()
	
	posFile = open("BaxterCamArmJointPosFile.dat", "r")
	cam_arm_joint_pos = cPickle.load(posFile)
	posFile.close()
	
	posFile = open("BaxterRetArmJointPosFile.dat", "r")
	ret_arm_joint_pos = cPickle.load(posFile) # Ontable, moveCamArm above table, left_ret_del_pos:retrieval dleivery position
	posFile.close()
	
	
	ret_back_img = cv2.imread("RetrieveImage.png")
	ret_arm_cam = moveGetCamArmImage(limb = 'left', file_name = "RetrieveBaxterCamArmJointPosFile.dat")
	
	mvObj = moveGetCamArmImage('right')
	mvObj.moveCamArm()
	img = mvObj.getCamArmImage()
			
	ppObj = PixelToBaxterPosTransformer()
	ppObj.setOffsets(0.0, 0.019, -0.025)
	
	# Figuring out smin
	smin = getSMin(img.copy(), initial_num_tools, 10)
		
	timgObj = ToolImageProcessHelp()
	timgObj.setImage(img, 'Original')
	timgObj.setSMin(smin)
	#timgObj.processImage_BackProject()
	timgObj.processImage()
	
	num = timgObj.getNumTools()
	h = timgObj.getHuMomentsList()
	c = timgObj.getCentroidsList()
	
	baxter_img = timgObj.getContourImage()
	baxter_img = cv2.resize(baxter_img, (1024, 600)) 
	send_image(baxter_img)
	print "SMIN:%s" % (smin)
	
	
	ToolList = []
	
	for i in range(num):
		tool = ToolIdentify()
		tool.setHuMoments([ h[0,i], h[1,i], h[2,i], h[3,i], h[4,i], h[5,i], h[6,i], h[7,i] ])
		tool.setCentroid(c[0, i], c[1, i])
		tool.identifyTool()
		ToolList.append(tool)
		#print "Tool #%s: %s" % (i+1, tool.getToolName())

	last_ret_time = time.time()
	key_pos = 0
	just_delivered = False
	while(num > 0):
		just_delivered = False
		for i in range(num):
			print "Tool #%s: %s" % (i+1, ToolList[i].getToolName())

		##############LEAP STUFF########################################
		index = -1
		lhObj.processFrame()
		print "Fingers: %s" % (lhObj.getNumFingers())
		if(lhObj.getKeyTapDetected() == True):
			
			print "Extend Fingers:"
			
			key_pos = key_pos + 1
			key_pos = key_pos % 2
		
			time.sleep(2)
			lhObj.processFrame()
			finger_count = lhObj.getNumFingers()
				
			if (finger_count) in range(1, num+1):
				print "Tool Available"
				print finger_count
				index = int(finger_count) - 1
				
			else:
				print"Selected tool unavaialable"
	
			time.sleep(1)
			
		################################################################

		#index = raw_input("Enter index of tool to deliver:")
		#index = int(index) - 1 # Because tools are printed from 1 onwards but indexing starts from 0

		
		if(index != -1):
			ppObj.calcBaxterPos(ToolList[index]._centroidX, ToolList[index]._centroidY)
			(joints_loc, inter_joints_loc1, inter_joints_loc2) = ppObj.getBaxterJointSpace(del_limb)
			print "Delivering %s..." % (ToolList[index].getToolName())
			just_delivered = True		
			
			del(ToolList[index])
			num = num - 1
			
			
			#############Chaneg right to left in cam_arm_joint_pos################ 
			#dummy = []
			#for i in cam_arm_joint_pos.keys():
			#	new_key = i.replace('right', 'left')
			#	dummy.append(new_key)
			#cam_right_pos = dict(zip(dummy, cam_arm_joint_pos.values()))
			########################################################################
			
			#Move Delivery Arm
			control_arm = baxter_interface.limb.Limb(del_limb)
			control_arm.set_joint_position_speed(0.5) # Joint movement speed
			control_arm.move_to_joint_positions(left_high_pos, 7.0)
			control_arm.move_to_joint_positions(inter_joints_loc1, 7.0)
			time.sleep(1)
			control_arm.move_to_joint_positions(joints_loc, 7.0)
			time.sleep(1)
			control_arm.move_to_joint_positions(inter_joints_loc2, 7.0)
			time.sleep(1)
			control_arm.move_to_joint_positions(left_del_pos, 7.0)
			time.sleep(3)
			control_arm.move_to_neutral(timeout = 7)
			ret_arm_cam.moveCamArm()
	
	
		############################################################################################
		tool_detected = False
		if(time.time() > 15 + last_ret_time) and (just_delivered == False):
			print "Checking retrieval tray"
			ret_arm_cam.moveCamArm()
			icObj = ImageClipper('RetrieveImageClipPosFile.dat')
			per_white = 100.0
			count = 0
			while (per_white >= 18.0 or per_white <= 2.0) and count <= 3:
				img = ret_arm_cam.getCamArmImage()
				img = icObj.clipImage(img)
				per_white, binary_img = getForeBack(ret_back_img, img)
				print "Waiting for next iteration"
				print
				count += 1
			
			if per_white >= 2.0 and per_white <= 15.0:
				print "TOOL PRESENT"
				tool_detected = True
			else:
				print "NO TOOL"
				tool_detected = False
			last_ret_time = time.time()
			
		##########
		
		###########
		if tool_detected == True:
			# Move Retrieval Arm
			control_arm = baxter_interface.limb.Limb(ret_limb)
			control_arm.set_joint_position_speed(0.5) # Joint movement speed
			ret_arm_cam.moveCamArm()
			time.sleep(1)
			control_arm.move_to_joint_positions(ret_arm_joint_pos, 7.0)
			time.sleep(1)
			ret_arm_cam.moveCamArm()
			time.sleep(1)
			control_arm.move_to_joint_positions(left_ret_del_pos, 7.0)
			time.sleep(1)
			ret_arm_cam.moveCamArm()
			
		############################################################################################
		
		
	
	return 0