#ifndef PLANE_H
#define PLANE_H

//#include "Line2D.h"
#include "Vector3D.h"
#include "Line3D.h"

namespace geometry{
	/**
	 * Class representing a plane in 3D space defined either by 3 3D coordinates
	 * or a normal vector with a given distance from origin. 
	 * The plane is not restricted by corner points thus endless. 
	 * There are methods provided to retrieve distances from any point to the specified
	 * plane, to get intersection with a line and to multiply the whole plane with 
	 * a transform matrix. 
	 * 
	 * @author Manuela Waldner
	 */
	template<class XNumber>
	class Plane{
	private:
		/** 3D normal vector of the plane. */
		Vector3D<XNumber> normal; 
		/** Distance from origin of the 3D coordinate system. */
		XNumber distance; 
	public: 
		/**
		 * Default constructor. 
		 */
		Plane(); 
		/**
		 * Constructor defines the plane by 3 incoming coordinates. 
		 * @param p0 The first 3D point. 
		 * @param p1 The second 3D point. 
		 * @param p2 The third 3D point. 
		 */
		Plane(Vector3D<XNumber> p0, Vector3D<XNumber> p1, Vector3D<XNumber> p2); 
		/**
		 * Constructor defines the plane by a normal vector and a distance
		 * to origin. 
		 * @param normal The normal vector of the 3D plane. 
		 * @param distance The distance from the coordinate origin. 
		 */
		Plane(Vector3D<XNumber> normal, XNumber distance); 

		/**
		 * Returns the normal vector of the plane. 
		 * @return The normal vector. 
		 */
		Vector3D<XNumber> GetNormalVector(); 
		/**
		 * Returns the distance from coordinate origin. 
		 * @return The distance from origin. 
		 */
		XNumber GetDistanceOrigin(); 

		/**
		 * Defines an offset of the plane to the origin. 
		 * The given value will be added to the existing distance member. 
		 * @param distance The distance to be added. 
		 */
		void SetOffset(XNumber distance); 
		/**
		 * Retrieves an intersection between the plane and a 3D line. If the return
		 * value is true, the intersection point will be stored in intersection. 
		 * @param line The line to be cut with the plane. 
		 * @param intersection [out] The intersection point, if an intersection occured. 
		 * @return Returns true if an intersection was found, otherwise false. 
		 */
		bool GetLineIntersection(LineSegment3D<XNumber> line, Vector3D<XNumber> &intersection); 
		/**
		 * Multiplies the 3D plane with a given transform matrix. 
		 * @param m The matrix to be applied. 
		 */
		void MultMatrix(Matrix4x4 m); 
		/**
		 * Returns the distance from a given point to the plane.
		 * @param p The point to be measured. 
		 * @return The distance from the point to the plane. 
		 */
		XNumber GetDistance(Vector3D<XNumber> p); 

	}; 

	typedef Plane<int> Planei; 
	typedef Plane<short> Planes; 
	typedef Plane<long> Planel; 
	typedef Plane<float> Planef; 
	typedef Plane<double> Planed; 

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

	template <class XNumber> Plane<XNumber>::Plane(Vector3D<XNumber> normal, XNumber distance){
		if(normal.GetQuadLength() <= 0.0f){ 
			printf("Plane::Plane: Invalid normal vector, length 0\n"); 
			return; 
		}
		this->normal = normal; 
		this->normal.Normalize(); 
		this->distance = distance; 
	}

	template <class XNumber> Plane<XNumber>::Plane(Vector3D<XNumber> p0, Vector3D<XNumber> p1, Vector3D<XNumber> p2){
		if(p0 == p1 || p0 == p2 || p1 == p2){
			printf("Plane::Plane: The 3 points defining the plane are coincident\n"); 
			throw -1; 
		}
		this->normal = (p1 - p0).GetCrossProduct(p2 - p0); 
		if(this->normal.GetQuadLength() <= 0.0f){
			printf("Plane::Plane: The 3 points defining the plane are on a line\n"); 
			throw -1; 
		}
		this->normal.Normalize(); 
		this->distance = this->normal.GetDotProduct(p0); 
	}

	template <class XNumber> Vector3D<XNumber> Plane<XNumber>::GetNormalVector(){
		return this->normal; 
	}

	template <class XNumber> XNumber Plane<XNumber>::GetDistanceOrigin(){
		return this->distance; 
	}

	template <class XNumber> void Plane<XNumber>::SetOffset(XNumber distance){
		this->distance += distance; 
	}

	template <class XNumber> bool Plane<XNumber>::GetLineIntersection(LineSegment3D<XNumber> line, Vector3D<XNumber> &intersection){
		//Vector3D<XNumber> intersection; 
		if(line.GetDirVector().GetQuadLength() <= 0.0f){
			printf("Plane::GetLineIntersection: The incoming line has length 0\n"); 
			return false; 
		}
		if(line.GetDirVector().GetDotProduct(this->normal) == 0.0f) return false; 
		float t = (this->distance - this->normal.GetDotProduct(line.GetPoint0())) / 
			this->normal.GetDotProduct(line.GetDirVector()); 
		//intersection = line.GetPoint0() + t * line.GetDirVector();
		intersection = line.GetPointOnLine(t); 
		return true; 
	}

	template <class XNumber> XNumber Plane<XNumber>::GetDistance(Vector3D<XNumber> p){
		return p.GetDotProduct(this->normal) - this->distance; 
	}

	template <class XNumber> void Plane<XNumber>::MultMatrix(Matrix4x4 m){
		Vector3D<XNumber> pointInPlane = this->normal * this->distance; 
		Matrix4x4 matrix; 
		GetInverseMatrix(matrix, m); 
		GetTransposeMatrix(matrix, matrix); 
		this->normal.MultMatrix(matrix); 
		pointInPlane.MultMatrix(matrix); 
		if(this->normal.GetQuadLength() <= 0.0f){
			printf("Plane::MultMatrix: The normal vector has length 0 now. Invalid. \n"); 
			return; 
		}
		this->normal.Normalize(); 
		this->distance = this->normal.GetDotProduct(pointInPlane); 
	}
};

#endif
