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

IMG_SCALE_UP = 4
TEMPLATE_SEARCH_AREA = 0.5

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


def bbox_extend(bbox: cv2.typing.Rect, search_area: float):
	we2 = int(round(bbox[2]*search_area/2))
	he2 = int(round(bbox[3]*search_area/2))
	res = (int(round(bbox[0]-we2)),)
	res += (int(round(bbox[1]-he2)),)
	res += (int(bbox[2]+2*we2),)
	res += (int(bbox[3]+2*he2),)

	return res


def image_crop(src, bbox: cv2.typing.Rect, search_area: float = 0):
	bbox = bbox_extend(bbox, search_area)
	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 match(source, template, angle, center):
	best_angle = 0
	best_r = 0
	best_max_loc = 0

	im_source = im.resize(source, IMG_SCALE_UP * source.shape[0], IMG_SCALE_UP * source.shape[1])

	for dangle in np.linspace(-1.0, +1.0, 10):
		# rotated and scale up to IMG_SCALE_UP-size
		im_template = im.rotate(template, angle + dangle, center=center, scale=IMG_SCALE_UP)

		# 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]/IMG_SCALE_UP, maxLoc[1]/IMG_SCALE_UP)

	angle += best_angle

	return angle, best_max_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
		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 = 'MOSSE'
tracker = trackers[tracker_type].create()

video = cv2.VideoCapture("./data/production_id 4525346 (1080p).mp4")

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

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

bbox = cv2.selectROI("Frame", frame, False)
image_template = process_image_bbox(image_crop(frame, bbox, TEMPLATE_SEARCH_AREA))
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, bbox)

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

	# Start timer
	timer = cv2.getTickCount()

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

	image_bbox = process_image_bbox(image_crop(frame, bbox))
	image_template_rotated = image_template

	if ok:
		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"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)
	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("Tracking", frame)
	cv2.imshow("image_bbox", image_template_rotated)

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