import numpy as np
import cv2 as cv
import math

def rotation_matrix_to_euler_angles(R):
	sy = math.sqrt(R[0,0]**2 + R[1,0]**2)
	singular = sy < 1e-6

	if not singular:
		x = math.atan2(R[2,1] , R[2,2])
		y = math.atan2(-R[2,0], sy)
		z = math.atan2(R[1,0], R[0,0])
	else:
		# Handle singularity
		x = math.atan2(-R[1,2], R[1,1])
		y = math.atan2(-R[2,0], sy)
		z = 0
	return np.array([x, y, z])

def to_board_pose_euler(r_vec, t_vec):
	r_mat = np.ndarray((3, 3))
	cv.Rodrigues(r_vec, r_mat)
	r_mat = np.hstack((r_mat, t_vec))
	r_mat = np.vstack((r_mat, np.array([0, 0, 0, 1])))
	r_mat = np.linalg.inv(r_mat)
	res = rotation_matrix_to_euler_angles(r_mat)
	return res, r_mat[:,3]

def cap_init(cap: cv.VideoCapture):
	cap.set(cv.CAP_PROP_SETTINGS, 1)
	cap.set(cv.CAP_PROP_FOURCC, cv.VideoWriter.fourcc('M', 'J', 'P', 'G'))
	cap.set(cv.CAP_PROP_FPS, 60.0)
	cap.set(cv.CAP_PROP_FRAME_WIDTH, 1280)
	cap.set(cv.CAP_PROP_FRAME_HEIGHT, 1024)
	cap.set(cv.CAP_PROP_EXPOSURE, 30)
	cap.set(cv.CAP_PROP_AUTO_EXPOSURE, 1)