# Path: Settings/Exceptions.py
# class DroneVisionError(Exception):
# 	def __init__(self, error_key=None):
# 		'''CONSTRUCTOR'''
# 
# 		self.__error_messages = {	'no_blobs_error_msg'								: 'No blobs detected!', \
# 									'blob_distance_is_not_calibrated_error_msg'			: 'Blob distance is not calibrated!', \
# 									'feature_descriptor_not_available_error_msg'		: 'Feature descriptor not avaiable - Use block matching!', \
# 									'find_line_limits_unexpected_angle'					: 'Unexpected angle: provide vertical or horizontal hough lines only',  \
# 									'error_msg_no_heading_angle_detected' 				: 'No heading angles detected', \
# 									'error_msg_no_backup_heading_available'				: 'No edge heading detected, and no backup heading available.', \
# 									'find_line_limits_no_hor_or_vert_found' 			: 'Only vertical or horizontal lines found - need both horizontal and vertical lines to process.', \
# 									'detect_boundary_edge_not_found_all_edge_lines'		: 'Could not compute all edges lines - need all boundary edge lines to process', \
# 									'no_3D_point_matches'								: 'Could not compute 3D points - no matching points.', \
# 									'could_not_get_point_list_from_slave'				: 'Timeout thread - could not get right point list from slave within timeout!', \
# 									'triangulation_error_msg'							: 'Triangulation failed!', \
# 									'3D_point_filtration_failed'						: 'Filtration of 3D points failed!'}
# 
# 		self.msg = None
# 		if error_key != None:
# 			self.msg = self.__error_messages[error_key]
# 
# 	def __str__(self):
# 		return repr(self.msg)
# 
# 	def GetErrorMessage(self, error_key):
# 		'''
# 		 @brief Return all possible error messages
# 		'''
# 		return self.__error_messages[error_key]


import cv2
import warnings
import numpy as np
from StereoCalibration import StereoCalibration
from Settings.Exceptions import DroneVisionError
		 @param key_point2 (homogenous image point in 2nd camera (u,v,1) - cv2 key point)
		 @param P2 (camera 2 matrix (projection matrix 3x4))
		 @param wi1 (weight for camera 1 - used for the iterative method)
		 @param wi2 (weight for camera 2 - used for the iterative method)

		 @return X (4x1 matrix as [x,y,z,1]^T)
		'''
		#build matrix A for homogenous equation system Ax = 0
		#assume X = (x,y,z,1), for Linear-LS method, 
		#which turns it into a AX = B system, where A is 4x3, X is 3x1 and B is 4x1

		A = np.zeros((4,3))
		X = np.zeros((3,1))
		B = np.zeros((4,1))

		# Creating A matrice - each column depends 
		for i in range(3):
			A[0,i] = (key_point1.pt[0]*P1[2,i] - P1[0,i])/wi1
			A[1,i] = (key_point1.pt[1]*P1[2,i] - P1[1,i])/wi1
			A[2,i] = (key_point2.pt[0]*P2[2,i] - P2[0,i])/wi2
			A[3,i] = (key_point2.pt[1]*P2[2,i] - P2[1,i])/wi2

		B[0,0] = -(key_point1.pt[0]*P1[2,3] - P1[0,3])/wi1
		B[1,0] = -(key_point1.pt[1]*P1[2,3] - P1[1,3])/wi1
		B[2,0] = -(key_point2.pt[0]*P2[2,3] - P2[0,3])/wi2
		B[3,0] = -(key_point2.pt[1]*P2[2,3] - P2[1,3])/wi2

		flags = cv2.DECOMP_SVD
		retval, X = cv2.solve(A, B, flags=flags)
		if not(retval):

# Path: Settings/Exceptions.py
# class DroneVisionError(Exception):
# 	def __init__(self, error_key=None):
# 		'''CONSTRUCTOR'''
# 
# 		self.__error_messages = {	'no_blobs_error_msg'								: 'No blobs detected!', \
# 									'blob_distance_is_not_calibrated_error_msg'			: 'Blob distance is not calibrated!', \
# 									'feature_descriptor_not_available_error_msg'		: 'Feature descriptor not avaiable - Use block matching!', \
# 									'find_line_limits_unexpected_angle'					: 'Unexpected angle: provide vertical or horizontal hough lines only',  \
# 									'error_msg_no_heading_angle_detected' 				: 'No heading angles detected', \
# 									'error_msg_no_backup_heading_available'				: 'No edge heading detected, and no backup heading available.', \
# 									'find_line_limits_no_hor_or_vert_found' 			: 'Only vertical or horizontal lines found - need both horizontal and vertical lines to process.', \
# 									'detect_boundary_edge_not_found_all_edge_lines'		: 'Could not compute all edges lines - need all boundary edge lines to process', \
# 									'no_3D_point_matches'								: 'Could not compute 3D points - no matching points.', \
# 									'could_not_get_point_list_from_slave'				: 'Timeout thread - could not get right point list from slave within timeout!', \
# 									'triangulation_error_msg'							: 'Triangulation failed!', \
# 									'3D_point_filtration_failed'						: 'Filtration of 3D points failed!'}
# 
# 		self.msg = None
# 		if error_key != None:
# 			self.msg = self.__error_messages[error_key]
# 
# 	def __str__(self):
# 		return repr(self.msg)
# 
# 	def GetErrorMessage(self, error_key):
# 		'''
# 		 @brief Return all possible error messages
# 		'''
# 		return self.__error_messages[error_key]


# Path: src/DroneVision/DroneVision_src/imgProcessing/CameraCalibration/StereoVision.py
import cv2
import warnings
import numpy as np
from StereoCalibration import StereoCalibration
from Settings.Exceptions import DroneVisionError

		 @param key_point2 (homogenous image point in 2nd camera (u,v,1) - cv2 key point)
		 @param P2 (camera 2 matrix (projection matrix 3x4))
		 @param wi1 (weight for camera 1 - used for the iterative method)
		 @param wi2 (weight for camera 2 - used for the iterative method)

		 @return X (4x1 matrix as [x,y,z,1]^T)
		'''
		#build matrix A for homogenous equation system Ax = 0
		#assume X = (x,y,z,1), for Linear-LS method, 
		#which turns it into a AX = B system, where A is 4x3, X is 3x1 and B is 4x1

		A = np.zeros((4,3))
		X = np.zeros((3,1))
		B = np.zeros((4,1))

		# Creating A matrice - each column depends 
		for i in range(3):
			A[0,i] = (key_point1.pt[0]*P1[2,i] - P1[0,i])/wi1
			A[1,i] = (key_point1.pt[1]*P1[2,i] - P1[1,i])/wi1
			A[2,i] = (key_point2.pt[0]*P2[2,i] - P2[0,i])/wi2
			A[3,i] = (key_point2.pt[1]*P2[2,i] - P2[1,i])/wi2

		B[0,0] = -(key_point1.pt[0]*P1[2,3] - P1[0,3])/wi1
		B[1,0] = -(key_point1.pt[1]*P1[2,3] - P1[1,3])/wi1
		B[2,0] = -(key_point2.pt[0]*P2[2,3] - P2[0,3])/wi2
		B[3,0] = -(key_point2.pt[1]*P2[2,3] - P2[1,3])/wi2

		flags = cv2.DECOMP_SVD
		retval, X = cv2.solve(A, B, flags=flags)
		if not(retval):

			raise DroneVisionError('triangulation_error_msg')