# 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/frameTools/frameTools.py
# def GetShape(frame):
# 	'''
# 	 @brief Get shape of frame, regardless of depth dimension.
# 
# 	 @param frame 
# 	 
# 	 return (height, width)
# 	'''
# 	return frame.shape[:2]
# 
# def CheckColor(frame):
# 	'''
# 	 @brief Check wether the frame is of grayscale or color.
# 	 	converts the frame to color if necessary.
# 
# 	 @param frame
# 	 
# 	 @return color frame.
# 	'''
# 	shape = frame.shape
# 	if len(shape) == 2:
# 		cl_frame = ToColor(frame)
# 	else:
# 		cl_frame = frame
# 	return cl_frame


import cv2, math, warnings
import numpy as np
from Settings.Exceptions import DroneVisionError
from EdgeHeading import EdgeHeading
from src.DroneVision.DroneVision_src.imgProcessing.frameTools.frameTools import GetShape, CheckColor
			for edge_heading in selected_blade_edge_headings:
				frame = self.DrawHeading(frame, edge_heading, color=(255,255,0)) 					#YELLOW
			frame = self.DrawHeading(frame, selected_blade_heading, color=(255,128,0))							#ORANGE

		return selected_blade_heading, selected_tip_or_root_heading, tip_or_root_detected, frame

	def ResetHeading(self):
		'''
		 @brief Reset heading
		'''
		self.__current_heading 				= (None, None)
		self.__following_horizontal_edges 	= None

	def FindHeading(self, frame, selected_blade_edge_headings, rho_step_distance=None, default_rho_step_distance_diag_perc=1/4.0, rho_min_diag_perc=1/4.0, flip_heading=False):
		'''
		 @brief Find the next heading based on selected edge heading and current heading.
		 	Computed using equations (6.14 to 6.17) in MSc final thesis - Hans Erik Heggem
		 	Reset the heading by using ResetHeading()

		 @param frame
		 @param selected_blade_edge_headings
		 @param rho_step_distance (step distance in pixels for the drone to fly during each iteration. If None = then each step distance is set to a quarter the diagonal frame size (default=None))
		 @param default_rho_step_distance_diag_perc (Default step distance if rho_step_distance==None. Given as a percent of the diagonal frame size (default=0.25))
		 @param rho_min_diag_perc (Default percent of the diagonal length from center to image edge, used to calculate rho_min to navigate along a singel edge. Given as a percent between 0-1 (default=0.25))
		 @param flip_heading (Flip heading in opposite direction (True/False))

		 @return selected_heading (rho = distance to next heading position (in image frame), theta = heading angle in radian degrees)
		'''
		selected_heading = (0.0,0.0)
		if len(selected_blade_edge_headings) > 0:

# 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/frameTools/frameTools.py
# def GetShape(frame):
# 	'''
# 	 @brief Get shape of frame, regardless of depth dimension.
# 
# 	 @param frame 
# 	 
# 	 return (height, width)
# 	'''
# 	return frame.shape[:2]
# 
# def CheckColor(frame):
# 	'''
# 	 @brief Check wether the frame is of grayscale or color.
# 	 	converts the frame to color if necessary.
# 
# 	 @param frame
# 	 
# 	 @return color frame.
# 	'''
# 	shape = frame.shape
# 	if len(shape) == 2:
# 		cl_frame = ToColor(frame)
# 	else:
# 		cl_frame = frame
# 	return cl_frame


# Path: src/DroneVision/DroneVision_src/imgProcessing/Heading/Heading.py
import cv2, math, warnings
import numpy as np
from Settings.Exceptions import DroneVisionError
from EdgeHeading import EdgeHeading
from src.DroneVision.DroneVision_src.imgProcessing.frameTools.frameTools import GetShape, CheckColor

			for edge_heading in selected_blade_edge_headings:
				frame = self.DrawHeading(frame, edge_heading, color=(255,255,0)) 					#YELLOW
			frame = self.DrawHeading(frame, selected_blade_heading, color=(255,128,0))							#ORANGE

		return selected_blade_heading, selected_tip_or_root_heading, tip_or_root_detected, frame

	def ResetHeading(self):
		'''
		 @brief Reset heading
		'''
		self.__current_heading 				= (None, None)
		self.__following_horizontal_edges 	= None

	def FindHeading(self, frame, selected_blade_edge_headings, rho_step_distance=None, default_rho_step_distance_diag_perc=1/4.0, rho_min_diag_perc=1/4.0, flip_heading=False):
		'''
		 @brief Find the next heading based on selected edge heading and current heading.
		 	Computed using equations (6.14 to 6.17) in MSc final thesis - Hans Erik Heggem
		 	Reset the heading by using ResetHeading()

		 @param frame
		 @param selected_blade_edge_headings
		 @param rho_step_distance (step distance in pixels for the drone to fly during each iteration. If None = then each step distance is set to a quarter the diagonal frame size (default=None))
		 @param default_rho_step_distance_diag_perc (Default step distance if rho_step_distance==None. Given as a percent of the diagonal frame size (default=0.25))
		 @param rho_min_diag_perc (Default percent of the diagonal length from center to image edge, used to calculate rho_min to navigate along a singel edge. Given as a percent between 0-1 (default=0.25))
		 @param flip_heading (Flip heading in opposite direction (True/False))

		 @return selected_heading (rho = distance to next heading position (in image frame), theta = heading angle in radian degrees)
		'''
		selected_heading = (0.0,0.0)
		if len(selected_blade_edge_headings) > 0:

			I_l, I_w 	= GetShape(frame)