# 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
	def ComputeHeading(self, frame, boundary_hough_lines, draw_possible_edge_headings=False, draw_headings=False, draw_elliptical_error_arrow=False, draw_min_rho=True):
		'''
		 @brief Compute heading according to the boundary_hough_lines.

		 	All headings are measured from image center, meaning returned rho and theta are in regard to the image center.
		 	Set draw_possible_edge_headings=True to return a colored frame showing all edge headings.
		 	Set draw_headings=True to return a colored frame showing the heading.
		 	The function considers the current heading to find the most probable next heading.
		 	Use SetCurrentHeading() to update the current heading, or GetCurrentHeading() to get it.

		 @param frame (undistorted frame)
		 @param boundary_hough_lines (See PointDetection.GetBoundaryHoughLines)
		 	boundary_hough_lines = [max_hor_hough_line, min_hor_hough_line, max_vert_hough_line, min_vert_hough_line],
		 	with hough line as (rho, theta)
		 @param draw_possible_edge_headings (default=False)
		 @param draw_headings (YELLOW = selected edge heading, ORANGE = selected heading, RED = selected tip/root heading (default=False))
		 @param draw_elliptical_error_arrow (DARK RED = drawn elliptical error arrow (True/False - default=True))
		 @param draw_min_rho (Draw minium rho of draw_elliptical_error_arrow == True (True/False - default=True))

	 	 @return selected_blade_heading, selected_tip_or_root_heading, tip_or_root_detected, frame
	 		 (Return 
	 		 	selected_blade_heading 			= (rho, theta) (Will be (None, None) if not detected), 
	 		 	selected_tip_or_root_heading 	= (rho, theta) (Will be (None, None) if not detected), 
	 		 	tip_or_root_detected 			= True/False
	 		 	frame
	 		rho = distance to edge, theta = heading direction in radian degrees.
	 		frame (if draw_possible_edge_headings or draw_headings = True, then it is converted to color frame))
		'''
		try:
			edge_heading_returns = self.ComputeEdgeHeading(frame, boundary_hough_lines, draw=draw_possible_edge_headings)

# 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

	def ComputeHeading(self, frame, boundary_hough_lines, draw_possible_edge_headings=False, draw_headings=False, draw_elliptical_error_arrow=False, draw_min_rho=True):
		'''
		 @brief Compute heading according to the boundary_hough_lines.

		 	All headings are measured from image center, meaning returned rho and theta are in regard to the image center.
		 	Set draw_possible_edge_headings=True to return a colored frame showing all edge headings.
		 	Set draw_headings=True to return a colored frame showing the heading.
		 	The function considers the current heading to find the most probable next heading.
		 	Use SetCurrentHeading() to update the current heading, or GetCurrentHeading() to get it.

		 @param frame (undistorted frame)
		 @param boundary_hough_lines (See PointDetection.GetBoundaryHoughLines)
		 	boundary_hough_lines = [max_hor_hough_line, min_hor_hough_line, max_vert_hough_line, min_vert_hough_line],
		 	with hough line as (rho, theta)
		 @param draw_possible_edge_headings (default=False)
		 @param draw_headings (YELLOW = selected edge heading, ORANGE = selected heading, RED = selected tip/root heading (default=False))
		 @param draw_elliptical_error_arrow (DARK RED = drawn elliptical error arrow (True/False - default=True))
		 @param draw_min_rho (Draw minium rho of draw_elliptical_error_arrow == True (True/False - default=True))

	 	 @return selected_blade_heading, selected_tip_or_root_heading, tip_or_root_detected, frame
	 		 (Return 
	 		 	selected_blade_heading 			= (rho, theta) (Will be (None, None) if not detected), 
	 		 	selected_tip_or_root_heading 	= (rho, theta) (Will be (None, None) if not detected), 
	 		 	tip_or_root_detected 			= True/False
	 		 	frame
	 		rho = distance to edge, theta = heading direction in radian degrees.
	 		frame (if draw_possible_edge_headings or draw_headings = True, then it is converted to color frame))
		'''
		try:
			edge_heading_returns = self.ComputeEdgeHeading(frame, boundary_hough_lines, draw=draw_possible_edge_headings)

		except DroneVisionError, err: