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.


# Colors (assuming 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

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

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

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

imgOriginal = img.copy()

# Remove BLUE
p = img.shape
for i in range(p[0]):
	for j in range(p[1]):
		if (img[i,j,0] > BLUE_THRESHOLD):
			img[i,j,:] = 0

# Convert to grayscale.
img = cv2.cvtColor(img, cv2.COLOR_RGB2GRAY)

# Threshold the image and find its contours.
thres, imgThres = cv2.threshold(img, BINARY_THRESHOLD, 255, cv2.THRESH_BINARY)
img2, contours, hierarchy = cv2.findContours(imgThres, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)

# Find the biggest contour.
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.
centerX = int( (x + x + w) / 2)
centerY = int( (y + y + h) / 2)

# Draw the bounding rectangle  and its centroid to the image.
cv2.circle(imgOriginal, (centerX, centerY), 5, YELLOW, 2)
cv2.rectangle(imgOriginal, (x,y), (x+w,y+h), RED, 2)

cv2.imshow('Output', imgOriginal)
cv2.waitKey(0)