import cv2
import numpy as np
import time

#####################################

def find_circle(img):
	circles = cv2.HoughCircles(img, cv2.cv.CV_HOUGH_GRADIENT, 1.2, 200, param1=50,param2=30,minRadius=40,maxRadius=0)
	if circles is None:
		return None
	height, width = img.shape[:2]
	output = np.zeros((height,width,3), np.uint8)
	
	circles = np.round(circles[0, :]).astype("int")
	for (x, y, r) in circles:
		cv2.circle(output, (x, y), r, (0, 255, 0), 4)
	return output

def find_cross(img):
	edges = cv2.Canny(img,50,150,apertureSize = 3)
	lines = cv2.HoughLines(edges,rho=1,theta=np.pi/180, threshold=50)

	if lines is None:
		return None
	r = []
	for rho,theta in lines[0]:
		if rho > 0:
			t = theta * 180
		else:
			t = theta*180 - 360
		if 10 < t < 80:
			r.append((rho,theta))
			break;

	for rho,theta in lines[0]:
		if rho > 0:
			t = theta * 180
		else:
			t = theta*180 - 360

		if 100 < t < 170:
			r.append((rho,theta))
			break;
	
	if len(r) != 2:
		return None

	height, width = img.shape[:2]
	output = np.zeros((height,width,3), np.uint8)
		
	for rho,theta in r:
		a = np.cos(theta)
		b = np.sin(theta)
		x0 = a*rho
		y0 = b*rho
		x1 = int(x0 + 1000*(-b))
		y1 = int(y0 + 1000*(a))
		x2 = int(x0 - 1000*(-b))
		y2 = int(y0 - 1000*(a))

		cv2.line(output,(x1,y1),(x2,y2),(0,0,255),2)
	return output


margin = 120
thr = 125

camera = cv2.VideoCapture(1)

print("Starting...")

try:
	while True:

		(grabbed, frame) = camera.read()
		if not grabbed:
			break
		frame = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
		height, width = frame.shape[:2]
		th, frame = cv2.threshold(frame, thr, 255, cv2.THRESH_BINARY);
		
		area = frame[margin:height-margin, margin:width-margin]
		
		circ = find_circle(area);
		cross = find_cross(area)
		
		if circ is not None:
			cv2.imshow("Camera", circ);
		elif cross is not None:
			cv2.imshow("Camera", cross);
		else:
			cv2.imshow("Camera", area);
		cv2.waitKey(1)


except KeyboardInterrupt:
	pass

finally:
	print("Stopping")
	camera.release()
	cv2.destroyAllWindows()