import cv2
import sys
import imutils as im

from util import bbox_extend, bbox_round, image_crop, to_rect, bbox_add_position

COLOR_TRACKER = (0, 255, 0)
COLOR_TRACKER_EXT = (0, 255, 255)
COLOR_MATCHER = (0, 0, 255)
COLOR_TEMPLATE = (255, 0, 0)
IMG_SCALE_UP = 1
TEMPLATE_MATCH_OVERLAP = 1

video = cv2.VideoCapture("./data/IMG_1730.MP4")

# Exit if video not open
# d.
if not video.isOpened():
	print("Could not open video")
	sys.exit()

# Read first frame.
ok, image = video.read()
if not ok:
	print('Cannot read video file')
	sys.exit()

bb_crop = bbox_extend(cv2.selectROI("Image", image, False), TEMPLATE_MATCH_OVERLAP)
crop = image_crop(image.copy(), bb_crop)

bb_template = cv2.selectROI("Image", crop, False)
template = image_crop(crop, bb_template)

# Initialize tracker with first frame and bounding box
tracker = cv2.TrackerKCF().create()
tracker.init(image, bb_crop)

while True:
	# Read a new frame
	ok, image = video.read()
	if not ok:
		break

	# Start timer
	timer = cv2.getTickCount()

	# Update tracker
	ok, bb_crop = tracker.update(image)
	crop = image_crop(image.copy(), bb_crop)
	image_anno = image.copy()

	if ok:
		# Perform template match operations
		crop_scaled = im.resize(crop, IMG_SCALE_UP * crop.shape[0], IMG_SCALE_UP * crop.shape[1])
		template_scaled = im.resize(template, IMG_SCALE_UP * template.shape[0], IMG_SCALE_UP * template.shape[1])
		res = cv2.matchTemplate(crop_scaled, template_scaled, cv2.TM_CCOEFF_NORMED)
		(min_val, max_val, min_loc, max_loc) = cv2.minMaxLoc(res)

		matcher_bbox_local = (max_loc[0]/IMG_SCALE_UP, max_loc[1]/IMG_SCALE_UP, template.shape[1], template.shape[0])
		matcher_top_left_local, matcher_bottom_right_local = to_rect(bbox_round(matcher_bbox_local))

		matcher_bbox = bbox_round(bbox_add_position(matcher_bbox_local, bb_crop))
		matcher_rect = to_rect(matcher_bbox)

		tracker_rect = to_rect(bb_crop)
		template_top_left, template_bottom_right = to_rect(bb_template)

		cv2.rectangle(crop, template_top_left, template_bottom_right, COLOR_TEMPLATE, 3)
		cv2.rectangle(crop, matcher_top_left_local, matcher_bottom_right_local, COLOR_MATCHER, 1)

		cv2.rectangle(image_anno, matcher_rect[0], matcher_rect[1], COLOR_MATCHER, 1)
		cv2.rectangle(image_anno, tracker_rect[0], tracker_rect[1], COLOR_TRACKER, 1)
		matcher_crop = image_crop(image, matcher_bbox)

		cv2.imshow("Crop", crop)
		cv2.imshow("Image", image_anno)
		cv2.imshow("Matcher res", res)
		cv2.imshow("Matcher view", matcher_crop)

	while True:
		# Exit if ESC pressed
		k = cv2.waitKey(1) & 0xff
		if k == 27:
			break
		if k == 32:
			break

	if k == 27:
		break