#ifndef LINE_3D_H
#define LINE_3D_H

#include "Vector3D.h"

namespace geometry{

	/**
	* Class representing a LineSegment3D (segemnt) defined by two points. 
	* Provides methods to calculate and return the direction and
	* normal vector of the LineSegment3D, to calculate the collision point
	* of two LineSegment3Ds and the distance from a LineSegment3D to a point. 
	* 
	*  p0 ---------- p1
	* 
	* @author Manuela Waldner
	*/
	template<class XNumber>
	class LineSegment3D{
	private:
		/** The first point on the LineSegment3D. */
		Vector3D<XNumber> p0; 
		/** The second point on the LineSegment3D. */
		Vector3D<XNumber> p1; 
		/** The direction vector of the LineSegment3D. */
		Vector3D<float> dir; 
		/** The normal vector of the LineSegment3D (normalized). */

		Vector3D<float> n; 

		/** Calculates the direction vector (p1 - p0). */
		void CalcDirVector(); 
		/** Calculates a normal vector and normalizes it. */
		void CalcNormalVector(); 
		
// 		void DistRegion0(float &s, float &t, float det); 
// 		void DistRegion1(float &s, float &t, XNumber b, XNumber c, XNumber e); 
// 		void DistRegion2(float &s, float &t, XNumber a, XNumber b, XNumber c, 
// 										 XNumber d, XNumber e, XNumber f); 
	public:
		/** Default constructor. */
		LineSegment3D(); 
		/** 
		* Constructor. 
		* @param p0 The first point on the LineSegment3D. 
		* @param p1 The second point on the LineSegment3D. 
		*/
		LineSegment3D(Vector3D<XNumber> p0, Vector3D<XNumber> p1);
		/** Destructor. */
		virtual ~LineSegment3D(); 

		/**
		* Sets the first point on the LineSegment3D. 
		* @param p0 The first point on the LineSegment3D. 
		*/
		void SetPoint0(Vector3D<XNumber> p0); 
		/**
		* Sets the second point on the LineSegment3D. 
		* @param p1 The second point on the LineSegment3D. 
		*/
		void SetPoint1(Vector3D<XNumber> p1); 
		/**
		* Sets a point on the LineSegment3D, specified by num. 
		* @param point The point to set. 
		* @param num Which point to set (0 or 1). Throws an exception if num is invalid. 
		*/
		void SetPoint(Vector3D<XNumber> point, int num); 
		/**
		 * Sets the start and end point of the LineSegment. 
		 * @param p0 Start point. 
		 * @param p1 End point. 
		 */
		void SetValues(Vector3D<XNumber> p0, Vector3D<XNumber> p1); 

		/**
		* Returns the first point on the LineSegment3D. 
		* @return The first point on the LineSegment3D. 
		*/
		Vector3D<XNumber> GetPoint0(); 
		/**
		* Returns the second point on the LineSegment3D. 
		* @return The second point on the LineSegment3D. 
		*/
		Vector3D<XNumber> GetPoint1(); 
		/**
		* Returns a point on the LineSegment3D, specified by num. 
		* @param num Which point to return (0 or 1). Throws an exception if num is invalid. 
		* @return Returns The point specified by num. 
		*/
		Vector3D<XNumber> GetPoint(int num); 

		/**
		* Returns the direction vector. 
		* @return The direction vector of the LineSegment3D. 
		*/
		Vector3D<float> GetDirVector(); 
		/**
		 * Returns the normalized direction vector. 
		 * @return The direction vector of the LineSegment3D - normalized. 
		 */
		Vector3D<float> GetNormalizedDirVector(); 
		/**
		* Returns the normal vector (normalized). 
		* @return The normal vector of the LineSegment3D. 
		*/
		Vector3D<float> GetNormalVector();

		/**
		 * Retrieves the angle between two LineSegment3Ds. 
		 * @param LineSegment3D The incoming LineSegment3D. 
		 * @return The angle in degrees. 
		 */
		float GetAngleBetween(LineSegment3D<XNumber> LineSegment3D); 

		/**
		 * Retrieves the intersection point between two LineSegment3Ds. 
		 * The intersection point, if available, will be stored in p. 
		 * @param p [out] The intersection point. Depending on the returned result
		 * this point might be uninitialized. 
		 * @param LineSegment3D The LineSegment3D to intersect. 
		 * @return Returns 0 if the two LineSegment3Ds intersect somewhere on the infinite extension of 
		 * the given LineSegment3D segments, 1 if they intersect on the given LineSegment3D segment, -1 if the LineSegment3Ds
		 * are coincident, and -2 if the LineSegment3Ds are parallel. P will only be applied a valid position, 
		 * if the return value is 1. 
		 */
		int GetIntersectionPoint(Vector3D<XNumber> &p, LineSegment3D<XNumber> LineSegment3D);
		
		LineSegment3D<XNumber> GetShortestConnectingLineSegment(LineSegment3D<XNumber> line1); 

		float GetPositionOnLine(Vector3D<XNumber> p); 

		/**
		* Calculates the t factor for the first LineSegment3D to collide. 
		* @param p0 One coordinate of the point0 of the first LineSegment3D. 
		* @param d One coordinate of the direction vector for the first LineSegment3D. 
		* @param p1 One coordinate of the point0 of the second LineSegment3D. 
		* @return Returns the result (t) as float. 
		*/
		float GetT(XNumber p0, XNumber d, XNumber p1); 

		/**
		* Calculates the t factor for the second LineSegment3D to collide. 
		* @param p0 One coordinate of the point0 of the first LineSegment3D. 
		* @param d0 One coordinate of the direction vector for the first LineSegment3D. 
		* @param p1 One coordinate of the point0 of the second LineSegment3D. 
		* @param d1 One coordinate of the direction vector for the second LineSegment3D. 
		* @param t The t value for the first LineSegment3D. 
		* @return Returns the result (t) as float. 
		*/
		float GetS(XNumber p0, XNumber d0, XNumber p1, XNumber d1, float t); 

		/**
		 * Gets the closest point on the line regarding to p. 
		 *              Q      D
		 *    SP x-----x------->
		 *        \    |
		 *         \   |
		 *          \  |
		 *           \ |
		 *            \|
		 *             x P
		 * @param p The incoming point to test. 
		 * @return Returns the closest point to p on the line. 
		 */
		Vector3D<XNumber> GetClosestPoint(Vector3D<XNumber> p); 

		/**
		* Calculates the distance of an incoming point to the LineSegment3D. 
		* @param p The incoming point. 
		* @return The distance as float value. 
		*/
		float GetDistancePoint(Vector3D<XNumber> p); 

		/**
		* Returns the length of the LineSegment3D. 
		* @return The length of the LineSegment3D as float value. 
		*/
		float GetLength(); 

		/** Prints the LineSegment3D on the console. */
		void Print();

		/**
		* Returns a point lying on the LineSegment3D specified by t. 
		* @param t The parameter specifying the position of the point on the LineSegment3D. If the point
		* should be on the LineSegment3D segment, t should be between 0 and 1. 
		* @return The resulting point as Vector3D. 
		*/
		Vector3D<XNumber> GetPointOnLine(float t); 
		
		XNumber GetDistanceLine(LineSegment3D<XNumber> line); 
		
		Vector3D<XNumber> GetCenterPoint(); 

		/**
		 * Multiplies the start and end point with a given matrix. 
		 * The direction and normal vector will be updated accordingly. 
		 * @param m The matrix to multiply with. 
		 */
		void MultMatrix(Matrix4x4 m); 
	};

	typedef LineSegment3D<int> LineSegment3Di; 
	typedef LineSegment3D<short> LineSegment3Ds; 
	typedef LineSegment3D<long> LineSegment3Dl; 
	typedef LineSegment3D<float> LineSegment3Df; 
	typedef LineSegment3D<double> LineSegment3Dd; 

	template <class XNumber> LineSegment3D<XNumber>::LineSegment3D(){
	}

	template <class XNumber> LineSegment3D<XNumber>::LineSegment3D(Vector3D<XNumber> p0, Vector3D<XNumber> p1){
		this->p0 = p0; 
		this->p1 = p1; 
		this->CalcDirVector(); 
		this->CalcNormalVector(); 
	}

	template <class XNumber> LineSegment3D<XNumber>::~LineSegment3D(){
	}

	template <class XNumber> void LineSegment3D<XNumber>::SetPoint0(Vector3D<XNumber> p0){
		this->p0 = p0; 
		this->CalcDirVector(); 
		this->CalcNormalVector(); 
	}

	template <class XNumber> void LineSegment3D<XNumber>::SetPoint1(Vector3D<XNumber> p1){
		this->p1 = p1; 
		this->CalcDirVector(); 
		this->CalcNormalVector(); 
	}

	template <class XNumber> void LineSegment3D<XNumber>::SetValues(Vector3D<XNumber> p0, Vector3D<XNumber> p1){
		this->p0 = p0; 
		this->p1 = p1; 
		this->CalcDirVector(); 
		this->CalcNormalVector(); 
	}

	template <class XNumber> void LineSegment3D<XNumber>::SetPoint(Vector3D<XNumber> point, int num){
		switch(num){
			case 0: 
				this->p0 = point; 
				break;
			case 1:
				this->p1 = point; 
				break; 
			default: 
				throw -1; 
		}
		this->CalcDirVector(); 
		this->CalcNormalVector(); 
	}

	template <class XNumber> Vector3D<XNumber> LineSegment3D<XNumber>::GetPoint0(){
		return this->p0; 
	}

	template <class XNumber> Vector3D<XNumber> LineSegment3D<XNumber>::GetPoint1(){
		return this->p1; 
	}

	template <class XNumber> Vector3D<XNumber> LineSegment3D<XNumber>::GetPoint(int num){
		switch(num){
			case 0: 
				return this->p0; 
			case 1: 
				return this->p1; 
			default: 
				throw -1; 
		}
	}

	template <class XNumber> Vector3D<float> LineSegment3D<XNumber>::GetDirVector(){
		return this->dir; 
	}

	template <class XNumber> Vector3D<float> LineSegment3D<XNumber>::GetNormalVector(){
		return this->n; 
	}

	template <class XNumber> Vector3D<float> LineSegment3D<XNumber>::GetNormalizedDirVector(){
		Vector3D<float> normDir = this->dir; 
		normDir.Normalize(); 
		return normDir; 
	}

	template <class XNumber> void LineSegment3D<XNumber>::Print(){
		printf("LineSegment3D:\n"); 
		this->p0.Print(); 
		this->p1.Print(); 
		printf("Length: %f (f)\n", this->GetLength()); 
		printf("Direction Vector: ");
		this->dir.Print(); 
		printf("Normal Vector: "); 
		this->n.Print(); 
		//printf("Bounding sphere: "); 
		//bs.Print(); 
	}

	template <class XNumber> float LineSegment3D<XNumber>::GetLength(){
		//calculate the distance between point0 and point1
		Vector3D<XNumber> p = this->p1 - this->p0; 
		float l = p.GetLength(); 
		return l;
	}

	template <class XNumber> void LineSegment3D<XNumber>::CalcDirVector(){
		//calculate the direction vector between point0 and point1
		this->dir = this->p1 - this->p0; 
	}

	template <class XNumber> void LineSegment3D<XNumber>::CalcNormalVector(){
		//get the normal vector of dir and normalize it (length = 1)
		this->n = this->dir.GetNormalVector();  
		this->n.Normalize();
	}

	template <class XNumber> float LineSegment3D<XNumber>::GetT(XNumber p0, XNumber d, XNumber p1){
		return (float)(p1 - p0) / (float)d; 
	}

	template <class XNumber> float LineSegment3D<XNumber>::GetS(XNumber p0, XNumber d0, XNumber p1, XNumber d1, float t){
		return (float)(p0 + d0 * t - p1) / (float) d1;
	}

	template <class XNumber> float LineSegment3D<XNumber>::GetPositionOnLine(Vector3D<XNumber> p){
		//point has to be on line, otherwise failes!
		for(int i = 0; i < 3; i++){
			if(this->dir[i] != 0){
				return (p[i] - this->p0[i]) / this->dir[i];
			}
		}
		return -1.0f; 
	}

	template <class XNumber> float LineSegment3D<XNumber>::GetAngleBetween(LineSegment3D<XNumber> LineSegment3D){
		//Vector3D<XNumber> normDir1 = this->dir; 
		Vector3D<XNumber> normDir1((XNumber)this->dir.GetX(), 
																(XNumber)this->dir.GetY(), 
															(XNumber)this->dir.GetZ()); 
		//Vector3D<XNumber> normDir2 = LineSegment3D.GetDirVector(); 
		Vector3D<XNumber> normDir2((XNumber)LineSegment3D.GetDirVector().GetX(), 
																(XNumber)LineSegment3D.GetDirVector().GetY(), 
																 (XNumber)LineSegment3D.GetDirVector().GetZ());  
		normDir1.Normalize(); 
		normDir2.Normalize(); 
		float dot = normDir1.GetDotProduct(normDir2); 
		return acos(dot) * 180 / M_PI;
	}

	template <class XNumber> int LineSegment3D<XNumber>::GetIntersectionPoint(Vector3D<XNumber> &p, LineSegment3D<XNumber> LineSegment3D){
		int result = 0; //intersection on infinite LineSegment3Ds

		float t = (LineSegment3D.p1[0] - LineSegment3D.p0[0]) * (this->p0[1] - LineSegment3D.p0[1]) - 
			(LineSegment3D.p1[1] - LineSegment3D.p0[1]) * (this->p0[0] - LineSegment3D.p0[0]);
		float s = (this->p1[0] - this->p0[0]) * (this->p0[1] - LineSegment3D.p0[1]) - 
			(this->p1[1] - this->p0[1]) * (this->p0[0] - LineSegment3D.p0[0]);
		float u  = (LineSegment3D.p1[1] - LineSegment3D.p0[1]) * (this->p1[0] - this->p0[0]) - 
			(LineSegment3D.p1[0] - LineSegment3D.p0[0]) * (this->p1[1] - this->p0[1]);

		if (u != 0){
			t /= u;
			s /= u;

			if(t >= 0.0f && t <= 1.0f && s >= 0.0f && s <= 1.0f){
				result = 1; //intersection on LineSegment3D segment
			}
			p = this->GetPointOnLine(t); 
		} 
		else{
			if(t == 0 || s == 0){
				result = -1; //coincident
			} 
			else{
				result = -2; //parallel
			}
		}

		return result;
	}
	
	template <class XNumber> LineSegment3D<XNumber> 
					LineSegment3D<XNumber>::GetShortestConnectingLineSegment(LineSegment3D<XNumber> line1)
	{
// 			d1343 = p13.x * p43.x + p13.y * p43.y + p13.z * p43.z;
// 			d4321 = p43.x * p21.x + p43.y * p21.y + p43.z * p21.z;
// 			d1321 = p13.x * p21.x + p13.y * p21.y + p13.z * p21.z;
// 			d4343 = p43.x * p43.x + p43.y * p43.y + p43.z * p43.z;
// 			d2121 = p21.x * p21.x + p21.y * p21.y + p21.z * p21.z;
// 
// 			denom = d2121 * d4343 - d4321 * d4321;
// 			if (ABS(denom) < EPS)
// 					return(FALSE);
// 			numer = d1343 * d4321 - d1321 * d4343;
// 
// 			*mua = numer / denom;
// 			*mub = (d1343 + d4321 * (*mua)) / d4343;
// 
// 			pa->x = p1.x + *mua * p21.x;
// 			pa->y = p1.y + *mua * p21.y;
// 			pa->z = p1.z + *mua * p21.z;
// 			pb->x = p3.x + *mub * p43.x;
// 			pb->y = p3.y + *mub * p43.y;
// 			pb->z = p3.z + *mub * p43.z;
// 
// 			return(TRUE);
			//p13: startDir
			//p21: this->dir
			//p43: line->dir
			
			//d1343: startDir | line->dir    = d21
			//d4321: line->dir | this->dir   = d10
			//d1321: startDir | this->dir    = d20
			//d4343: line->dir | line->dir   = d11
			//d2121: this->dir | this->dir   = d00
			geometry::LineSegment3D<XNumber> line2(this->p0, line1.p0); 
			geometry::LineSegment3D<XNumber> result; 
			XNumber d21, d10, d20, d11, d00; 
			d21 = line2.dir[0] * line1.dir[0] + 
							line2.dir[1] * line1.dir[1] + 
							line2.dir[2] * line1.dir[2]; 
			d10 = line1.dir[0] * this->dir[0] +
							line1.dir[1] * this->dir[1] + 
							line1.dir[2] * this->dir[2]; 
			d20 = line2.dir[0] * this->dir[0] + 
							line2.dir[1] * this->dir[1] + 
							line2.dir[2] * this->dir[2]; 
			d11 = line1.dir[0] * line1.dir[0] + 
							line1.dir[1] * line1.dir[1] + 
							line1.dir[2] * line1.dir[2]; 
			d00 = this->dir[0] * this->dir[0] + 
							this->dir[1] * this->dir[1] + 
							this->dir[2] * this->dir[2]; 
			XNumber denom = d00 * d11 - d10 * d10; 
			XNumber numer = d21 * d10 - d20 * d11; 
			if(fabs(denom) < 0) return result; 
			double dir1 = numer / denom; 
			double dir2 = (d21 + d10 * dir1) / d11; 
			geometry::Vector3D<XNumber> line2p0, line2p1, temp0, temp1; 
			temp0 = (this->dir * dir1); 
			temp1 = (line1.dir * dir2); 
			line2p0 = this->p0 + temp0; 
			line2p1 = line1.p0 + temp1; 
			result.p0 = line2p0; 
			result.p1 = line2p1; 
			return result; 
	}

	template <class XNumber> Vector3D<XNumber> LineSegment3D<XNumber>::GetClosestPoint(Vector3D<XNumber> p){
		geometry::Vector3D<XNumber> A = p - this->p0; 
		geometry::Vector3D<XNumber> B = this->p1 - this->p0; 
		float cosTheta = A.GetDotProduct(B) / (A.GetLength() * B.GetLength()); 
		float connectorPrimeLength = A.GetLength() * cosTheta; 
		float scale = connectorPrimeLength / B.GetLength(); 
		B = B * scale; 
		return this->p0 + B; 
		//float numerator = (p - this->p0).GetDotProduct(this->dir);
		//float denumerator = this->GetNormalizedDirVector().GetLength(); //this->dir.GetLength();
		//return (this->p0 + this->dir * (numerator/denumerator));
	}
	//// Find the cos of the angle between the vectors 
 //  float cosTheta = A.dotProduct(B) / (A.length() * B.length()); 
 //  // Use that to calculate the length of B' 
 //  float BPrimeLength = A.length() * cosTheta; 
 //  // Find the ratio of the length of B' and B 
 //  float scale = BPrimeLength / B.length(); 
 //  // Scale B by that ratio 
 //  B *= scale; 
 //  // Translate p1 by B, this puts it at o 
 //  Ogre::Vector3 C = pt1 + B; 

 //  return C; 

//float numerator = (point - this->pos).dot(this->dir); //this->pos = this->p0
//float denumerator = this->dir.length();
//
//#if COIN_DEBUG
//  if(denumerator == 0.0f)
//    SoDebugError::postWarning("SbLine::getClosestPoint",
//                              "This line has no direction (zero length).");
//#endif // COIN_DEBUG
//
//return (this->pos + this->dir * (numerator/denumerator));

	template <class XNumber> float LineSegment3D<XNumber>::GetDistancePoint(Vector3D<XNumber> p){
		//float dist = (this->p1 - this->p0).GetCrossProduct(p - this->p0); 
		//float dot = (this->p1 - this->p0).GetDotProduct(this->p1 - this->p0);
		//dist /= sqrt(dot); 
		//return abs(dist);
		Vector3D<XNumber> pl = this->GetClosestPoint(p); 
		Vector3D<XNumber> pDir = pl - p; 

		return pDir.GetLength(); 
	}

	template <class XNumber> Vector3D<XNumber> LineSegment3D<XNumber>::GetPointOnLine(float t){
		Vector3D<XNumber> p; 
		p = this->dir * t; 
		p = this->p0 + p; 
		return p; 
	}
	
// 	template <class XNumber> void LineSegment3D<XNumber>::DistRegion0(float &s, 
// 					float &t, float det)
// 	{
// 			float invDet = 1.0f / det; 
// 			s *= invDet; 
// 			t *= invDet; 
// 	}
// 	
// 	template <class XNumber> void LineSegment3D<XNumber>::DistRegion1(float &s, 
// 					float &t, XNumber b, XNumber c, XNumber e)
// 	{ 
// 	}
// 	
// 	template <class XNumber> void LineSegment3D<XNumber>::DistRegion2(float &s, 
// 					float &t, XNumber a, XNumber b, XNumber c, XNumber d, XNumber e, XNumber f)
// 	{
// 	}
// 		
// 	template <class XNumber> XNumber LineSegment3D<XNumber>::GetDistanceLine(LineSegment3D<XNumber> line){
// 			//from: http://www.geometrictools.com/Documentation/DistanceLine3Line3.pdf
// 			Vector3D<XNumber> D = this->p0 - line.p0; 
// 			XNumber a = this->dir.GetDotProduct(this->dir); 
// 			XNumber b = (-this->dir).GetDotProduct(line.dir); 
// 			XNumber c = line.dir.GetDotProduct(line.dir); 
// 			XNumber d = this->dir.GetDotProduct(D); 
// 			XNumber e = (-line.dir).GetDotProduct(D); 
// 			XNumber f = D.GetDotProduct(D); 
// 			float det = a * c - b * b; 
// 			float s = b * e - c * d; 
// 			float t = b * d - a * e; 
// 			if(s >= 0){
// 					if(s <= det){
// 							if(t >= 0){
// 									if(t <= det){
// 											this->DistRegion0(s, t, det); 
// 									}
// 									else{
// 											//region 3
// 									}
// 							}
// 							else{
// 									//region 7
// 							}
// 					}
// 					else{
// 							if(t >= 0){
// 									if(t <= det){
// 											//region 1
// 									}
// 									else{
// 											//region 2
// 									}
// 							}
// 							else{
// 									//region 8
// 							}
// 					}
// 			}
// 			else{
// 					if(t >= 0){
// 							if(t <= det){
// 									//region 5
// 							}
// 							else{
// 									//region 4
// 							}
// 					}
// 					else{
// 							//region 6
// 					}
// 			}
// 	}

	template <class XNumber> XNumber LineSegment3D<XNumber>::GetDistanceLine(LineSegment3D<XNumber> line){
		Vector3D<XNumber> p0 = this->GetPointOnLine(0.5f); 
		Vector3D<XNumber> p1 = line.GetPointOnLine(0.5f); 
		Vector3D<XNumber> p2 = p0 - p1; 
		return p2.GetLength(); 
	}
	
	template <class XNumber> Vector3D<XNumber> LineSegment3D<XNumber>::GetCenterPoint(){
			return this->GetPointOnLine(0.5f); 
	}

	template <class XNumber> void LineSegment3D<XNumber>::MultMatrix(Matrix4x4 m){
		this->p0.MultMatrix(m); 
		this->p1.MultMatrix(m); 
		this->CalcDirVector(); 
		this->CalcNormalVector(); 
	}

}

#endif
