import cv2

# Constant variables definition.
DESIRED_HEIGHT = 480		# The input image will be resized to this height, preserving its aspect ratio.
BLUE_THRESHOLD = 150		# If the blue channel is bigger than this, it is considered background and removed.
BINARY_THRESHOLD = 1		# If the pixel is not brighter than this, it is removed before detection.
LINE_THICKNESS = 2			# Thickness of the drawn lines.
BLUR_KERNEL_SIZE = 3		# The size of the Gaussian blur kernel.
DILATION_KERNEL_SIZE = 5	# The size of the dilation kernel.
DILATION_ITERATIONS = 5		# The number of dilation iterations.

# Colors (assuming the default BGR order).
RED = (0, 0, 255)
GREEN = (0, 255, 0)
BLUE = (255, 0, 0)
YELLOW = (0, 255, 255)


# -------------- Function definitions -----------------------------
def resizeImage(img):
	"Resize the input image based on the DESIRED_HEIGHT variable."
	p = img.shape;
	aspectRatio = p[0]/p[1]
	width = DESIRED_HEIGHT*aspectRatio
	img = cv2.resize(img, ( DESIRED_HEIGHT, int(width) ))
	return img

def processImage(img):

	# Resize image to the desired height.
	resized = resizeImage(img)
	
	dim = resized.shape

	# Remove BLUE
	noBlue = resized.copy()
	for i in range(dim[0]):
		for j in range(dim[1]):
			if (resized[i,j,0] > BLUE_THRESHOLD):
				noBlue[i,j,:] = 0

	# Convert to grayscale.
	gray = cv2.cvtColor(noBlue, cv2.COLOR_BGR2GRAY)

	# Blur the image.
	blur = cv2.GaussianBlur(gray, (BLUR_KERNEL_SIZE, BLUR_KERNEL_SIZE), 0)
	
	# Threshold the image and find its contours.
	_, imgThres = cv2.threshold(blur, BINARY_THRESHOLD, 255, cv2.THRESH_BINARY)
	
	# Dilate the image.
	dilated = cv2.dilate(imgThres, (DILATION_KERNEL_SIZE,DILATION_KERNEL_SIZE), iterations=DILATION_ITERATIONS)
	
	# Find the largest image contour.
	_, contours, hierarchy = cv2.findContours(dilated, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)
	maxContour = max(contours, key = cv2.contourArea)

	'''
	hull = cv2.convexHull(maxContour)
	cv2.drawContours(imgOriginal, maxContour, -1, (0,0,255), 3)
	cv2.drawContours(imgOriginal, hull, -1, (255,0,0), 3)
	'''

	# Get the bounding rectangle of the contour.
	x,y,w,h = cv2.boundingRect(maxContour)

	# Get the centroid of the rectangle.
	objCenterX = int( (x + x + w) / 2)
	objCenterY = int( (y + y + h) / 2)
	imgCenterX = int(dim[1]/2)
	imgCenterY = int(dim[0]/2)
	
	# Draw the bounding rectangle  and its centroid to the image.
	cv2.circle(resized, (objCenterX, objCenterY), 5, YELLOW, LINE_THICKNESS)
	cv2.rectangle(resized, (x,y), (x+w,y+h), RED, LINE_THICKNESS)
	
	return resized

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


# Read image from source
img = cv2.imread('/home/stelios/Desktop/IoT/Talos_Drones_Tracking_and_Telemetry/camera module/drone.jpg', cv2.IMREAD_COLOR)

# Process the image and get the output.
output = processImage(img)

cv2.imshow('Output', output)

# Terminate if the escape key is pressed or the window is closed.
while True:
	k = cv2.waitKey()
	if k == 27 or k == 255:
		break
cv2.destroyAllWindows()