import cv2
import sys
import imutils as im

from template_matcher import template_match_rotation
from util import bbox_extend, bbox_round, process_image_bbox, image_crop, bbox_add_position, to_rect

IMG_SCALE_UP = 1
TEMPLATE_MATCH_OVERLAP = 1.0


def match(source, template, angle, center):
	# coarse
	angle_c, loc = template_match_rotation(source, template, angle, center)
	# fine
	angle, loc = template_match_rotation(source, template, angle_c, center, rot_min=-0.25, rot_max=+0.25, n_steps=20, scale=IMG_SCALE_UP)

	return angle, loc


class Reference:
	def __init__(self, ref_gray):
		self.center = (0, 0)
		self.ref = cv2.cvtColor(ref_gray, cv2.COLOR_GRAY2BGR)
		self.work = self.ref.copy()
		cv2.namedWindow('Ref')
		cv2.setMouseCallback('Ref', self.extract_coordinates)

	def set(self):
		while True:
			cv2.imshow("Ref", self.work)
			key = cv2.waitKey(1)
			if key == 32:
				break
			elif key == 27:
				break
		cv2.destroyWindow("Ref")
		return self.center

	def extract_coordinates(self, event, x, y, flags, parameters):
		if event == cv2.EVENT_LBUTTONDOWN:
			self.work = self.ref.copy()
			self.center = (x, y)

			print(f"Pixel at {self.center} = {self.work[y, x]}")
			self.work[y, x] = (0, 0, 255)
			print(f"Pixel at {self.center} = {self.work[y, x]}")


trackers = {
	'KCF': cv2.TrackerKCF,
	'CSRT': cv2.TrackerCSRT,
	'MIL': cv2.TrackerMIL,
	'GOTURN': cv2.TrackerGOTURN,
	'NANO': cv2.TrackerNano,
	'MOSSE': cv2.legacy.TrackerMOSSE,
	'TLD': cv2.legacy.TrackerTLD,
	'BOOSTING': cv2.legacy.TrackerBoosting,
	'MEDIANFLOW': cv2.legacy.TrackerMedianFlow
}
tracker_type = 'KCF'
tracker = trackers[tracker_type].create()

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, frame_init = video.read()
if not ok:
	print('Cannot read video file')
	sys.exit()

bbox_template = cv2.selectROI("Frame", frame_init, False)
image_template = process_image_bbox(image_crop(frame_init, bbox_template))
ref = Reference(image_template)
ref_center = ref.set()

print(f"ref_center = {ref_center}")
# Initialize tracker with first frame and bounding box
tracker.create()
tracker.init(frame_init, bbox_template)

current_angle = 0
path = []
line_from = None

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

	# Start timer
	timer = cv2.getTickCount()

	# Update tracker
	ok, bbox_tracker = tracker.update(frame)

	image_template_rotated = image_template
	bbox_tracker_ext = bbox_extend(bbox_tracker, TEMPLATE_MATCH_OVERLAP)

	if ok:
		image_bbox = process_image_bbox(image_crop(frame, bbox_tracker_ext))
		current_angle, max_loc = match(image_bbox, image_template, current_angle, ref_center)

		# determine the starting and ending (x, y)-coordinates of the
		# bounding box
		(startX, startY) = max_loc
		print(f"current_angle: {current_angle:.2f}, startX: {startX}, startY: {startY}")
		image_template_rotated = im.rotate(image_template, current_angle, center=ref_center)

		# Matcher
		matcher_bbox = (max_loc[0], max_loc[1], image_template.shape[1], image_template.shape[0])
		matcher_bbox_global = bbox_round(bbox_add_position(matcher_bbox, bbox_tracker))
		matcher_top_left, matcher_bottom_right = to_rect(matcher_bbox_global)
		cv2.rectangle(frame, matcher_top_left, matcher_bottom_right, (255, 0, 0), 3)

		# Tracker
		tracker_top_left, tracker_bottom_right = to_rect(bbox_round(bbox_tracker))
		cv2.rectangle(frame, tracker_top_left, tracker_bottom_right, (0, 0, 255), 3)

		# Tracker ext
		tracker_top_left, tracker_bottom_right = to_rect(bbox_round(bbox_tracker_ext))
		cv2.rectangle(frame, tracker_top_left, tracker_bottom_right, (255, 0, 255), 3)

		# Calculate Frames per second (FPS)
		fps = cv2.getTickFrequency() / (cv2.getTickCount() - timer)

		# Draw path
		line_to = (round(startX + bbox_tracker[0]), round(startY + bbox_tracker[1]))
		if line_from is not None:
			path.append({'from': line_from, 'to': line_to})
		line_from = line_to

		for p in path:
			cv2.line(frame, p['from'], p['to'], (0, 0, 255), 2)

	else:
		# Tracking failure
		cv2.putText(frame, "Tracking failure detected", (100, 80), cv2.FONT_HERSHEY_SIMPLEX, 0.75, (0, 0, 255), 2)

	# Display tracker type on frame
	cv2.putText(frame, tracker_type + " Tracker", (100, 20), cv2.FONT_HERSHEY_SIMPLEX, 0.75, (50, 170, 50), 2)

	# Display FPS on frame
	cv2.putText(frame, "FPS : " + str(int(fps)), (100, 50), cv2.FONT_HERSHEY_SIMPLEX, 0.75, (50, 170, 50), 2)

	# Display result
	cv2.imshow("Frame", frame)

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