#ifndef VECTOR_3D_H
#define VECTOR_3D_H

#include "MatrixHelper.h"
#include "Vector2D.h"
#include <math.h>
#include <iostream>
#include <limits>

namespace geometry{

	/**
	 * Class describing a vector in 3D with x, y and z coordinates. 
	 * 
	 * @author Manuela Waldner
	 */
	template<class XNumber>
	class Vector3D{
	private:
		/** The x coordinate of the point. */
		XNumber x; 
		/** The y coordinate of the point. */
		XNumber y; 
		/** The z coordinate of the point. */
		XNumber z; 
	public:
		/**
		 * Default constructor. 
		 * Sets the coordinates to zero. 
		 */
		Vector3D(); 
		/**
		 * Constructor. 
		 * Creates a point defined by x, y and z coordinate. 
		 * @param x The x coordinate. 
		 * @param y The y coordinate. 
		 * @param z The z coordinate. 
		 */
		Vector3D(XNumber x, XNumber y, XNumber z); 
		/** Destructor. */
		virtual ~Vector3D(); 

		/**
		 * Sets the x coordinate. 
		 * @param x The x coordinate. 
		 */
		void SetX(XNumber x); 
		/**
		 * Sets the y coordinate. 
		 * @param y The y coordinate. 
		 */
		void SetY(XNumber y); 
		/**
		 * Sets the z coordinate. 
		 * @param z The z coordinate. 
		 */
		void SetZ(XNumber z);

		/**
		 * Sets the x, y, and z coordinate. 
		 * @param x The x coordinate. 
		 * @param y The y coordinate. 
		 * @param z The z coordinate. 
		 */
		void SetValues(XNumber x, XNumber y, XNumber z); 
		
		/**
		 * Returns the x coordiante. 
		 * @return The x coordinate.
		 */
		XNumber GetX(); 
		/**
		 * Returns the y coordinate. 
		 * @return The y coordinate.
		 */
		XNumber GetY(); 
		/**
		 * Returns the z coordinate. 
		 * @return The z coordinate.
		 */
		XNumber GetZ();
    
    Vector2D<XNumber> Get2DVector(); 

		/** Prints the vector to the console. */
		void Print(); 

		/**
		 *   Overloads the unary - operator. 
		 *   Negates the whole vector. 
		 * @return The -vector. 
		 */
		Vector3D<XNumber> operator-(); 
		/**
		 * Overloads the - operator. 
		 * Subtracts two points 
		 * @param p The point to subtract. 
		 * @return The resulting point. 
		 */
		Vector3D<XNumber> operator-(Vector3D<XNumber> p); 
		/**
		 * Overloads the - operator. 
		 * Subtracts a scalar from the point. 
		 * @param s The scalar to subtract. 
		 * @return The resulting point. 
		 */
		Vector3D<XNumber> operator-(XNumber s); 
		/**
		 * Overloads the + operator. 
		 * Adds two points. 
		 * @param p The point to add. 
		 * @return The resulting point. 
		 */
		Vector3D<XNumber> operator+(Vector3D<XNumber> p); 
		/**
		 * Overloads the + operator. 
		 * Adds a scalar to the point. 
		 * @param s The scalar to add. 
		 * @return The resulting point. 
		 */
		Vector3D<XNumber> operator+(XNumber s); 
		/**
		 * Overloads the / operator. 
		 * Divides the vector by a scalar. 
		 * @param s The scalar to divide. 
		 * @return The resulting point. 
		 */
		Vector3D<XNumber> operator/(XNumber s); 
		/**
		 * Overloads the * operator. 
		 * Multiplies the point with a scalar
		 * @param s The scalar to multiply. 
		 * @return The resulting point. 
		 */
		Vector3D<XNumber> operator*(XNumber s); 
		/**
		 * Overloads the index operator. 
		 * Returns x for 0, y for 1 and z for 2. 
		 * @param i The index that should be returned. 
		 * @return Returns the value at the certain instance. 
		 */
		inline XNumber & operator[](const int i); 
		inline const XNumber & operator[](const int i) const; 
		bool operator==(Vector3D<XNumber> p); 
		Vector3D<XNumber> operator=(Vector3D<int> p); 
		Vector3D<XNumber> operator=(Vector3D<double> p); 
		Vector3D<XNumber> operator=(Vector3D<float> p); 
		void MultMatrix(Matrix4x4 m); 
		XNumber GetDotProduct(Vector3D<XNumber> p); 
		/**
		 * Calculates the cross product between two vectors. 
		 * @param p The incoming vector. 
		 * @return Returns the result of the cross product. 
		 */
		Vector3D<XNumber> GetCrossProduct(Vector3D<XNumber> p);

		/**
		 * Normalizes the current vector.  
		 */
		Vector3D<XNumber> GetNormalVector();
		/**
		 * Calculates the length of a vector but does not make a square root (for comparing).  
		 * @return The length of the vector as float value. 
		 */
		float GetQuadLength(); 
		/**
		 * Calculates the length of a vector.  
		 * @return The length of the vector as float value. 
		 */
		float GetLength();

		void Normalize(); 
		//Vector3D<XNumber> GetCross(Vector3D<XNumber> p); 
	};

	typedef Vector3D<int> Vector3Di; 
	typedef Vector3D<short> Vector3Ds; 
	typedef Vector3D<long> Vector3Dl; 
	typedef Vector3D<float> Vector3Df; 
	typedef Vector3D<double> Vector3Dd; 

	template <class XNumber> Vector3D<XNumber>::Vector3D(){
		this->x = 0; 
		this->y = 0; 
		this->z = 0; 
	}

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

	template <class XNumber> Vector3D<XNumber>::Vector3D(XNumber x, XNumber y, XNumber z){
		this->x = x; 
		this->y = y; 
		this->z = z; 
	}

	template <class XNumber> void Vector3D<XNumber>::SetX(XNumber x){
		this->x = x; 
	}

	template <class XNumber> void Vector3D<XNumber>::SetY(XNumber y){
		this->y = y; 
	}

	template <class XNumber> void Vector3D<XNumber>::SetZ(XNumber z){
		this->z = z; 
	}

	template <class XNumber> void Vector3D<XNumber>::SetValues(XNumber x, XNumber y, XNumber z){
		this->x = x; 
		this->y = y; 
		this->z = z; 
	}

	template <class XNumber> XNumber Vector3D<XNumber>::GetX(){
		return this->x; 
	}

	template <class XNumber> XNumber Vector3D<XNumber>::GetY(){
		return this->y; 
	}

	template <class XNumber> XNumber Vector3D<XNumber>::GetZ(){
		return this->z; 
	}
  
  template <class XNumber> Vector2D<XNumber>
      Vector3D<XNumber>::Get2DVector(){
    Vector2D<XNumber> output; 
    output.SetX(this->x); 
    output.SetY(this->y);
    return output; 
  }

	template <class XNumber> void Vector3D<XNumber>::Print(){
		std::cout << "\tVector3D: " << this->x << "\t" << this->y << "\t" << this->z << std::endl; 
		//printf("POINT(F): (%f / %f / %f) | ", this->x, this->y, this->z); 
		//printf("(I): (%d / %d / %d) \n", this->x, this->y, this->z); 
	}
	
	template <class XNumber> Vector3D<XNumber> Vector3D<XNumber>::operator-(){
			Vector3D<XNumber> output; 
			output.SetX(-this->x); 
			output.SetY(-this->y); 
			output.SetZ(-this->z); 
			return output; 
	}

	template <class XNumber> Vector3D<XNumber> Vector3D<XNumber>::operator-(Vector3D<XNumber> p){
		Vector3D<XNumber> output; 
		output.SetX(this->x - p.GetX()); 
		output.SetY(this->y - p.GetY()); 
		output.SetZ(this->z - p.GetZ()); 
		return output; 
	}

	template <class XNumber> Vector3D<XNumber> Vector3D<XNumber>::operator-(XNumber s){
		Vector3D<XNumber> output; 
		output.SetX(this->x - s); 
		output.SetY(this->y - s); 
		output.SetZ(this->z - s); 
		return output; 
	}

	template <class XNumber> Vector3D<XNumber> Vector3D<XNumber>::operator+(Vector3D<XNumber> p){
		Vector3D<XNumber> output; 
		output.SetX(this->x + p.GetX()); 
		output.SetY(this->y + p.GetY()); 
		output.SetZ(this->z + p.GetZ()); 
		return output; 
	}

	template <class XNumber> Vector3D<XNumber> Vector3D<XNumber>::operator+(XNumber s){
		Vector3D<XNumber> output; 
		output.SetX(this->x + s); 
		output.SetY(this->y + s); 
		output.SetZ(this->z + s); 
		return output; 
	}

	template <class XNumber> Vector3D<XNumber> Vector3D<XNumber>::operator/(XNumber s){
		Vector3D<XNumber> output; 
		output.SetX(this->x / (float)s); 
		output.SetY(this->y / (float)s); 
		output.SetZ(this->z / (float)s); 
		return output; 
	}

	template <class XNumber> Vector3D<XNumber> Vector3D<XNumber>::operator*(XNumber s){
		Vector3D<XNumber> output; 
		output.SetX(this->x * s); 
		output.SetY(this->y * s); 
		output.SetZ(this->z * s); 
		return output; 
	}

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

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

	template <class XNumber> bool Vector3D<XNumber>::operator==(Vector3D<XNumber> p){
// 		if(this->x == p.x && this->y == p.y && this->z == p.z) return true; 
// 		return false; 
			return ((fabs(p[0] - this->x) < std::numeric_limits<XNumber>::epsilon()) && 
							(fabs(p[1] - this->y) < std::numeric_limits<XNumber>::epsilon()) && 
						  (fabs(p[2] - this->z) < std::numeric_limits<XNumber>::epsilon())); 
	}
	
	template <class XNumber> Vector3D<XNumber> Vector3D<XNumber>::operator=(Vector3D<int> p){
		this->x = (XNumber)p.GetX(); 
		this->y = (XNumber)p.GetY(); 
		this->z = (XNumber)p.GetZ(); 
		return *this; 
	}
	
	template <class XNumber> Vector3D<XNumber> Vector3D<XNumber>::operator=(Vector3D<float> p){
		this->x = (XNumber)p.GetX(); 
		this->y = (XNumber)p.GetY(); 
		this->z = (XNumber)p.GetZ(); 
		return *this; 
	}
	
	template <class XNumber> Vector3D<XNumber> Vector3D<XNumber>::operator=(Vector3D<double> p){
		this->x = (XNumber)p.GetX(); 
		this->y = (XNumber)p.GetY(); 
		this->z = (XNumber)p.GetZ(); 
		return *this; 
	}

	template <class XNumber> void Vector3D<XNumber>::MultMatrix(Matrix4x4 m){
		float x = m[0] * this->x + m[4] * this->y + m[8] * this->z + m[12]; 
		float y = m[1] * this->x + m[5] * this->y + m[9] * this->z + m[13];
		float z = m[2] * this->x + m[6] * this->y + m[10] * this->z + m[14]; 
		float w = m[3] * this->x + m[7] * this->y + m[11] * this->z + m[15]; 

		this->x = x / w; 
		this->y = y / w; 
		this->z = z / w; 
	}

	template <class XNumber> XNumber Vector3D<XNumber>::GetDotProduct(Vector3D<XNumber> p){
		return this->x * p.GetX() + this->y * p.GetY() + this->z * p.GetZ(); 
	}

	template <class XNumber> Vector3D<XNumber> Vector3D<XNumber>::GetCrossProduct(Vector3D<XNumber> p){
		Vector3D<XNumber> output; 
		output.SetX(((this->y * p.GetZ()) - (this->z * p.GetY())));
		output.SetY(((this->z * p.GetX()) - (this->x * p.GetZ())));
		output.SetZ(((this->x * p.GetY()) - (this->y * p.GetX())));
		return output;
	}

	template <class XNumber> Vector3D<XNumber> Vector3D<XNumber>::GetNormalVector(){
// 		Vector3D<XNumber> output; 
// 		float mag = this->GetLength(); 		
// 		output = *this / mag;
// 		return output;
    Vector3D<XNumber> output; 
    output.SetX(this->y); 
    output.SetY(-this->x); 
    output.SetZ(this->z); 
    return output; 
	}

	template <class XNumber> float Vector3D<XNumber>::GetQuadLength(){
		return this->x * this->x + this->y * this->y + this->z * this->z; 
	}

	template <class XNumber> float Vector3D<XNumber>::GetLength(){
		return (sqrt(this->GetQuadLength())); 
	}

	template <class XNumber> void Vector3D<XNumber>::Normalize(){
		float div = 1.0f / this->GetLength(); 
		this->x *= div; 
		this->y *= div; 
		this->z *= div; 
	}
};



#endif

