/*---------------------------------------------------------------
	FILE: GEO.h
	USE: See description below.

   Part of the MRPT Library
   ISA - Universidad de Malaga - http://www.isa.uma.es
  ---------------------------------------------------------------*/
#ifndef  GEO_H
#define  GEO_H

#include <MRPT/UTILS/utils_defs.h>
#include <MRPT/UTILS/CMatrixTemplateNumeric.h>

/*---------------------------------------------------------------
	Class
  ---------------------------------------------------------------*/
namespace UTILS
{
	/** This base provides a set of static method for geometrical computations.
	 */
	namespace GEO
	{
		/** Computes the closest point from a given point to a segment, and returns that minimum distance.
		  */
		float  minimumDistanceFromPointToSegment(
				float	Px,
				float	Py,
				float	x1,
				float	y1,
				float	x2,
				float	y2,
				float	&out_x,
				float	&out_y);

		/** Returns the distance between 2 points in 2D.
		  */
		float  distanceBetweenPoints(float x1,float y1,float x2,float y2);

		/** Returns the intersection point, and if it exists, between two segments.
		  */
		bool  SegmentsIntersection(
					float	x1,float	y1,
					float	x2,float	y2,
					float	x3,float	y3,
					float	x4,float	y4,
					float	&ix,float  &iy);

		/** Returns true if the 2D point (px,py) falls INTO the given polygon.
		  */
		bool  pointIntoPolygon2D(float px, float py, unsigned int polyEdges, float *poly_xs, float *poly_ys );

		/** Returns the closest distance of a given 2D point to a polygon, or "0" if the point is INTO the polygon or its perimeter.
		  */
		float  distancePointToPolygon2D(float px, float py, unsigned int polyEdges, float *poly_xs, float *poly_ys );

		/** Calculates the minimum distance between a pair of lines.
		  The lines are given by:
			- Line 1 = P1 + f (P2-P1)
			- Line 2 = P3 + f (P4-P3)
		  The Euclidean distance is returned in "dist", and the mid point between the lines in (x,y,z)
		  \return It returns false if there is no solution, i.e. lines are (almost, up to EPS) paralel.
		 */
		bool  minDistBetweenLines(
							float	p1_x, float	p1_y, float p1_z,
							float	p2_x, float	p2_y, float p2_z,
							float	p3_x, float p3_y, float p3_z,
							float	p4_x, float p4_y, float p4_z,
							float	&x,   float &y,   float &z,
							float	&dist);

		/** Calculates the minimum distance between a pair of lines.
		  The lines are given by:
			- Line 1 = P1 + f (P2-P1)
			- Line 2 = P3 + f (P4-P3)
		  The Euclidean distance is returned in "dist", and the mid point between the lines in (x,y,z)
		  \return It returns false if there is no solution, i.e. lines are (almost, up to EPS) paralel.
		 */
		bool  minDistBetweenLines(
							double	p1_x, double	p1_y, double p1_z,
							double	p2_x, double	p2_y, double p2_z,
							double	p3_x, double p3_y, double p3_z,
							double	p4_x, double p4_y, double p4_z,
							double	&x,   double &y,   double &z,
							double	&dist);


		/** Returns wether two rotated rectangles intersect.
		 *  The first rectangle is not rotated and given by (R1_x_min,R1_x_max)-(R1_y_min,R1_y_max).
		 *  The second rectangle is given is a similar way, but it is internally rotated according
		 *   to the given coordinates translation (R2_pose_x,R2_pose_y,R2_pose_phi(radians)), relative
		 *   to the coordinates system of rectangle 1.
		 */
		bool  RectanglesIntersection(
					float	R1_x_min,	float	R1_x_max,
					float	R1_y_min,	float	R1_y_max,
					float	R2_x_min,	float	R2_x_max,
					float	R2_y_min,	float	R2_y_max,
					float	R2_pose_x,
					float	R2_pose_y,
					float	R2_pose_phi );

		/** Computes an axis base (a set of three 3D normal vectors) with the given vector being the first of them.
		  * NOTE: Make sure of passing all floats or doubles and that the template of the receiving matrix is of the same type!
		  *
		  *  If   \f$ d = [ dx ~ dy ~ dz ] \f$ is the input vector, then this function returns a matrix \f$ M \f$ such as:
		  *
		    \f[  M = \left(
					\begin{array}{c c c}
					v^1_x ~ v^2_x ~ v^3_x \\
					v^1_y ~ v^2_y ~ v^3_y \\
					v^1_z ~ v^2_z ~ v^3_z
					\end{array} \right)
			\f]
		  *
		  *   And the three normal vectors are computed as:

			  \f[ v^1 = \frac{d}{|d|}  \f]

			  If (dx!=0 or dy!=0):
				\f[ v^2 = \frac{[-dy ~ dx ~ 0 ]}{\sqrt{dx^2+dy^2}}  \f]
			  otherwise (the direction vector is vertical):
				\f[ v^2 = [1 ~ 0 ~ 0]  \f]

			  And finally, the third vector is the cross product of the others:

			    \f[ v^3 = v^1 \times v^2  \f]
		  *
		  * \return The 3x3 matrix (CMatrixTemplateNumeric<T>), containing one vector per column.
		  * \except Throws an std::exception on invalid input (i.e. null direction vector)
		  *
		  * (JLB @ 18-SEP-2007)
		  */
		template<class T>
		CMatrixTemplateNumeric<T> generateAxisBaseFromDirection( T dx, T dy, T dz )
		{
			MRPT_TRY_START;

			if (dx==0 && dy==0 && dz==0)
				THROW_EXCEPTION("Invalid input: Direction vector is (0,0,0)!");

			CMatrixTemplateNumeric<T>	P(3,3);

			// 1st vector:
			P(0,0) = dx;
			P(1,0) = dy;
			P(2,0) = dz;

			// 2nd perpendicular vector:
			if (dx!=0 || dy!=0)
			{
				T	n = sqrt(square(dx)+square(dy));
				P(0,1) = -dy / n;
				P(1,1) = dx / n;
				P(2,1) = 0;
			}
			else
			{
				// Any vector in the XY plane will work:
				P(0,1) = 1;
				P(1,1) = 0;
				P(2,1) = 0;
			}

			// 3rd perpendicular vector: cross product of the two last vectors:
			crossProduct3D(
				P(0,0),P(1,0),P(2,0),
				P(0,1),P(1,1),P(2,1),
				P(0,2),P(1,2),P(2,2) );

			return P;
			MRPT_TRY_END;
		}


		/** Computes the cross product of two 3D vectors, returning a vector normal to both.
		  *  It uses the simple implementation:

		    \f[  v_out = \left(
					\begin{array}{c c c}
					\hat{i} ~ \hat{j} ~ \hat{k} \\
					x0 ~ y0 ~ z0 \\
					x1 ~ y1 ~ z1 \\
					\end{array} \right)
			\f]
		  */
		template<class T>
		void crossProduct3D(
			T x0, T y0, T z0,
			T x1, T y1, T z1,
			T &x_out, T &y_out, T &z_out )
		{
			x_out =  y0*z1 - z0*y1;
			y_out = -x0*z1 + z0*x1;
			z_out =  x0*y1 - y0*x1;
		}


	} // End of class def.

} // End of namespace

#endif
