#ifndef POLYGON_2D_H
#define POLYGON_2D_H

#include "Rectangle2D.h"
#include <vector>
#include <map>

namespace geometry{
	/**
	 * This class represents a 2D polygon consisting of an arbitrary number of
	 * points. Methods are provided to retrieve the bounding rectangle, the centroid,
	 * or to specify whether a point lies within the polygon.
	 * In order to work correctly, some methods require a counter clockwise drawn, convex
	 * polygon.
	 *
	 *      p3<br>
	 *    /    \<br>
	 *  p4     p2<br>
	 *  |       |<br>
	 *  |       |<br>
	 *  p0 --- p1<br>
	 *
	 * @author Manuela Waldner
	 */
	template<class XNumber>
	class Polygon2D{
	private:
		/** Vector of Vector2D points. */
		std::vector<Vector2D<XNumber> > points;
		/** Vector2D of the min point of the bounding rectangle. */
		Vector2D<XNumber> min;
		/** Vector2D of the max point of the bounding rectangle. */
		Vector2D<XNumber> max;

		/**
		 * Updates the bounding box regarding to the incoming point.
		 * If one of the dimensions of the point is smaller min or
		 * greater max, the corresponding point is updated.
		 * @param point The current point in the polygon.
		 */
		void UpdateBoundingBox(Vector2D<XNumber> point);
		/**
		 * Sets the min and max point to the first point in the polygon.
		 * For all further points in the polygon, the UpdateBoundingBox method
		 * is called.
		 */
		void CalcBoundingBox();
	public:

		enum order{CW, CCW, CONCAVE};
		enum combine{OR, AND};
		/**
		 * Default constructor.
		 * Sets min and max to zero.
		 */
		Polygon2D();
		/**
		 * Constructor.
		 * Sets the point vector to the incoming vector.
		 * Calculates the bounding box.
		 * @param points A vector of Vector2D points.
		 */
		Polygon2D(std::vector<Vector2D<XNumber> > points);
    /**
     * Constructor.
     * Constructs the polygon from an incoming rectangle.
     * @param rect A 2D rectangle.
     */
    Polygon2D(Rectangle2D<XNumber> rect);
		/**
		 * Destructor.
		 */
		~Polygon2D();

		/**
		 * Adds a point to the polygon.
		 * The point will be added to the end of the list.
		 * @param point The point to be added.
		 */
		void AddPoint(Vector2D<XNumber> point);

		void AddPoint(XNumber x, XNumber y);
    /**
     * Adds a list of points to the polygon.
     * The points will be added to the end of the list.
     * @param points A std::vector holding the points.
     */
    void AddPoints(std::vector<Vector2D<XNumber> > points);
		/**
		 * Removes a given point from the point list.
		 * @param point The point to be removed.
		 * @return Returns true if the given point was found
		 * and removed, otherwise false.
		 */
		bool RemovePoint(Vector2D<XNumber> point);
		/**
		 * Removes a point, given by an index, from the point list.
		 * @param index The index of the point to be removed.
		 * @return Returns true if the given point was found
		 * and removed, otherwise false.
		 */
		bool RemovePoint(int index);
    /**
     * Clears the polygon.
     */
    void RemoveAllPoints();
		/**
		 * Returns the points in the polygon.
		 * @return The point list in the polygon.
		 */
		std::vector<Vector2D<XNumber> > GetPoints();
		/**
		 * Returns a point specified by an index.
		 * @param index The index of the desired point.
		 * @return Returns the point of the list or 0, if the index is
		 * invalid.
		 */
		Vector2D<XNumber> GetPoint(int index);

    /**
     * Sets a point on a given index.
     * The incoming point is exchanged with the existing
     * point at index num.
     * @param point The point to be set.
     * @param num The index where to set the point.
     */
    void SetPoint(Vector2D<XNumber> point, int num);
    /**
     * Sets all points of the polygon.
     * This exchanges the whole points vector. The
     * existing points will be discared.
     * @param points The points to be set for the polygon.
     */
    void SetPoints(std::vector<Vector2D<XNumber> > points);
    /**
     * Returns a line segment specified by an index.
     * The line holds the point associated with the
     * same index and the following point.
     * @param index The index of the line segment.
     * @return Returns the line segment. Returns 0 if the
     * index is invalid.
     */
    LineSegment2D<XNumber> GetLine(int index);
		/**
		 * Returns the number of points in the polygon.
		 * @return The size of the polygon list as int.
		 */
		int GetNumPoints();
		/**
		 * Returns the min point of the bounding rectangle.
		 * @return The min point of the bounding rectangle.
		 */
		Vector2D<XNumber> GetMin();
		/**
		 * Returns the max point of the bounding rectangle.
		 * @return The max point of the bounding rectangle.
		 */
		Vector2D<XNumber> GetMax();
		/**
		 * Returns the bounding rectangle of the polygon.
		 * @return The bounding rectangle of the polygon.
		 */
		Rectangle2D<XNumber> GetBoundingRectangle();

		/**
		 * Returns the centroid of the polygon.
		 * @return The centroid of the polygon.
		 */
		Vector2D<XNumber> GetCentroid();
		/**
		 * Retrieves whether an incoming point is inside the polygon.
		 * NOTE: The points have to be added in counter clockwise order
		 * and the polygon has to be convex.
		 * @param The point to be tested.
		 * @return Returns true if the point is inside, otherwise false.
		 */
		bool IsPointInside(Vector2D<XNumber> point, bool clockwise=false);
		bool IsPointInsideOrOnEdge(Vector2D<XNumber> point, bool clockwise=false);
		bool IsPointInsideScanLine(Vector2D<XNumber> point);
		int IsClockwise();
		/**
		 * Calculates the point on the opposite side of the polygon.
		 * A line is created from the previous point through the current
		 * point to an unspecified location. The length of the line is the
		 * maximum distance between two points in the polygon. This line is then
		 * tested against each polygon edge. The intersection point is returned.
		 * @param prev The previous position vector.
		 * @param current The current position vector.
		 * @param intersect [out] If an intersection occurred, the intersection point
		 * will be saved in this vector.
		 * @return Returns true if an intersection was found, otherwise false.
		 */
		bool GetOppositePoint(Vector2D<XNumber> prev, Vector2D<XNumber> current, Vector2D<XNumber> &intersect);
		/**
		 * Retrieves the maximum distance between two arbitrary points on the
		 * polygon.
		 * @param p0 [out] A point on the polygon.
		 * @param p1 [out] The opposite point to p0 which has the maximum distance.
		 * @return Returns the distance between the two points.
		 */
		XNumber GetMaxDistancePoints(Vector2D<XNumber> &p0, Vector2D<XNumber> &p1);

		/**
		 * Calculates a statistical moment on the polygon.
		 * @param p The first moment parameter.
		 * @param q The second moment parameter.
		 * @return The moment result.
		 */
		float GetMoment(int p, int q);
		/**
		 * Calculates a central statistical moment on the polygon.
		 * Calculates the moment regarding to the polygon's centroid.
		 * @param p The first moment parameter.
		 * @param q The second moment parameter.
		 * @return The moment result.
		 */
		float GetCentralMoment(int p, int q);

    float GetDistanceToBorder(Vector2D<XNumber> p);

		std::vector<Vector2D<XNumber> > GetIntersection(LineSegment2D<XNumber> line);
		std::vector<Vector2D<XNumber> > GetIntersection(Polygon2D<XNumber>* poly);
		bool AreEdgesIntersecting(Polygon2D<XNumber> *poly);
		bool IsAreaIntersecting(Polygon2D<XNumber> *poly, bool clockwise=false, bool inClockwise=false);

    /**
     * Combines two convex polygons to a new (concave) one.
     * NOTE: Incoming polygons have to be convex. The resulting
     * polygon is possibly concave.
     * @param poly The polygon to be combined with.
     * @return The resulting polygon.
     */
    Polygon2D<XNumber>* GetCombinedPolygon(Polygon2D<XNumber>* poly);
    /**
     * Sets the polygon points to a combined polygon of the incoming ones.
     * The incoming polygons can be arbitrary, the resulting polygon
     * might be concave and points are in counter-clockwise order.
     * If the current polygon contains any points, these will be removed. <br>
     * TODO: The first point of the first polygon has to lie outside (no
     * overlaps) otherwise the algorithm fails. Has to be fixed. <br>
     * NOTE: If the polygons are not overlapping, polygons might be lost and
     * not added to the resulting polygon.
     * @param polygons Vector of polygons to be combined.
     */
    void GetCombinedPolygon(std::vector<Polygon2D<XNumber>* > polygons,
                            bool clockwise = false, int combine = OR, double distance=std::numeric_limits<XNumber>::epsilon());

    /**
     * Method to be called by GetCombinedPolygon:
     * Given incoming polygons as sources, a startPolygon, a start point and
     * a startIndex, this method recursively tests for intersections and adds
     * new points to the polygon such that the resulting polygon is the concave
     * boundary of the incoming sources.
     * @param sources The polygons to be combined.
     * @param startPolyon The polygon where to start.
     * @param start The starting point.
     * @param startIndex Index indicating the current polygon edge.
     */
    void AddPointsToCombinedPolygon(std::vector<Polygon2D<XNumber>* >sources,
                                    Polygon2D<XNumber>* startPolygon,
                                    Vector2D<XNumber> start,
                                    int startIndex,
																	  bool clockwise,
																	  int combine,
																	  int maxNumPoints, double epsilon);

// 		Polygon2D<XNumber>* GetConvexHull(bool clockwise=false);
// 		int GetNextOnHull(std::vector<Vector2D<XNumber> > points,
// 										 LineSegment2D<XNumber> line, bool clockwise);
		/**
		 * Multiplies the polygon with a given matrix.
		 * All points will be multiplied with the matrix.
		 * @param m The matrix to be applied.
		 */
		void MultMatrix(Matrix4x4 m);

    /**
     * Prints the polygon and all its points.
     */
    void Print();
	};

	typedef Polygon2D<int> Polygon2Di;
	typedef Polygon2D<short> Polygon2Ds;
	typedef Polygon2D<long> Polygon2Dl;
	typedef Polygon2D<float> Polygon2Df;
	typedef Polygon2D<double> Polygon2Dd;

	template <class XNumber> Polygon2D<XNumber>::Polygon2D(){
		this->min.SetValues(0, 0);
		this->max.SetValues(0, 0);
	}

	template <class XNumber> Polygon2D<XNumber>::Polygon2D(std::vector<Vector2D<XNumber> > points){
		this->points = points;
		this->CalcBoundingBox();
	}

  template <class XNumber> Polygon2D<XNumber>::Polygon2D(
      Rectangle2D<XNumber> rect){
    this->points = rect.GetPoints();
    this->CalcBoundingBox();
  }

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

	template <class XNumber> void Polygon2D<XNumber>::CalcBoundingBox(){
		for(int i = 0; i < this->points.size(); i++){
			if(i == 0){
				this->min = this->points.at(i);
				this->max = this->points.at(i);
			}
			else{
				this->UpdateBoundingBox(this->points.at(i));
			}
		}
	}

	template <class XNumber> void Polygon2D<XNumber>::UpdateBoundingBox(Vector2D<XNumber> point){
		if(point[0] < this->min[0]) this->min[0] = point[0];
		if(point[1] < this->min[1]) this->min[1] = point[1];
		if(point[0] > this->max[0]) this->max[0] = point[0];
		if(point[1] > this->max[1]) this->max[1] = point[1];
	}

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

	template <class XNumber> void Polygon2D<XNumber>::AddPoint(Vector2D<XNumber> point){
		this->points.push_back(point);
		if(this->points.size() == 1) this->CalcBoundingBox();
		else this->UpdateBoundingBox(point);
	}

  template <class XNumber> void Polygon2D<XNumber>::AddPoints(
      std::vector<Vector2D<XNumber> > points){
    for(int i = 0; i < points.size(); i++){
      this->AddPoint(points.at(i));
    }
  }

	template <class XNumber> bool Polygon2D<XNumber>::RemovePoint(Vector2D<XNumber> point){
		for(int i = 0; i < this->points.size(); i++){
			if(this->points.at(i)[0] == point[0] && this->points.at(i)[1] == point[1]){
				this->points.erase(this->points.begin() + i);
				this->CalcBoundingBox();
				return true;
			}
		}
		return false;
	}

	template <class XNumber> bool Polygon2D<XNumber>::RemovePoint(int index){
		if(index >= 0 && index < this->points.size()){
			this->points.erase(this->points.begin() + index);
			this->CalcBoundingBox();
			return true;
		}
		return false;
	}

	template <class XNumber> void Polygon2D<XNumber>::RemoveAllPoints(){
			this->points.clear();
	}

	template <class XNumber> int Polygon2D<XNumber>::GetNumPoints(){
		return this->points.size();
	}

	template <class XNumber> std::vector<Vector2D<XNumber> > Polygon2D<XNumber>::GetPoints(){
		return this->points;
	}

	template <class XNumber> Vector2D<XNumber> Polygon2D<XNumber>::GetPoint(int index){
		Vector2D<XNumber> p;
		if(index >= 0 && index < this->points.size()){
			return this->points.at(index);
		}
		return p;
	}

  template <class XNumber> void Polygon2D<XNumber>::SetPoint(Vector2D<XNumber>
                                                            point, int num){
    if(num >= 0 && num < this->points.size()){
      this->points.at(num) = point;
    }
  }

  template <class XNumber> void Polygon2D<XNumber>::SetPoints(
      std::vector<Vector2D<XNumber> > points){
    this->points = points;
  }

  template <class XNumber> LineSegment2D<XNumber> Polygon2D<XNumber>::GetLine(
      int index){
    //if(index < 0 || index >= this->points.size()) return 0;
    int end = index + 1;
    if(end >= this->points.size()) end = 0;
    return LineSegment2D<XNumber>(this->points.at(index),
                                  this->points.at(end));
  }

	template <class XNumber> Vector2D<XNumber> Polygon2D<XNumber>::GetMin(){
		return this->min;
	}

	template <class XNumber> Vector2D<XNumber> Polygon2D<XNumber>::GetMax(){
		return this->max;
	}

	template <class XNumber> Rectangle2D<XNumber> Polygon2D<XNumber>::GetBoundingRectangle(){
		return Rectangle2D<XNumber>(this->min, this->max);
	}

	template <class XNumber> Vector2D<XNumber> Polygon2D<XNumber>::GetCentroid(){
		Vector2D<XNumber> output(0, 0);
		output[0] = this->GetMoment(1, 0) / this->GetMoment(0, 0);
		output[1] = this->GetMoment(0, 1) / this->GetMoment(0, 0);
		return output;
	}

	template <class XNumber> bool Polygon2D<XNumber>::IsPointInside(Vector2D<XNumber> point, bool clockwise){
		LineSegment2D<XNumber> l;
		int s, e;
		if(this->points.size() > 2){
			for(int i = 0; i < this->points.size(); i++){
				s = i;
				e = i + 1;
				if(e >= this->points.size()) e = 0;
				l.SetValues(points.at(s), points.at(e));
				//if(l.IsLeft(point) <= 0) return false; //!!! CHANGED SOMETHING HERE (=)
				if((!clockwise) && (l.IsLeft(point) <= 0)) return false;
				if((clockwise) && (l.IsLeft(point) >= 0)) return false;
			}
			return true;
		}
		return false;
	}

	template <class XNumber> bool Polygon2D<XNumber>::IsPointInsideOrOnEdge
					(Vector2D<XNumber> point, bool clockwise){
			LineSegment2D<XNumber> l;
			int s, e;
			if(this->points.size() > 2){
					for(int i = 0; i < this->points.size(); i++){
							s = i;
							e = i + 1;
							if(e >= this->points.size()) e = 0;
							l.SetValues(points.at(s), points.at(e));
				//if(l.IsLeft(point) <= 0) return false; //!!! CHANGED SOMETHING HERE (=)
							if((!clockwise) && (l.IsLeft(point) < 0)) return false;
							if((clockwise) && (l.IsLeft(point) > 0)) return false;
					}
					return true;
			}
			return false;
	}

	template <class XNumber> bool Polygon2D<XNumber>::IsPointInsideScanLine
					(Vector2D<XNumber> point)
		{
				this->CalcBoundingBox();
				//create horizontal scanline through the desired point
				XNumber y = point.GetY();
				XNumber x1 = this->min.GetX() - 1;
				XNumber x2 = this->max.GetX() + 1;
				LineSegment2D<XNumber> scanLine(Vector2D<XNumber>(x1, y),
																				Vector2D<XNumber>(x2, y));
				//retrieve intersections of this scanline
				std::vector<Vector2D<XNumber> > intersections =
								this->GetIntersection(scanLine);
				//if there are no intersections, the point must lie outside
				if(intersections.size() <= 0) return false;
				//count the intersections before (smaller x) the point
				float tp = scanLine.GetPositionOnLine(point);
				int numCrossesBefore = 0;
				for(int i = 0; i < intersections.size(); i++){
						float t = scanLine.GetPositionOnLine(intersections.at(i));
						if(t <= tp) numCrossesBefore++;
						//if the intersection point is co-incident with the test-point,
						//the point lies on the border, i.e. within
						//if(intersections.at(i) == point) return true;
						if(t == tp || intersections.at(i).GetX() == point.GetX()){
								return true;
						}
				}
				//if the number of intersections is uneven, the point is inside
				return ((numCrossesBefore % 2) != 0);
		}


	template <class XNumber> bool Polygon2D<XNumber>::GetOppositePoint(Vector2D<XNumber> prev, Vector2D<XNumber> current, Vector2D<XNumber> &intersect){
		Vector2D<XNumber> p0, p1;
		Vector2D<XNumber> dir = current - prev;
		float len = dir.length();
		if(len > 0){
			float maxLen = this->GetMaxDistancePoints(p0, p1);
			float ratio = maxLen / len;
			Vector2D<XNumber> dir1 = dir * ratio;
			Vector2D<XNumber> cp = prev + dir1;
			prev = current + dir;
			LineSegment2D<XNumber> ref(prev, cp);
			LineSegment2D<XNumber> l;
			int s, e;
			for(int i = 0; i < this->points.size(); i++){
				s = i;
				e = i + 1;
				if(e >= this->points.size()) e = 0;
				l.SetValues(points.at(s), points.at(e));
				if(ref.GetIntersectionPoint(intersect, l) == 1){
					intersect = intersect + dir;
					return true;
				}
			}
		}
		return false;
	}

	template <class XNumber> int Polygon2D<XNumber>::IsClockwise(){
			int start = 0;
			int end = 0;
			int numLeft = 0;
			int numRight = 0;
			//if the polygon has less than 3 points it cannot be clockwise
			if(this->points.size() < 3) return false;
			for(int i = 0; i < this->points.size(); i++){
					//create reference line
					LineSegment2D<XNumber> refLine = this->GetLine(i);
					start = i;
					end = i + 1;
					if(end >= this->points.size()) end = 0;
					for(int j = 0; j < this->points.size(); j++){
							if(j != start && j != end){
									//create test point that does not lie on the line
									Vector2D<XNumber> testPoint = this->GetPoint(j);
									//if test point lies left of the reference line,
									//the polygon is not purely clockwise (either counter-clockwise
									//or concave)
									int sideOfLine = refLine.GetSideOfLine(testPoint);
									if(sideOfLine == LineSegment2D<XNumber>::LEFT){
											//return false;
											numLeft++;
									}
									if(sideOfLine == LineSegment2D<XNumber>::RIGHT){
											numRight++;
									}
									if(sideOfLine == LineSegment2D<XNumber>::EQUAL){
											return CONCAVE;
									}
							}
					}
			}
			//if all points lie left of each line segment, the polygon is counter-clockwise
			if(numLeft > 0 && numRight == 0) return CCW;
			//if all points lie right of each line segment, the polygon is clockwise
			if(numRight > 0 && numLeft == 0) return CW;
			//otherwise, the polygon is concave (or even worse...)
			return CONCAVE;
	}

	template <class XNumber> XNumber Polygon2D<XNumber>::GetMaxDistancePoints(Vector2D<XNumber> &p0, Vector2D<XNumber> &p1){
		p0.SetValues(0, 0);
		p1.SetValues(0, 0);
		float maxLen = 0.0f;
		float currentLen = 0.0f;
		if(this->points.size() > 2){
			for(int i = 0; i < this->points.size(); i++){
				Vector2D<XNumber> cp = this->points.at(i);
				if(i < this->points.size() - 1){
					for(int j = i + 1; j < this->points.size(); j++){
						Vector2D<XNumber> diff = cp - this->points.at(j);
						currentLen = diff.GetLength();
						if(currentLen > maxLen){
							maxLen = currentLen;
							p0 = cp;
							p1 = this->points.at(j);
						}
					}
				}
			}
		}
		return maxLen;
	}

	template <class XNumber> float Polygon2D<XNumber>::GetMoment(int p, int q){
		float result = 0.0f;
		for(int i = 0; i < this->points.size(); i++){
			result += pow(this->points.at(i)[0], p) * pow(this->points.at(i)[1], q);
		}
		return result;
	}

	template <class XNumber> float Polygon2D<XNumber>::GetCentralMoment(int p, int q){
		Vector2D<XNumber> c = this->GetCentroid();
		float result = 0.0f;

		for(int i = 0; i < this->points.size(); i++){
			result += pow(this->points.at(i)[0] - c[0], p) * pow(this->points.at(i)[1] - c[1], q);
		}
		return result;
	}

	template <class XNumber> std::vector<Vector2D<XNumber> > Polygon2D<XNumber>::GetIntersection
					(LineSegment2D<XNumber> line)
	{
			std::vector<Vector2D<XNumber> > intersections;
			std::vector<float> intersectionPoint;
			//std::map<float,std::vector<int> > intersectionMap;
			//std::map<float,std::vector<int> >::iterator iter;
			for(int i = 0; i < this->points.size(); i++){
					LineSegment2D<XNumber> edge = this->GetLine(i);
					Vector2D<XNumber> p;
					if(edge.GetIntersectionPoint(p, line) == 1){
							float t = line.GetPositionOnLine(p);
							int pos = intersections.size();
							for(int j = 0; j < intersectionPoint.size(); j++){
									if(intersectionPoint.at(j) > t){
											pos = j;
											break;
									}
							}
							intersections.insert(intersections.begin() + pos, p);
							intersectionPoint.insert(intersectionPoint.begin() + pos, t);

 					}
 			}
			return intersections;
	}

	template <class XNumber> std::vector<Vector2D<XNumber> > Polygon2D<XNumber>::GetIntersection
					(Polygon2D<XNumber>* poly)
	{
			std::vector<Vector2D<XNumber> > intersections;
			for(int i = 0; i < poly->GetNumPoints(); i++){
					LineSegment2D<XNumber> edge = poly->GetLine(i);
					std::vector<Vector2D<XNumber> > edgeIntersections =
									this->GetIntersection(edge);
					for(int j = 0; j < edgeIntersections.size(); j++){
							intersections.push_back(edgeIntersections.at(j));
					}
					edgeIntersections.clear();
			}
			return intersections;
	}

	template <class XNumber> bool Polygon2D<XNumber>::AreEdgesIntersecting(Polygon2D<XNumber> *poly)
	{
			return (this->GetIntersection(poly).size() > 0);
	}

	template <class XNumber> bool Polygon2D<XNumber>::IsAreaIntersecting(Polygon2D<XNumber> *poly,
																																			bool clockwise,
																																			bool inClockwise)
	{
			for(int i = 0; i < this->GetNumPoints(); i++){
					if(poly->IsPointInside(this->GetPoint(i), inClockwise)){
							return true;
					}
			}
			for(int i = 0; i < poly->GetNumPoints(); i++){
					if(this->IsPointInside(poly->GetPoint(i), clockwise)){
							return true;
					}
			}
			return false;
	}

  template <class XNumber> float Polygon2D<XNumber>::GetDistanceToBorder(Vector2D<XNumber> p)
  {
      float minDist = -1.0f;
      for(int i = 0; i < this->GetNumPoints(); i++){
          LineSegment2D<XNumber> line = this->GetLine(i);
          Vector2D<XNumber> projected = line.ProjectPoint(p);
          float t = line.GetPositionOnLine(projected);
          if(t >= 0.0f && t <= 1.0f){
              float dist = (float)p.GetDistance(projected);
              if(minDist < 0 || dist < minDist){
                  minDist = dist;
              }
          }
//           float dist = line.GetDistancePoint(p);
//           if(minDist < 0.0 || dist < minDist){
//               minDist = dist;
//           }
      }
      return minDist;
  }

  template <class XNumber> Polygon2D<XNumber>*
      Polygon2D<XNumber>::GetCombinedPolygon(Polygon2D<XNumber>* poly){
    //the resulting new polygon
    Polygon2D<XNumber>* result = new Polygon2D<XNumber>();
    //the polygon lines of each polygon to intersect
    LineSegment2D<XNumber> l0;
    LineSegment2D<XNumber> l1;
    //intersection point of 2 polygon lines
    Vector2D<XNumber> intersect;
    //temporary point
    Vector2D<XNumber> p;
    //add all polygon points to the resulting polygon
    result->AddPoints(this->points);
    result->AddPoints(poly->points);

   // result->Print();

    //check each polygon lines for intersections
    for(int i = 0; i < this->points.size(); i++){
      l0 = this->GetLine(i);
      for(int j = 0; j < poly->points.size(); j++){
        l1 = poly->GetLine(j);
        //add intersection point to polygon
        if(l0.GetIntersectionPoint(intersect, l1) == 1){
          result->AddPoint(intersect);
        }
      }
    }

    //result->Print();

    //remove points lying inside the perfect concave polygon
    for(int i = 0; i < result->points.size(); i++){
      p = result->points.at(i);
      //check if point lies within one of the two polygons
      if(this->IsPointInside(p) || poly->IsPointInside(p)){
        result->RemovePoint(i);
        i--;
      }
    }

   // result->Print();

    return result;
  }

  template <class XNumber> void
      Polygon2D<XNumber>::GetCombinedPolygon(std::vector<Polygon2D<XNumber>* >
					                                   polygons, bool clockwise, int combine, double epsilon){

      if(polygons.size() == 1){
          this->points = polygons.at(0)->GetPoints();
          return;
      }

    //empty polygon
    this->points.clear();

		//determine start polygon, start vertex, and start vertex index
		geometry::Polygon2D<XNumber> * startPoly;
		geometry::Vector2D<XNumber> startPoint;
		int startIndex = -1;

		//step through all polygons
		for(int i = 0; i < polygons.size(); i++){
				startPoly = polygons.at(i);
				//step through all polygon's vertices
				for(int j = 0; j < startPoly->GetNumPoints(); j++){
						startPoint = startPoly->GetPoint(j);
						//step through all other polygons
						for(int k = 0; k < polygons.size(); k++){
								geometry::Polygon2D<XNumber> * cmpPoly = polygons.at(k);
								if(startPoly != cmpPoly){
										printf("Combine polygons: check point %d of polygon %d with poly %d\n",
													 j, i, k);
										//check if point lies inside any other polygon
										bool inside = cmpPoly->IsPointInsideScanLine(startPoint);
										//if OR combination: point has to lie outside
										//if AND combination: point has to lie inside
										if((combine == OR && !inside) ||
										   (combine == AND && inside))
										{
												printf("Found start point %d\n", j);
												startIndex = j;
												break;
										}
								}
						}
						if(startIndex >= 0) break;
				}
				if(startIndex >= 0) break;
		}

    //in the union case we have the problem that points do not necessarily lie within the other polygon
    if(combine == AND && startIndex < 0){
        for(int i = 0; i < polygons.size(); i++){
            Polygon2D<XNumber> * poly1 = polygons.at(i);
            for(int li = 0; li < poly1->GetNumPoints(); li++){
                LineSegment2D<XNumber> line1 =  poly1->GetLine(li);
                for(int j = i; j < polygons.size(); j++){
                    Polygon2D<XNumber> * poly2 = polygons.at(j);
                    for(int lj = 0; lj < poly2->GetNumPoints(); lj++){
                        LineSegment2D<XNumber> line2 = poly2->GetLine(lj);
                        if(line1.GetIntersectionPoint(startPoint, line2) == 1){
                            printf("Found intersection point at poly %d-%d at index %d-%d\n",
                                   i, j, li, lj);
                            startPoly = poly2;
                            startIndex = lj;
                            break;
                        }
                    }
                    if(startIndex >= 0) break;
                }
                if(startIndex >= 0) break;
            }
            if(startIndex >= 0) break;
        }
    }

		int maxSize = 0;
		for(int i = 0; i < polygons.size(); i++){
				maxSize += polygons.at(i)->GetNumPoints();
		}
    //take one more as we erase the last point in the end
    maxSize++;

		//a valid starting point was found
		if(startIndex >= 0){
				//start iterative logical polygon combination
				this->AddPointsToCombinedPolygon(polygons, startPoly,
																				 startPoint, startIndex, clockwise,
																				 combine, maxSize, epsilon);
				//if a combined polygon was created, delete last point
				//(should be equal to the first)
				if(this->GetNumPoints() > 0){
          this->RemovePoint(this->GetNumPoints() - 1);
        }
		}


		//re-calculate bounding box
		this->CalcBoundingBox();
  }

  template <class XNumber> void Polygon2D<XNumber>::
      AddPointsToCombinedPolygon(
      std::vector<Polygon2D<XNumber>* >  sources,
      Polygon2D<XNumber>* startPolygon,
      Vector2D<XNumber> start, int startIndex,
			bool clockwise, int combine, int maxNumPoints, double epsilon)
	{

// 		int maxSize = 0;
// 		for(int i = 0; i < sources.size(); i++){
// 				maxSize += sources.at(i)->GetNumPoints();
// 		}
// 		if(this->GetNumPoints() > maxSize){
// 				printf("Combining polygons...something went wrong...");
// 				return;
// 		}
			if(this->GetNumPoints() > maxNumPoints){
					printf("Resulting polygon has more entries than sources --> something went wrong\n");
					return;
			}

    if(this->GetNumPoints() == 0 || this->GetNumPoints() == 1 ||
       //!(this->GetPoint(this->GetNumPoints() - 1) == this->GetPoint(0)))
       (this->GetPoint(this->GetNumPoints()-1).GetDistance(this->GetPoint(0)) > epsilon))
    {
			printf("Polygon2D: Add point: ");
			start.Print();
      //start point has to lie on polygon
      this->AddPoint(start);
      //get end point of this line segment
      int end = startIndex + 1;
      if(end >= startPolygon->GetNumPoints()) end = 0;

      //create the two lines to intersect
      LineSegment2D<XNumber> l0;
      LineSegment2D<XNumber> l1;
      //set main line: from incoming start point to end point
      l0.SetPoint0(start);
			Vector2D<XNumber> endPoint = startPolygon->GetPoint(end);
      l0.SetPoint1(endPoint);
      //intersection point of 2 polygon lines
      Vector2D<XNumber> intersect;

      //step through polygons
      for(int i = 0; i < sources.size(); i++){
        //just check other polygons
        if(sources.at(i) != startPolygon){
          //step through all
          for(int j = 0; j < sources.at(i)->GetNumPoints(); j++){
            //create second line
            l1 = sources.at(i)->GetLine(j);
            //add intersection point to polygon
            if(l0.GetIntersectionPoint(intersect, l1) == 1){
              //if(!(intersect == this->GetPoint(this->GetNumPoints() - 1))){
                //if(l1.IsLeft(start) < 0){
								if((combine == OR && !clockwise && l1.IsLeft(start) < 0) ||
									 (combine == OR && clockwise && l1.IsLeft(start) > 0) ||
									 (combine == AND && !clockwise && l1.IsLeft(start) > 0 ) ||
									 (combine == AND && clockwise && l1.IsLeft(start) < 0))
								{
                    printf("Combining polygon: intersected width edge %d from poly %d\n",
                           j, i);
										// check if distance to start point or end point is too small:
										// tangent rather than intersection --> proceed on original polygon
                    if(start.GetDistance(intersect) > epsilon &&
											 endPoint.GetDistance(intersect) > epsilon)
										{
                        this->AddPointsToCombinedPolygon(sources,
                                sources.at(i), intersect, j, clockwise, combine, maxNumPoints, epsilon);
                    }
                    else{
                        printf("Distance between start or end and intersect was smaller than %lf\n",
															 epsilon);
                    }
//                     this->AddPointsToCombinedPolygon(sources,
//                             sources.at(i), intersect, j, clockwise, combine, maxNumPoints, epsilon);
                }//if right of line
              //}//
            }//if intersection
          }//end for j (points in polygons)
        }//end if not startPolygon
      }//end for i (polygons)

      //add end of polygon edge if no intersection was found
      this->AddPointsToCombinedPolygon(sources,
            startPolygon, endPoint, end, clockwise, combine, maxNumPoints, epsilon);
    }//end if still valid
  }

// 	template <class XNumber> int Polygon2D<XNumber>::GetNextOnHull(std::vector<Vector2D<XNumber> > points,
// 																																LineSegment2D<XNumber> line,
// 																																bool clockwise)
// 	{
// 			for(int i = 0; i < points; i++){
// 					if(points.at(i) != line.GetPoint0() &&
// 						points.at(i) != line.GetPoint1())
// 					{
// 						int result = line.IsLeft(points.at(i));
// 						bool success = false;
//
// 						if(clockwise){
// 								if(result > 0) success = true;
// 						}
// 						else{
// 								if(result < 0) success = true;
// 						}
//
// 						if(success) return i;
// 					}
// 			}
// 			return -1;
// 	}

// 	template <class XNumber> void Polygon2D<XNumber>::SortConvex(bool clockwise=false){
// 			if(this->points.size() < 3) return;
// 			std::vector<int> indices;
// 			//find point nearest to origin to define as first point
// 			XNumber x = this->points.at(0).GetX();
// 			XNumber y = this->points.at(0).GetY();
// 			int index = 0;
// 			for(int i = 1; i < this->points.size(); i++){
// 					if(this->points.at(i).GetX() <= x &&
// 						this->points.at(i).GetY() <= y)
// 					{
// 							x = this->points.at(i).GetX();
// 							y = this->points.at(i).GetY();
// 							index = i;
// 					}
// 			}
// 			//save first point
// 			indices.push_back(index);
//
// 			std::vector<Polygon2D<XNumber> > hull;
// 			std::vector<Polygon2D<XNumber> > remaining;
//
// 			for(int i = 0; i < this->points.size(); i++){
// 					remaining.push_back(this->points.at(i));
// 			}
//
// 			remaining.erase(remaining.begin() + index);
// 			hull.push_back(this->points.at(index));
//
// 			while(1){
// 					if(remaining.size() == 0)
// 					{
// 							break;
// 					}
// 					Vector2D<XNumber> start = hull.at(hull.size() - 1);
// 					Vector2D<XNumber> end = remaining.at(0);
// 					LineSegment2D<XNumber> line(start, end);
// 					int result = this->GetNextOnHull(remaining, line, clockwise);
// 					if(result == -1){
// 							hull.push_back
// 					}
// 			}
//
// 	}

	template <class XNumber> void Polygon2D<XNumber>::MultMatrix(Matrix4x4 m){
		for(int i = 0; i < this->points.size(); i++){
			this->points.at(i).MultMatrix(m);
		}
	}

  template <class XNumber> void Polygon2D<XNumber>::Print(){
    printf("Polygon2D (%d) :\n", this->GetNumPoints());
    for(int i = 0; i < this->points.size(); i++){
      this->points.at(i).Print();
    }
  }
};


#endif
