import cv2
import sys
import numpy as np
import imutils as im

IMG_SCALE_UP = 1
TEMPLATE_MATCH_OVERLAP = 0.0

(major_ver, minor_ver, subminor_ver) = cv2.__version__.split('.')
print(cv2.__version__)


def bbox_round(src):
	x = int(round(src[0]))
	y = int(round(src[1]))
	w = int(round(src[2]))
	h = int(round(src[3]))

	return x, y, w, h


def bbox_extend(bbox: cv2.typing.Rect, factor: float = 0):
	we2 = bbox[2]*factor/2
	he2 = bbox[3]*factor/2

	return bbox_round((bbox[0]-we2, bbox[1]-he2, bbox[2]+2*we2, bbox[3]+2*he2))


def image_crop(src, bbox: cv2.typing.Rect):
	x = bbox[0]
	y = bbox[1]
	w = bbox[2]
	h = bbox[3]

	dst = src[y:y+h, x:x+w]
	return dst


def process_image_bbox(src):
	# convert to gray
	gray = cv2.cvtColor(src, cv2.COLOR_BGR2GRAY)

	return gray


def template_match_rotation(source, template, angle, center, rot_min=-1.0, rot_max=+1.0, n_steps=10, scale=1):
	best_angle = 0
	best_r = 0
	best_max_loc = 0
	r = 0
	im_source = im.resize(source, scale * source.shape[0], scale * source.shape[1])
	for dangle in np.linspace(rot_min, rot_max, n_steps):
		# rotated and scale up to IMG_SCALE_UP-size
		im_template = im.rotate(template, angle + dangle, center=center, scale=scale)

		# Perform template match operations
		res = cv2.matchTemplate(im_source, im_template, cv2.TM_CCOEFF_NORMED)
		(minVal, maxVal, minLoc, maxLoc) = cv2.minMaxLoc(res)

		r = maxVal
		if r > best_r:
			best_r = r
			best_angle = dangle
			best_max_loc = (maxLoc[0]/scale, maxLoc[1]/scale)
		else:
			break

	angle += best_angle
	loc_x = best_max_loc[0] + template.shape[0]/2 - 1
	loc_y = best_max_loc[1] + template.shape[1]/2 - 1

	return angle, (loc_x, loc_y)


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_init = cv2.selectROI("Frame", frame_init, False)
bbox_template = bbox_extend(bbox_init, TEMPLATE_MATCH_OVERLAP)
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_init)

current_angle = 0
#line_from = (ref_center[0] + bbox_template[0], ref_center[1] + bbox_template[1])
line_from = None

path = []

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 = bbox_round(bbox_tracker)

	if ok:
		image_bbox = process_image_bbox(image_crop(frame, bbox))
		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)

		# draw the bounding box on the image
		p1 = (int(startX + bbox[0]), int(startY + bbox[1]))
		p2 = (int(bbox[0]+bbox[2]), int(bbox[1]+bbox[3]))
		cv2.rectangle(frame, p1, p2, (255, 0, 0), 3)

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

		# Draw path
		line_to = (round(startX + bbox[0]), round(startY + bbox[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