#ifndef LINE_2D_H
#define LINE_2D_H

#include "Vector2D.h"
#include <math.h>

namespace geometry{

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

		Vector2D<float> n; 

		/** Calculates the direction vector (p1 - p0). */
		void CalcDirVector(); 
		/** Calculates a normal vector and normalizes it. */
		void CalcNormalVector(); 
	public:
		enum side{LEFT, RIGHT, EQUAL}; 	
			
		/** Default constructor. */
		LineSegment2D(); 
		/** 
		* Constructor. 
		* @param p0 The first point on the LineSegment2D. 
		* @param p1 The second point on the LineSegment2D. 
		*/
		LineSegment2D(Vector2D<XNumber> p0, Vector2D<XNumber> p1);
		/** Destructor. */
		virtual ~LineSegment2D(); 

		/**
		* Sets the first point on the LineSegment2D. 
		* @param p0 The first point on the LineSegment2D. 
		*/
		void SetPoint0(Vector2D<XNumber> p0); 
		/**
		* Sets the second point on the LineSegment2D. 
		* @param p1 The second point on the LineSegment2D. 
		*/
		void SetPoint1(Vector2D<XNumber> p1); 
		/**
		* Sets a point on the LineSegment2D, 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(Vector2D<XNumber> point, int num); 
		/**
		 * Sets the start and end point of the LineSegment. 
		 * @param p0 Start point. 
		 * @param p1 End point. 
		 */
		void SetValues(Vector2D<XNumber> p0, Vector2D<XNumber> p1); 

		/**
		* Returns the first point on the LineSegment2D. 
		* @return The first point on the LineSegment2D. 
		*/
		Vector2D<XNumber> GetPoint0(); 
		/**
		* Returns the second point on the LineSegment2D. 
		* @return The second point on the LineSegment2D. 
		*/
		Vector2D<XNumber> GetPoint1(); 
		/**
		* Returns a point on the LineSegment2D, 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. 
		*/
		Vector2D<XNumber> GetPoint(int num); 

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

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

		/**
		 * Retrieves the intersection point between two LineSegment2Ds. 
		 * 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 LineSegment2D The LineSegment2D to intersect. 
		 * @return Returns 0 if the two LineSegment2Ds intersect somewhere on the infinite extension of 
		 * the given LineSegment2D segments, 1 if they intersect on the given LineSegment2D segment, -1 if the LineSegment2Ds
		 * are coincident, and -2 if the LineSegment2Ds are parallel. P will only be applied a valid position, 
		 * if the return value is 1. 
		 */
		int GetIntersectionPoint(Vector2D<XNumber> &p, LineSegment2D<XNumber> LineSegment2D);


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

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

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

		bool IsPointOnLine(Vector2D<XNumber> p); 

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

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

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

		/**
		* Indicates whether a given point p is left of the LineSegment2D, on the LineSegment2D or 
		* right of the LineSegment2D. 
		* @param p The point to be tested. 
		* @return Returns a value < 0 if the point is right of the LineSegment2D, 0 if the point
		* is on the LineSegment2D and a value > 0 if the point is left of the LineSegment2D. 
		*/
		float IsLeft(Vector2D<XNumber> p); 
		int GetSideOfLine(Vector2D<XNumber> p); 
		
		Vector2D<XNumber> ProjectPoint(Vector2D<XNumber> p); 

		/**
		 * 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);

		inline const Vector2D<XNumber> & operator[](const int i) const;
		inline Vector2D<XNumber> & operator[](const int i);
		
		
// 		LineSegment2D<XNumber> operator=(LineSegment2D<int> l);
// 		LineSegment2D<XNumber> operator=(LineSegment2D<float> l); 
// 		LineSegment2D<XNumber> operator=(LineSegment2D<double> l);  
	};

	typedef LineSegment2D<int> LineSegment2Di; 
	typedef LineSegment2D<short> LineSegment2Ds; 
	typedef LineSegment2D<long> LineSegment2Dl; 
	typedef LineSegment2D<float> LineSegment2Df; 
	typedef LineSegment2D<double> LineSegment2Dd; 

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

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

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

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

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

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

	template <class XNumber> void LineSegment2D<XNumber>::SetPoint(Vector2D<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> Vector2D<XNumber> LineSegment2D<XNumber>::GetPoint0(){
		return this->p0; 
	}

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

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

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

	template <class XNumber> Vector2D<float>
LineSegment2D<XNumber>::GetNormalVector(){
    this->CalcNormalVector(); 
		return this->n; 
	}

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

	template <class XNumber> void LineSegment2D<XNumber>::Print(){
		printf("LineSegment2D:\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 LineSegment2D<XNumber>::GetLength(){
		//calculate the distance between point0 and point1
		Vector2D<XNumber> p = this->p1 - this->p0; 
		float l = p.GetLength(); 
		return l;
	}

	template <class XNumber> void LineSegment2D<XNumber>::CalcDirVector(){
		////calculate the direction vector between point0 and point1
		//this->dir.SetX((float)(this->p1.GetX() - this->p0.GetX())); 
		//this->dir.SetY((float)(this->p1.GetY() - this->p0.GetY())); 
		this->dir = this->p1 - this->p0; 
	}

	template <class XNumber> void LineSegment2D<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 LineSegment2D<XNumber>::GetT(XNumber p0, XNumber d, XNumber p1){
		return (float)(p1 - p0) / (float)d; 
	}

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

	template <class XNumber> float LineSegment2D<XNumber>::GetAngleBetween(LineSegment2D<XNumber> LineSegment2D){
		Vector2D<XNumber> normDir1 = this->dir; 
		Vector2D<XNumber> normDir2 = LineSegment2D.GetDirVector(); 
		normDir1.Normalize(); 
		normDir2.Normalize(); 
		float dot = normDir1.GetDotProduct(normDir2); 
		return acos(dot) * 180 / M_PI;
	}

	// 1: intersection on line segments
	// 0: intersection on infinite lines
	// -1: co-inicident (p not set)
	// -2: parallel (p not set)
	template <class XNumber> int LineSegment2D<XNumber>::GetIntersectionPoint(Vector2D<XNumber> &p, LineSegment2D<XNumber> LineSegment2D){
		int result = 0; //intersection on infinite LineSegment2Ds

		float t = 0.0f;
		float s = 0.0f;
		float u = 0.0f; 

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

		//if (u != 0){
		if(fabs(u) > std::numeric_limits<float>::epsilon()){
			t /= u;
			s /= u;

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

		return result;
	}

	template <class XNumber> float LineSegment2D<XNumber>::GetDistancePoint(Vector2D<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 fabs(dist); 
	}

	template <class XNumber> bool LineSegment2D<XNumber>::IsPointOnLine(Vector2D<XNumber> p){
			float dist = this->GetDistancePoint(p);
			return (dist < 2 * std::numeric_limits<float>::epsilon());
	}

	template <class XNumber> Vector2D<XNumber> LineSegment2D<XNumber>::GetPointOnLine(float t){
		Vector2D<XNumber> p; 
		//p = this->dir * t;
		float px = (float)(this->dir[0]) * t;
		float py = (float)(this->dir[1]) * t;
		p[0] = (XNumber)px;
		p[1] = (XNumber)py; 
		p = this->p0 + p; 
		return p; 
	}
	
	template <class XNumber> float LineSegment2D<XNumber>::GetPositionOnLine(Vector2D<XNumber> p){
		//point has to be on line, otherwise failes!
		for(int i = 0; i < 2; i++){
			//if(this->dir[i] != 0){
			if(fabs(this->dir[i]) > std::numeric_limits<XNumber>::epsilon()){
				return (float)(p[i] - this->p0[i]) / (float)this->dir[i];
			}
		}
		return -1.0f; 
	}
	
	template <class XNumber> int LineSegment2D<XNumber>::GetSideOfLine(Vector2D<XNumber> p){
			float result = this->IsLeft(p); 
			if(result < 0) return RIGHT; 
			if(result > 0) return LEFT; 
			return EQUAL; 
	}

	template <class XNumber> float LineSegment2D<XNumber>::IsLeft(Vector2D<XNumber> p){
		return ((this->p1.GetX() - this->p0.GetX()) * (p.GetY() - this->p0.GetY())
			- (p.GetX() - this->p0.GetX()) * (this->p1.GetY() - this->p0.GetY()));
	}
	
	template <class XNumber> Vector2D<XNumber> LineSegment2D<XNumber>::ProjectPoint(Vector2D<XNumber> p){
			//m_pt1 + m_line_vector * ((point - m_pt1) * m_line_vector);
			//return this->p0 + this->dir * ((p - this->p0) * this->dir); 
			//return this->p0 + this->dir * ((p - this->p0).GetDotProduct(this->dir)); 
			XNumber u = ((p[0] - this->p0[0]) * (this->p1[0] - this->p0[0]) + 
							(p[1] - this->p0[1]) * (this->p1[1] - this->p0[1])) / 
							fabs(pow((this->p1 - this->p0).GetLength(), 2)); 
			geometry::Vector2D<XNumber> pp = this->p0 + (this->p1 - this->p0) * u; 
			return pp; 
	}

	template <class XNumber> void LineSegment2D<XNumber>::MultMatrix(Matrix4x4 m){
		this->p0.MultMatrix(m); 
		this->p1.MultMatrix(m); 
		this->CalcDirVector(); 
		this->CalcNormalVector(); 
	}
	
// 	template <class XNumber> LineSegment2D< XNumber > 
// 					geometry::LineSegment2D<XNumber>::operator =(LineSegment2D< int > l)
// 	{
// 			this->p0 = l.GetPoint0(); 
// 			this->p1 = l.GetPoint1(); 
// 			return *this; 
// 	}
// 	
// 	template <class XNumber> LineSegment2D< XNumber > 
// 					geometry::LineSegment2D<XNumber>::operator =(LineSegment2D< double > l)
// 	{
// 			this->p0 = l.GetPoint0(); 
// 			this->p1 = l.GetPoint1(); 
// 			return *this; 
// 	}
// 	
// 	template <class XNumber> LineSegment2D< XNumber > 
// 					geometry::LineSegment2D<XNumber>::operator =(LineSegment2D< float > l)
// 	{
// 			this->p0 = l.GetPoint0(); 
// 			this->p1 = l.GetPoint1(); 
// 			return *this; 
// 	}

	template <class XNumber> inline const Vector2D<XNumber> & LineSegment2D<XNumber>::operator[](const int i) const{
			if(i == 0) return this->p0;
			if(i == 1) return this->p1;
			printf("ERROR: LineSegment2D::operator[]: Index %d is not valid (should be 0-2)\n", i);
			return Vector2D<XNumber>(0, 0);
	}

	template <class XNumber> inline Vector2D<XNumber> & LineSegment2D<XNumber>::operator[](const int i){
			if(i == 0) return this->p0;
			if(i == 1) return this->p1;
			printf("ERROR: LineSegment2D::operator[]: Index %d is not valid (should be 0-2)\n", i);
			return Vector2D<XNumber>(0, 0);
	}

}



#endif
