#ifndef VECTOR_2D_H
#define VECTOR_2D_H

#include "MatrixHelper.h"
#include <iostream>
#include <limits>
#include <cstdio>

namespace geometry{
	/**
	 * Class describing a vector in 2D with x and y coordinates.
	 * Provides methods to calculate the cross product, the dot
	 * product, to normalize, to calculate the normal vector and to
	 * get the distance between two points.
	 *
	 * @author Manuela Waldner
	 */
	template<class XNumber>
	class Vector2D{
	private:
		/** The x coordinate of the point. */
		XNumber x;
		/** The y coordinate of the point. */
		XNumber y;
	public:
		/**
		 * Default constructor.
		 * Sets the coordinates to zero.
		 */
		Vector2D();
		/**
		 * Constructor.
		 * Creates a point defined by x and y coordinate.
		 * @param x The x coordinate.
		 * @param y The y coordinate.
		 */
		Vector2D(XNumber x, XNumber y);
		/** Destructor. */
		virtual ~Vector2D();

		/**
		 * 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 x and y coordinate.
		 * @param x The x coordinate.
		 * @param y The y coordinate.
		 */
		void SetValues(XNumber x, XNumber y);

		/**
		 * Returns the x coordiante.
		 * @return The x coordinate.
		 */
		XNumber GetX();
		/**
		 * Returns the y coordinate.
		 * @return The y coordinate.
		 */
		XNumber GetY();

		/**
		 * Overloads the + operator.
		 * Adds two points.
		 * @param p The point to add.
		 * @return The resulting point.
		 */
		Vector2D<XNumber> operator+(Vector2D<XNumber> p);
		/**
		 * Overloads the + operator.
		 * Adds a scalar to the point.
		 * @param s The scalar to add.
		 * @return The resulting point.
		 */
		Vector2D<XNumber> operator+(XNumber s);
		/**
		 * Overloads the - operator.
		 * Subtracts two points
		 * @param p The point to subtract.
		 * @return The resulting point.
		 */
		Vector2D<XNumber> operator-(Vector2D<XNumber> p);
		/**
		 * Overloads the - operator.
		 * Subtracts a scalar from the point.
		 * @param s The scalar to subtract.
		 * @return The resulting point.
		 */
		Vector2D<XNumber> operator-(XNumber s);
		/**
		 * Overloads the * operator.
		 * Multiplies the point with a scalar
		 * @param s The scalar to multiply.
		 * @return The resulting point.
		 */
		Vector2D<XNumber> operator*(XNumber s);
		/**
		 * Overloads the = operator.
		 * Sets the point to a float point and casts it to XNumber.
		 * @param p The point to set.
		 * @return The resulting point.
		 */
		Vector2D<XNumber> operator=(Vector2D<float> p);
		/**
		 * Overloads the = operator.
		 * Sets the point to a int point and casts it to XNumber.
		 * @param p The point to set.
		 * @return The resulting point.
		 */
		Vector2D<XNumber> operator=(Vector2D<int> p);
		Vector2D<XNumber> operator=(Vector2D<double> p);
		inline XNumber & operator[](const int i);
		inline const XNumber & operator[](const int i) const;
    inline const bool operator==(const Vector2D<XNumber> p) const;

		/**
		 * Multiplies the point with a 4x4 matrix.
		 * Just cuts the z-coordinate and divides through w.
		 * @param m The matrix to mulitply.
		 */
		void MultMatrix(Matrix4x4 m);
		/**
		 * Calculates the dot product between two vectors.
		 * @param p The incoming vector.
		 * @return Returns the result of the dot product.
		 */
		XNumber GetDotProduct(Vector2D<XNumber> p);
		/**
		 * Calculates the cross product between two vectors.
		 * @param p The incoming vector.
		 * @return Returns the result of the cross product.
		 */
		XNumber GetCrossProduct(Vector2D<XNumber> p);
		/**
		 * 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();
		/**
		 * Normalizes the current vector.
		 */
		void Normalize();
		/**
		 * Calculates the normal vector (turned to the right).
		 * @return Returns the normal vector.
		 */
		Vector2D<XNumber> GetNormalVector();
		/**
		 * Mirrors the vector.
		 */
		void Mirror();

		/**
		 * Calculates the distance between to points.
		 * @param p The point to get the distance from.
		 * @return Returns the distance as float.
		 */
		float GetDistance(Vector2D<XNumber> p);

		void Interpolate(Vector2D<XNumber> p);

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

	typedef Vector2D<int> Vector2Di;
	typedef Vector2D<short> Vector2Ds;
	typedef Vector2D<long> Vector2Dl;
	typedef Vector2D<float> Vector2Df;
	typedef Vector2D<double> Vector2Dd;

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

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

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

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

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

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

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

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

	template <class XNumber> void Vector2D<XNumber>::Print(){
		std::cout << "\tVector2D: " << this->x << "\t" << this->y << std::endl;
		//printf("POINT(F): %f / %f | ", this->x, this->y);
		//printf("(I): %d / %d\n", this->x, this->y);
	}

	template <class XNumber> void Vector2D<XNumber>::MultMatrix(Matrix4x4 m){
		float x = m[0] * this->x + m[4] * this->y + m[12];
		float y = m[1] * this->x + m[5] * this->y + m[13];
		float w = m[3] * this->x + m[7] * this->y + m[15];

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

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

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

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

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

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

	template <class XNumber> Vector2D<XNumber> Vector2D<XNumber>::operator=(Vector2D<float> p){
		this->x = (XNumber)p.GetX();
		this->y = (XNumber)p.GetY();
		return *this;
	}

	template <class XNumber> Vector2D<XNumber> Vector2D<XNumber>::operator=(Vector2D<int> p){
		this->x = (XNumber)p.GetX();
		this->y = (XNumber)p.GetY();
		return *this;
	}

	template <class XNumber> Vector2D<XNumber> Vector2D<XNumber>::operator=(Vector2D<double> p){
		this->x = (XNumber)p.GetX();
		this->y = (XNumber)p.GetY();
		return *this;
	}

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

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

  template <class XNumber> inline const bool Vector2D<XNumber>::
      operator==(const Vector2D<XNumber> p) const{
//     if(p[0] == this->x && p[1] == this->y) return true;
//     return false;
			bool t = ((fabs(p[0] - this->x) < std::numeric_limits<XNumber>::epsilon()) &&
							  (fabs(p[1] - this->y) < std::numeric_limits<XNumber>::epsilon()));
			return t; 
  }

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

	template <class XNumber> XNumber Vector2D<XNumber>::GetCrossProduct(Vector2D<XNumber> p){
		return this->x * p.GetY() - this->y * p.GetX();
	}

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

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

	template <class XNumber> void Vector2D<XNumber>::Normalize(){
		float div = 1 / this->GetLength();
		this->x *= div;
		this->y *= div;
	}

	template <class XNumber> Vector2D<XNumber> Vector2D<XNumber>::GetNormalVector(){
		Vector2D<XNumber> n;
		n.SetX(this->y);
		n.SetY(-this->x);
		return n;
	}

	template <class XNumber> void Vector2D<XNumber>::Mirror(){
		this->x = -this->x;
		this->y = -this->y;
	}

	template <class XNumber> float Vector2D<XNumber>::GetDistance(Vector2D<XNumber> p){
		Vector2D<XNumber> dist = *this - p;
		return fabs(dist.GetLength());
	}

	template <class XNumber> void Vector2D<XNumber>::Interpolate(Vector2D<XNumber> p){
			this->x = (XNumber)((double)(this->x + p.x) / 2.0f);
			this->y = (XNumber)((double)(this->y + p.y) / 2.0f);
	}


}



#endif

