import sys
import numpy as np
import cv2 as cv
import argparse
from aruco_types import ARUCO_DICT

def factory(dict_type: int, board_size: cv.typing.Size, marker_length: float=100.0, marker_sep=10, margins=10):
	aruco_dict = cv.aruco.getPredefinedDictionary(dict_type)
	params = cv.aruco.DetectorParameters()
	detector = cv.aruco.ArucoDetector(aruco_dict, params)
	board = cv.aruco.GridBoard(board_size, markerLength=marker_length, markerSeparation=marker_sep, dictionary=aruco_dict)
	im_w = int(board_size[0] * (marker_length + marker_sep) - marker_sep + 2 * margins)
	im_h = int(board_size[1] * (marker_length + marker_sep) - marker_sep + 2 * margins)
	image = board.generateImage((im_w, im_h), None, margins, 1)
	cv.imwrite("results/aruco_board.png", image)

	return detector, board

def main():
	# construct the argument parser and parse the arguments
	ap = argparse.ArgumentParser()
	ap.add_argument("-t", "--type", type=str,
		default="DICT_ARUCO_ORIGINAL",
		help="type of ArUCo tag to detect")
	args = vars(ap.parse_args())

	# verify that the supplied ArUCo tag exists and is supported by
	# OpenCV
	if ARUCO_DICT.get(args["type"], None) is None:
		print("[INFO] ArUCo tag of '{}' is not supported".format(
			args["type"]))
		sys.exit(0)
	# load the ArUCo dictionary, grab the ArUCo parameters, and detect
	# the markers
	print("[INFO] detecting '{}' tags...".format(args["type"]))
	detector, board = factory(ARUCO_DICT[args["type"]], (5, 5))

	# load the input image from disk and resize it
	print("[INFO] start processing...")
	process_video(detector, board)

def process_video(detector: cv.aruco.ArucoDetector, board):
	with np.load('results/cam_calib_charuco.npz') as X:
		mtx, dist, _, _ = [X[i] for i in ('mtx', 'dist', 'rvecs', 'tvecs')]

	# Open camera
	video = cv.VideoCapture(0)
	if not video.isOpened():
		print("Cannot open camera")
		exit()

	while True:
		# Read a new frame
		ok, img = video.read()
		if not ok:
			return
		process(detector, board, img, mtx, dist)
		k = cv.waitKey(1) & 0xFF
		if k == ord('q'):
			break

def process(detector: cv.aruco.ArucoDetector, board: cv.aruco.GridBoard, img, mtx, dist, rvecs=None, tvecs=None, draw_marker_box=True, draw_marker_id=False, draw_marker_axis=True):	# Load previously saved data
	gray = cv.cvtColor(img, cv.COLOR_BGR2GRAY)
	(corners, ids, rejected) = detector.detectMarkers(gray)

	# Draw a square around the markers
	if draw_marker_box:
		cv.aruco.drawDetectedMarkers(img, corners)

	# Draw marker id
	if draw_marker_id:
		cv.aruco.drawDetectedMarkers(img, corners, ids)

	# Draw marker axis
	if draw_marker_axis:
		_rvecs, _tvecs, _ = cv.aruco.estimatePoseSingleMarkers(corners, markerLength=0.1, cameraMatrix=mtx, distCoeffs=dist)
		for i in range(len(_rvecs)):
			cv.drawFrameAxes(img, mtx, dist, _rvecs[i], _tvecs[i], 0.05)

	if len(corners) > 0:
		# Estimate pose of each marker and return the values rvec and tvec---(different from those of camera coefficients)
		obj_points, img_points = board.matchImagePoints(corners, ids)
		pose, rvec, tvec = cv.solvePnP(obj_points, img_points, mtx, dist, rvecs, tvecs)
		if pose:
			# Draw Axis
			cv.drawFrameAxes(img, mtx, dist, rvec, tvec, board.getMarkerLength()*1.5)


	cv.imshow('img', img)


if __name__ == '__main__':
	main()
	cv.destroyAllWindows()