/* +---------------------------------------------------------------------------+
   |          The Mobile Robot Programming Toolkit (MRPT) C++ library          |
   |                                                                           |
   |                   http://mrpt.sourceforge.net/                            |
   |                                                                           |
   |   Copyright (C) 2005-2008  University of Malaga                           |
   |                                                                           |
   |    This software was written by the Perception and Robotics               |
   |      research group, University of Malaga (Spain).                        |
   |    Contact: Jose-Luis Blanco  <jlblanco@ctima.uma.es>                     |
   |                                                                           |
   |  This file is part of the MRPT project.                                   |
   |                                                                           |
   |     MRPT is free software: you can redistribute it and/or modify          |
   |     it under the terms of the GNU General Public License as published by  |
   |     the Free Software Foundation, either version 3 of the License, or     |
   |     (at your option) any later version.                                   |
   |                                                                           |
   |   MRPT is distributed in the hope that it will be useful,                 |
   |     but WITHOUT ANY WARRANTY; without even the implied warranty of        |
   |     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         |
   |     GNU General Public License for more details.                          |
   |                                                                           |
   |     You should have received a copy of the GNU General Public License     |
   |     along with MRPT.  If not, see <http://www.gnu.org/licenses/>.         |
   |                                                                           |
   +---------------------------------------------------------------------------+ */
#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.
		  */
		double minimumDistanceFromPointToSegment(
				const double &	Px,
				const double &	Py,
				const double &	x1,
				const double &	y1,
				const double &	x2,
				const double &	y2,
				double &	out_x,
				double &	out_y);

		/** Computes the closest point from a given point to a segment, and returns that minimum distance.
		  */
		double minimumDistanceFromPointToSegment(
				const double &	Px,
				const double &	Py,
				const double &	x1,
				const double &	y1,
				const double &	x2,
				const double &	y2,
				float &	out_x,
				float &	out_y);


		/** Computes the closest point from a given point to a segment.
		  * \sa closestFromPointToLine
		  */
		void closestFromPointToSegment(
				const double &	Px,
				const double &	Py,
				const double &	x1,
				const double &	y1,
				const double &	x2,
				const double &	y2,
				double &out_x,
				double &out_y);

		/** Computes the closest point from a given point to a (infinite) line.
		  * \sa closestFromPointToSegment
		  */
		void closestFromPointToLine(
				const double &	Px,
				const double &	Py,
				const double &	x1,
				const double &	y1,
				const double &	x2,
				const double &	y2,
				double &out_x,
				double &out_y);

		/** Returns the square distance from a point to a line.
		  */
		double closestSquareDistanceFromPointToLine(
				const double &	Px,
				const double &	Py,
				const double &	x1,
				const double &	y1,
				const double &	x2,
				const double &	y2 );


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

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

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

		/** Returns true if the 2D point (px,py) falls INTO the given polygon.
		  */
		bool  pointIntoPolygon2D(const double & px, const double & py, unsigned int polyEdges, const double *poly_xs, const double *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.
		  */
		double distancePointToPolygon2D(const double & px, const double & py, unsigned int polyEdges, const double *poly_xs, const double *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(
							const double &	p1_x, const double &	p1_y, const double & p1_z,
							const double &	p2_x, const double &	p2_y, const double & p2_z,
							const double &	p3_x, const double & p3_y, const double & p3_z,
							const double &	p4_x, const double & p4_y, const 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(
					const double &	R1_x_min,	const double &	R1_x_max,
					const double &	R1_y_min,	const double &	R1_y_max,
					const double &	R2_x_min,	const double &	R2_x_max,
					const double &	R2_y_min,	const double &	R2_y_max,
					const double &	R2_pose_x,
					const double &	R2_pose_y,
					const double &	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:
			T	n_xy = square(dx)+square(dy);
			T	n = sqrt(n_xy+square(dz));
			n_xy = sqrt(n_xy);
			P(0,0) = dx / n;
			P(1,0) = dy / n;
			P(2,0) = dz / n;

			// 2nd perpendicular vector:
			if (fabs(dx)>1e-4 || fabs(dy)>1e-4)
			{
				P(0,1) = -dy / n_xy;
				P(1,1) = dx / n_xy;
				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
