#include "Polygon.h"
#include "Line.h"
#include <list>
#include <math.h>


float Polygon::signedArea() const {
  float d = 0.0;
  
  for(const_iterator it = begin(); it != end(); it++) {
    const_iterator it2 = it+1;
    if(it2 == end()) it2 = begin();
    
    d += (it2->getX()+it->getX())*(it2->getY()-it->getY());
  }
  
  return d/2;
}

float Polygon::area() const {
  return fabsf(signedArea());
}

float Polygon::perimeter() const {
  float d = 0.0;
  
  for(const_iterator it = begin(); it != end(); it++) {
    const_iterator it2 = it+1;
    if(it2 == end()) it2 = begin();
    
    d += it->distance(*it2);
  }
  
  return d;
}

int Polygon::quadrant(const Vertex &v) const {
  if(v.getX() > 0 && v.getY() >= 0) return 1;
  if(v.getX() >= 0 && v.getY() < 0) return 2;
  if(v.getX() < 0 && v.getY() <= 0) return 3;
  if(v.getX() <= 0 && v.getY() > 0) return 4;
  
  return 0;
}

bool Polygon::contains(const Vertex &v) const {
  const Line d1(-1, -1, 1, 1);
  const Line d2(-1, 1, 1, -1);
  int d = 0;
  int q, ql, q2;
  Line l;
  Vertex v2;
  
  
  if(empty()) return false;
  
  v2 = back() - v;
  q = quadrant(v2);
  
  if(q == 0) return true;
  
  for(const_iterator it = begin(); it != end(); it++) {
    ql = q;
    
    v2 = *it - v;
    q = quadrant(v2);
    
    if(q == 0) return true;
    
    switch(q-ql) {
      case 0:
        break;
      case 1:
      case -3:
        d++;
        break;
      case 3:
      case -1:
        d--;
        break;
      default:
        l.setVertex1(((it == begin()) ? back() : *(it-1)) - v);
        l.setVertex2(v2);
        
        if(q == 1 || q == 3) {
          if(!(l.intersects(d2, &v2) & INTERSECTION_LINE)) return false;
          
          q2 = quadrant(v2);
          if(q2 == 0) return true;
          
          if((q == 1 && q2 == 2) || (q == 3 && q2 == 4)) d -= 2;
          else d += 2;
        }
        else {
          if(!(l.intersects(d1, &v2) & INTERSECTION_LINE)) return false;
          
          q2 = quadrant(v2);
          if(q2 == 0) return true;
          
          if((q == 2 && q2 == 3) || (q == 4 && q2 == 1)) d -= 2;
          else d += 2;
        }
    }
  }
  
  return (d != 0);
}

bool Polygon::intersects(const Line &l) const {
  Line line;
  
  for(const_iterator it = begin(); it != end(); it++) {
    const_iterator it2 = it+1;
    if(it2 == end()) it2 = begin();
    
    line.setVertex1(*it);
    line.setVertex2(*it2);
    
    if(l.intersects(line, NULL) == INTERSECTION_SEGMENT_SEGMENT)
      return true;
  }
  
  return false;
}

bool Polygon::isConcave(const Direction &dir, const Vertex &v1, const Vertex &v2, const Vertex &v3) const {
  switch(dir) {
    case Triangle::CW:
      return (v1.getX()-v2.getX())*(v3.getY()-v2.getY()) > (v3.getX()-v2.getX())*(v1.getY()-v2.getY());
    case Triangle::CCW:
      return (v1.getX()-v2.getX())*(v3.getY()-v2.getY()) < (v3.getX()-v2.getX())*(v1.getY()-v2.getY());
    default:
      return false;
  }
}

bool Polygon::intersections(std::vector<Intersection> *intersections) const {
  bool ret = false;
  size_t s = size();
  
  for(size_t i = 0; i+2 < s; i++) {
    Line l(at(i), at(i+1));
    
    for(size_t j = i+2; j < s; j++) {
      if(i == (j+1)%s) continue;
      
      Line l2(at(j), at((j+1)%s));
      
      Vertex v;
      if(l.intersects(l2, &v) == INTERSECTION_SEGMENT_SEGMENT) {
        if(intersections) {
          Intersection in = {i, i+1, j, ((j+1)%s), v};
          
          intersections->push_back(in);
          ret = true;
        }
        else {
          return true;
        }
      }
    }
  }
  
  return ret;
}

Polygon::Direction Polygon::getDirection() const {
  float area = signedArea();
  
  return (area > 0) ? Triangle::CW : (area < 0) ? Triangle::CCW : Triangle::UNKNOWN;
}

bool Polygon::isSimple() const {
  return !intersections();
}

bool Polygon::simplify(std::list<Polygon> &polygons) const {
  std::vector<Intersection> ins;
  std::vector<Intersection*> inlist;
  
  if(!intersections(&ins)) return false;
  
  size_t s = size();
  size_t start = 0;
  
  for(size_t i = 1; i < s; i++) {
    if(at(i).getX() < at(start).getX())
      start = i;
    else if(at(i).getX() == at(start).getX() && at(i).getY() < at(start).getY())
      start = i;
  }
  
  int dir = Triangle(at((s+start-1)%s), at(start), at((start+1)%s)).getDirection();
  
  size_t i2 = start;
  const Vertex *v2 = &at(i2);
  bool intersected;
  
  polygons.push_back(Polygon());
  
  do {
    intersected = false;
    
    size_t i = i2;
    
    if(dir == Triangle::CW)
      i2 = (i2+1)%s;
    else
      i2 = (s+i2-1)%s;
    
	const Vertex *v1 = v2;
	v2 = &at(i2);
	
	float dl = v2->distanceSq(*v1);
	
	Intersection *intr = NULL;
	Vertex *v = NULL;
	float dv = 0;
	size_t v3 = 0, v4 = 0;
	
	for(std::vector<Intersection>::iterator in = ins.begin(); in != ins.end(); ++in) {
	  float di = v2->distanceSq(in->v);
	  
	  if(di >= dl || v1 == &in->v)
	    continue;
	  
	  if(v && di < dv)
	    continue;
	  
	  if((i == in->va1 && i2 == in->va2) || (i == in->va2 && i2 == in->va1)) {
	    intr = &*in;
	    
	    v = &in->v;
	    dv = di;
	    v3 = in->vb1;
	    v4 = in->vb2;
	  }
	  else if((i == in->vb1 && i2 == in->vb2) || (i == in->vb2 && i2 == in->vb1)) {
	    intr = &*in;
	    
	    v = &in->v;
	    dv = di;
	    v3 = in->va1;
	    v4 = in->va2;
	  }
    }
	
	if(intr) {
	  if(isConcave(Triangle::CW, *v1, *v2, at(v4))) {
	    if(!Line(*v1, *v2).contains(at(v3))) {
	      i2 = v3;
	      dir = Triangle::CW;
	    }
	  }
	  else {
	    if(!Line(*v1, *v2).contains(at(v4))) {
	      i2 = v4;
	      dir = Triangle::CCW;
	    }
	  }
	  
	  v2 = v;
	  intersected = true;
	  
	  std::vector<Intersection*>::reverse_iterator it = inlist.rbegin();
	  for(; it != inlist.rend(); it++) {
	    if(*it == intr) break;
	  }
	  
	  if(it != inlist.rend()) {
	    inlist.erase(it.base()-1, inlist.end());
	    
	    polygons.push_back(Polygon());
	    
	    while(!polygons.front().empty()) {
	      polygons.back().push_back(polygons.front().back());
	      polygons.front().pop_back();
	      
	      if(v->distanceSq(polygons.back().back()) < 1E-12)
	        break;
	    }
	  }
	  else {
	    inlist.push_back(intr);
	  }
	}
	
	polygons.front().push_back(*v2);
  } while(i2 != start || intersected);
  
  return true;
}

void Polygon::doTriangulate(std::vector<Triangle> &triangles) const {
  size_t s = size();
  
  if(s < 3) return;
  
  std::vector<size_t> p;
  std::list<size_t> concave;
  Direction dir = getDirection();
  
  for(size_t i = 0; i < s; i++) {
    p.push_back((i+1)%s);
    
    if(isConcave(dir, at(i), at((i+1)%s), at((i+2)%s)))
      concave.push_back((i+1)%s);
  }
  
  for(size_t i = 0; i < p.size() && p.size() > 3; i++) {
    size_t s2 = p.size();
    
    const Vertex *v0 = &at(p[i]);
    const Vertex *v1 = &at(p[(i+1)%s2]);
    const Vertex *v2 = &at(p[(i+2)%s2]);
    const Vertex *v3 = &at(p[(i+3)%s2]);
    const Vertex *v4 = &at(p[(i+4)%s2]);
    
    
    if(isConcave(dir, *v1, *v2, *v3))
      continue;
    
    Triangle t(*v1, *v2, *v3);
    
    std::list<size_t>::iterator it = concave.begin();
    
    for(; it != concave.end(); it++) {
      if(*it != p[(i+1)%s2] && *it != p[(i+3)%s2] && t.contains(at(*it)))
        break;
    }
    
    if(it != concave.end())
      continue;
    
    triangles.push_back(t);
    
    if(isConcave(dir, *v2, *v3, *v4) && !isConcave(dir, *v1, *v3, *v4))
      concave.remove(p[(i+3)%s2]);
    
    if(isConcave(dir, *v0, *v1, *v2) && !isConcave(dir, *v0, *v1, *v3)) {
      concave.remove(p[(i+1)%s2]);
    }
    
    p.erase(p.begin()+(i+2)%s2);
    i = -1;
  }
  
  
  triangles.push_back(Triangle(at(p[0]), at(p[1]), at(p[2])));
}

void Polygon::triangulate(std::vector<Triangle> &triangles) const {
  std::list<Polygon> polygons;
  
  if(simplify(polygons)) {
    for(std::list<Polygon>::iterator p = polygons.begin(); p != polygons.end(); p++)
      p->doTriangulate(triangles);
  }
  else doTriangulate(triangles);
}