#ifndef RECTANGLE_2D_H
#define RECTANGLE_2D_H

#include "Line2D.h"
#include <vector>

namespace geometry{
	/**
	 * Class representing a Rectangle2D consisting of 4 points. 
	 * Provides methods to read its width and height and to 
	 * test if a point lies within the Rectangle2D or if a line crosses
	 * a Rectangle2D side. 
	 *
	 *  p3 ---------- p2<br>
	 *  |             |<br>
	 *  |             |<br>
	 *  p0 ---------- p1<br>
	 * 
	 * @author Manuela Waldner
	 */
	template<class XNumber>
	class Rectangle2D{
	private:
		/** First point of the Rectangle2D. */
		Vector2D<XNumber> p0; 
		/** Second point of the Rectangle2D. */
		Vector2D<XNumber> p1; 
		/** Third point of the Rectangle2D. */
		Vector2D<XNumber> p2; 
		/** Fourth point of the Rectangle2D. */
		Vector2D<XNumber> p3; 
	public:
		/** Default constructor. */
		Rectangle2D(); 
		/**
		 * Constructor. 
		 * Defines the Rectangle2D by setting all four points. 
		 * @param p0 The first point. 
		 * @param p1 The second point. 
		 * @param p2 The third point. 
		 * @param p3 The fourth point. 
		 */
		Rectangle2D(Vector2D<XNumber> p0, Vector2D<XNumber> p1, Vector2D<XNumber> p2, Vector2D<XNumber> p3);
		/**
		 * Constructor. 
		 * Defines the Rectangle2D by setting the diagonal points. 
		 * Calls SetDiagonal(..). 
		 * @param p0 The first point. 
		 * @param p2 The third point. 
		 */
		Rectangle2D(Vector2D<XNumber> p0, Vector2D<XNumber> p2); 
		/** Destructor. */
		virtual ~Rectangle2D(); 

		/**
		 * Sets the first point of the Rectangle2D. 
		 * @param p0 The first point. 
		 */
		void SetPoint0(Vector2D<XNumber> p0); 
		/**
		 * Sets the second point of the Rectangle2D. 
		 * @param p1 The second point. 
		 */
		void SetPoint1(Vector2D<XNumber> p1); 
		/**
		 * Sets the third point of the Rectangle2D. 
		 * @param p2 The third point. 
		 */
		void SetPoint2(Vector2D<XNumber> p2); 
		/**
		 * Sets the fourth point of the Rectangle2D. 
		 * @param p3 The fourth point. 
		 */
		void SetPoint3(Vector2D<XNumber> p3); 
		/**
		 * Sets a point of the Rectangle2D defined by num. 
		 * @param point The point to set. 
		 * @param num Which point to set. Num must be between 0 and 3, otherwise
		 * an exception is thrown. 
		 */
		void SetPoint(Vector2D<XNumber> point, int num); 
    void SetPoints(std::vector<Vector2D<XNumber> > points); 
		
		/**
		 * Sets the diagonal points of the Rectangle2D. 
		 * P0 and p1 share the same x value, p0 and p3 share the same y value and so on. 
		 * @param p0 The first point. 
		 * @param p2 The third point. 
		 */
		void SetDiagonal(Vector2D<XNumber> p0, Vector2D<XNumber> p2); 

		/**
		 * Returns the first point of the Rectangle2D. 
		 * @return The first point. 
		 */
		Vector2D<XNumber> GetPoint0(); 
		/**
		 * Returns the second point of the Rectangle2D. 
		 * @return The second point. 
		 */
		Vector2D<XNumber> GetPoint1(); 
		/**
		 * Returns the thrid point of the Rectangle2D. 
		 * @return The third point. 
		 */
		Vector2D<XNumber> GetPoint2(); 
		/**
		 * Returns the fourth point of the Rectangle2D. 
		 * @return The fourth point. 
		 */
		Vector2D<XNumber> GetPoint3(); 
		/**
		 * Returns a point of the Rectangle2D defined by num.  
		 * @param num Which point to return. Num must be between 0 and 3, otherwise
		 * an exception is thrown. 
		 * @return The point defined by num. 
		 */
		Vector2D<XNumber> GetPoint(int num); 
    
    std::vector<Vector2D<XNumber> > GetPoints(); 

		/**
		 * Returns the center of the Rectangle2D as Vector2D. 
		 * @return The center of the Rectangle2D. 
		 */
		Vector2D<XNumber> GetCenter(); 

		/**
		 * Returns the width of the Rectangle2D. 
		 * The width is defined as the length of the vector between p1 and p0. 
		 * @return The width. 
		 */
		XNumber GetWidth(); 
		/**
		 * Returns the height of the Rectangle2D. 
		 * The height is defined as the length of the vector between p3 and p0. 
		 * @return The height. 
		 */
		XNumber GetHeight(); 
		/**
		 * Returns the size of the Rectangle2D. 
		 * The size is defined by the width multiplied with the height. 
		 * @return the size. 
		 */
		XNumber GetSize(); 
		LineSegment2D<XNumber> GetLine(int i); 

		/**
		 * Converts the Rectangle2D in lines and returns them. 
		 * @return The Rectangle2D lines in a vector. 
		 */
		std::vector<LineSegment2D<XNumber> > GetRectangle2DLines(); 
		/**
		 * Returns true, if the incoming line collides with one of the sides of the Rectangle2D. 
		 * @param line The line to test. 
		 * @param invalid Defines an invalid value for setting an invalid collision point. 
		 * @return Returns true, if a collision point was found. 
		 */
		bool IsCollidingWithLine(LineSegment2D<XNumber> line, XNumber invalid); 
		/**
		 * Returns true, if the incoming point is in the Rectangle2D-area. 
		 * @param point The point to test. 
		 * @return Returns true, if the point is in the Rectangle2D. 
		 */
		bool IsPointInRect(Vector2D<XNumber> point); 
		
		void Sort(bool clockwise=false); 

		/** Prints the Rectangle2D. */
		void Print(); 

		/**
		 * Multiplies all four corner points of the face with a given matrix. 
		 * @param m The matrix to be applied. 
		 */
		void MultMatrix(Matrix4x4 m); 

		/**
		 * Returns the corner points as 4x3 array. 
		 * NOTE: Don't forget to delete after use!
		 * @return The corner points as array. 
		 */
		XNumber** GetArray();


		inline const Vector2D<XNumber> & operator[](const int i) const;
		inline Vector2D<XNumber> & operator[](const int i);
	};

	typedef Rectangle2D<int> Rectangle2Di; 
	typedef Rectangle2D<short> Rectangle2Ds; 
	typedef Rectangle2D<long> Rectangle2Dl; 
	typedef Rectangle2D<float> Rectangle2Df; 
	typedef Rectangle2D<double> Rectangle2Dd; 

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

	template <class XNumber> Rectangle2D<XNumber>::Rectangle2D(Vector2D<XNumber> p0, Vector2D<XNumber> p1, Vector2D<XNumber> p2, Vector2D<XNumber> p3){
		this->p0 = p0; 
		this->p1 = p1; 
		this->p2 = p2; 
		this->p3 = p3; 
	}

	template <class XNumber> Rectangle2D<XNumber>::Rectangle2D(Vector2D<XNumber> p0, Vector2D<XNumber> p2){
		this->SetDiagonal(p0, p2); 
	}

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

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

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

	template <class XNumber> void Rectangle2D<XNumber>::SetPoint2(Vector2D<XNumber> p2){
		this->p2 = p2; 
	}

	template <class XNumber> void Rectangle2D<XNumber>::SetPoint3(Vector2D<XNumber> p3){
		this->p3 = p3; 
	}

	template <class XNumber> void Rectangle2D<XNumber>::SetDiagonal(Vector2D<XNumber> p0, Vector2D<XNumber> p2){
		this->p0 = p0; 
		this->p2 = p2; 
		this->p1.SetX(p2.GetX()); 
		this->p1.SetY(p0.GetY()); 
		this->p3.SetX(p0.GetX()); 
		this->p3.SetY(p2.GetY()); 
	}

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

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

	template <class XNumber> Vector2D<XNumber> Rectangle2D<XNumber>::GetPoint2(){
		return this->p2; 
	}

	template <class XNumber> Vector2D<XNumber> Rectangle2D<XNumber>::GetPoint3(){
		return this->p3; 
	}

	template <class XNumber> Vector2D<XNumber> Rectangle2D<XNumber>::GetPoint(int num){
		switch(num){
			case 0:
				return p0; 
			case 1:
				return p1; 
			case 2:
				return p2; 
			case 3: 
				return p3; 
			default: 
				throw -1; 
		}
	}
  
  template <class XNumber> std::vector<Vector2D<XNumber> >
        Rectangle2D<XNumber>::GetPoints(){
    std::vector<Vector2D<XNumber> > points; 
    points.push_back(p0); 
    points.push_back(p1); 
    points.push_back(p2); 
    points.push_back(p3); 
    return points; 
  }

	template <class XNumber> void Rectangle2D<XNumber>::SetPoint(Vector2D<XNumber> point, int num){
		switch(num){
			case 0:
				this->p0 = point; 
				break;
			case 1:
				this->p1 = point; 
				break;
			case 2:
				this->p2 = point; 
				break;
			case 3: 
				this->p3 = point; 
				break;
			default: 
				throw -1; 
		}
	}
  
  template <class XNumber> void Rectangle2D<XNumber>::SetPoints(
      std::vector<Vector2D<XNumber> > points){
    if(points.size() == 4){
      this->p0 = points.at(0); 
      this->p1 = points.at(1); 
      this->p2 = points.at(2); 
      this->p3 = points.at(3); 
    }
  }

	template <class XNumber> Vector2D<XNumber> Rectangle2D<XNumber>::GetCenter(){
		Vector2D<XNumber> center; 
		center.SetX(p0.GetX() + (p1.GetX() - p0.GetX()) / 2); 
		center.SetY(p0.GetY() + (p3.GetY() - p0.GetY()) / 2); 
		return center; 
	}

	template <class XNumber> XNumber Rectangle2D<XNumber>::GetWidth(){
		return p1.GetX() - p0.GetX(); 
	}

	template <class XNumber> XNumber Rectangle2D<XNumber>::GetHeight(){
		return p3.GetY() - p0.GetY(); 
	}

	template <class XNumber> XNumber Rectangle2D<XNumber>::GetSize(){
		return this->GetWidth() * this->GetHeight(); 
	}

	template <class XNumber> void Rectangle2D<XNumber>::Print(){
		printf("Rectangle2D (%d):\n", this->GetSize()); 
		p0.Print(); 
		p1.Print(); 
		p2.Print(); 
		p3.Print(); 
	}

	template <class XNumber> std::vector<LineSegment2D<XNumber> > Rectangle2D<XNumber>::GetRectangle2DLines(){
		std::vector<LineSegment2D<XNumber> > rectLines; 

		//calculate lines to connect the rect-points
		for(int i = 1; i < 4; i++){
			LineSegment2D<XNumber> l(this->GetPoint(i - 1), this->GetPoint(i)); 
			rectLines.push_back(l); 
		}
		LineSegment2D<XNumber> l(this->p3, this->p0); 
		rectLines.push_back(l); 
		return rectLines; 
	}

	template <class XNumber> LineSegment2D<XNumber> Rectangle2D<XNumber>::GetLine(int i){
		LineSegment2D<XNumber> line; 
		if(i >= 0 && i < 4){
			std::vector<LineSegment2D<XNumber> > rectLines = this->GetRectangle2DLines(); 
			line = rectLines.at(i); 
		}
		return line; 
	}

	template <class XNumber> bool Rectangle2D<XNumber>::IsCollidingWithLine(LineSegment2D<XNumber> line, XNumber invalid){
		std::vector<LineSegment2D<XNumber> > rectLines = this->GetRectangle2DLines(); 

		float sl; //temp variable
		//step through all Rectangle2D lines
		for(int i = 0; i < rectLines.size(); i++){
			//test each Rectangle2D line with the incoming line
			Vector2D<int> s = rectLines.at(i).GetCollisionPointLines(line, invalid, sl);
			//if a collision point was found, return true
			if(s.GetX() != invalid){
				return true; 
			}
		}
		return false; 
	}

	template <class XNumber> bool Rectangle2D<XNumber>::IsPointInRect(Vector2D<XNumber> point){
		//compare the incoming point's coordinates with the diagonal coordinates. 
		//return(point.GetX() > p0.GetX() && point.GetX() < p2.GetX() && point.GetY() > p0.GetY() && point.GetY() < p2.GetY()); 
		std::vector<LineSegment2D<XNumber> > rectLines =
this->GetRectangle2DLines(); 
		for(int i = 0; i < 4; i++){
			if(rectLines.at(i).IsLeft(point) <= 0)return false; 
		}
		return true; 
	}

	template <class XNumber> XNumber** Rectangle2D<XNumber>::GetArray(){
		XNumber** arr = new XNumber*[4]; 
		for(int i = 0; i < 4; i++){
			arr[i] = new XNumber[2]; 
			for(int j = 0; j < 2; j++){
				arr[i][j] = this->points.at(i)[j]; 
			}
		}
		return arr; 
	}

	template <class XNumber> void Rectangle2D<XNumber>::MultMatrix(Matrix4x4 m){
		p0.MultMatrix(m); 
		p1.MultMatrix(m); 
		p2.MultMatrix(m); 
		p3.MultMatrix(m); 
	}
	
	template <class XNumber> void Rectangle2D<XNumber>::Sort(bool clockwise){
			Vector2D<XNumber> center = this->GetCenter(); 
			//set first point
			for(int i = 0; i < 4; i++){
					Vector2D<XNumber> p = this->GetPoint(i); 
					if(p.GetX() < center.GetX() && p.GetY() < center.GetY()){
							if(i != 0){
							 this->SetPoint(this->GetPoint(0), i); 
							 this->SetPoint(p, 0); 
							}
							break;
					}
			}
			for(int i = 0; i < 4; i++){
					int j = i + 1; 
					if(j >= 4) j = 0; 
					Vector2D<XNumber> p0 = this->GetPoint(i); 
					bool exchange = true; 
					while(exchange){
							exchange = false; 
					    Vector2D<XNumber> p1 = this->GetPoint(j); 
							LineSegment2D<XNumber> line(p0, p1); 
							for(int k = i + 1; k < 4; k++){
									Vector2D<XNumber> test = this->GetPoint(k); 
									int lineSide = line.GetSideOfLine(test); 
									if((lineSide == LineSegment2D<XNumber>::RIGHT && clockwise) || 
											lineSide == LineSegment2D<XNumber>::LEFT && !clockwise)
									{
											this->SetPoint(test, j); 
											this->SetPoint(p1, k); 
											exchange = true; 
											break;
									}
							}
					}
			}
	}

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

	template <class XNumber> inline Vector2D<XNumber> & Rectangle2D<XNumber>::operator[](const int i){
			if(i == 0) return this->p0;
			if(i == 1) return this->p1;
			if(i == 2) return this->p2;
			if(i == 3) return this->p3;
			// get a seg-fault somewhere here, but this is not nice at all! 
			return this->p0; 
// 			printf("ERROR: Rectangle2D::operator[]: Index %d is not valid (should be 0-3)\n", i);
// 			return Vector2D<XNumber>(0, 0); 
	}


};

#endif
