#include "Shape.h"

#include <iostream>
#include <stack>

#include <tulip/SuperGraph.h>
#include <tulip/TlpTools.h>
#include <tulip/Coord.h>

using namespace std;

class ConvexHull {
public:
  ConvexHull() {
    graph = TlpTools::newSuperGraph();
    layout = getProxy<LayoutProxy>(graph, "convexHull");
  }
  
  ~ConvexHull(){
    delete graph;
  }
  
  list<Coord> getConvexHull() const {
    list<Coord> tmp;  
    node next = start;
    if (graph->numberOfNodes() == 1) tmp.push_back(layout->getNodeValue(start));
    else
      for (int i=0; i<graph->numberOfNodes(); ++i) {
        tmp.push_back(layout->getNodeValue(next));
        next=graph->getOutNode(next, 1);
      }
    return tmp;
  }

  void addPoint(const Coord &p) {
    //    cerr << __PRETTY_FUNCTION__ << ": (" << (uint)p[0] <<"," << (uint)p[1] << ") ";
    if (graph->numberOfNodes() == 0) {
      start = graph->addNode();
      layout->setNodeValue(start, p);
      //      cerr << endl;
      return;
    }
    
    if (graph->numberOfNodes() == 1) {
      if (layout->getNodeValue(start) == p) {
        //        cerr << endl;
        return;
      }
      node tmp = graph->addNode();
      layout->setNodeValue(tmp, p);
      graph->addEdge(start, tmp);
      graph->addEdge(tmp, start);
      //      cerr << endl;
      return;
    }

    if (graph->numberOfNodes() == 2) {
      node back = start;
      node forw = graph->getOutNode(start, 1);
      Coord c1 = layout->getNodeValue(back);
      Coord c2 = layout->getNodeValue(forw);
      if ((c1 == p) || (c2 == p)) return;

      Coord vn = (c2 - c1);
      Coord vp = (p - c1);

      //check for alignment, and drop the point between the 2 others
      if ((vn^vp)[2]==0) {
        if (vn*vp < 0) { //drop "back"
          layout->setNodeValue(back, p);
          //          cerr << "drop back ";
        }
        else if ((-1 * vn)*vp < 0) { //drop "forw"
          layout->setNodeValue(forw, p);
          //          cerr << "drop forw ";
        }
        //else "p" is implicitly dropped
        //        cerr << endl;
        return;
      }

      if ((vn^vp)[2]<0) {
        //        cerr << " change... ";
        back = forw;
        forw = start;
      }
      node tmp = graph->addNode();
      layout->setNodeValue(tmp, p);
      Iterator<edge>*ite = graph->getOutEdges(back);
      edge e = ite->next();
      graph->delEdge(e);
      delete ite;
      graph->addEdge(back, tmp);
      graph->addEdge(tmp, forw);
      start = tmp;
      //      cerr << "OK " << endl;
      return;
    }
    
    bool modif = false;
    float sinus;
    node cur = graph->getOutNode(start, 1);
    Coord A = layout->getNodeValue(start);
    Coord B = layout->getNodeValue(cur);
    Coord AB, Ap;
    AB = (B-A);
    Ap = (p-A);
    //    cerr << "start=(" << (uint)A[0] << "," << (uint)A[1] << ") cur=(" << (uint)B[0] << "," << (uint)B[1] << ") ";
    node back = start;
    node forw = cur;
    int stopIt = 0;

    //skip segments for whom p is on the right or which are aligned;
    //previous tests ensure we are working on a triangle with distinct points
    while((((sinus=((AB)^(Ap))[2]) < 0) || ((sinus==0) && ((AB*Ap) > 0)))
          && (stopIt < graph->numberOfNodes())){
      if (A == p) return;
      back = forw;
      forw = graph->getOutNode(forw, 1);
      A = B;
      B = layout->getNodeValue(forw);
      AB = (B-A);
      Ap = (p-A);
      ++stopIt;
    }

    //    cerr << "back=(" << (uint)A[0] << "," << (uint)A[1] << ") forw=(" << (uint)B[0] << "," << (uint)B[1] << ") ";
    //the second test verify that p is not in the middle of the segment [back,forward]: !(cos() > 0 && sin() == 0)
    if ((stopIt < graph->numberOfNodes()))// && !((sinus == 0) && ((AB*Ap) > 0)))
      {
      modif = true;
      cur = forw;
      start = back;
      //forward
      AB = (B-A);
      Ap = (p-A);
      // eliminates forward nodes inside the segment [start, p];
      while(((AB^Ap)[2] == 0) && (AB*Ap < 0)) {
        if (A == p) return;
	cur = graph->getOutNode(cur, 1);
	B = layout->getNodeValue(cur);
        //  cerr << "forwseg B=(" << (uint)B[0] << "," << (uint)B[1] << ") ";
        AB=(B-A);
        Ap=(p-A);
      }

      // eliminates nodes inside the new hull
      do {
	forw = cur;
	cur = graph->getOutNode(cur, 1);
	A = layout->getNodeValue(forw);
        if (A == p) return;
        //cerr << "forw A=(" << (uint)A[0] << "," << (uint)A[1] << ") ";
	B = layout->getNodeValue(cur);
        //cerr << "B=(" << (uint)B[0] << "," << (uint)B[1] << ") ";
        AB=(B-A);
        Ap=(p-A);
      } while(((AB^Ap)[2] >= 0));

      //backward
      B = layout->getNodeValue(start);
      cur = start;
      //cur = graph->getInNode(start, 1);
      AB = (B-A);
      Ap = (p-A);
      //eliminates nodes inside the segment [start, p]
      while(((AB^Ap)[2] == 0) && (AB*Ap < 0)) {
        cur = graph->getInNode(cur, 1);
        A = layout->getNodeValue(cur);
        if (A == p) return;
        //       cerr << "backseg A=(" << (uint)A[0] << "," << (uint)A[1] << ") ";
        AB = (B-A);
        Ap = (p-A);
      }
      
      do {
	back = cur;
	cur = graph->getInNode(cur, 1);
	B = layout->getNodeValue(back);
        //        cerr << "back B=(" << (uint)B[0] << "," << (uint)B[1] << ") ";
	A = layout->getNodeValue(cur);
        if (A == p) return;
        //        cerr << "A=(" << (uint)A[0] << "," << (uint)A[1] << ") ";
        AB=(B-A);
        Ap=(p-A);
      } while(((AB^Ap)[2] >= 0));
    }

    if(modif) {
      A = layout->getNodeValue(back);
      B = layout->getNodeValue(forw);
      //      cerr << "fin: back(" << (uint)A[0] << "," << (uint)A[1] << ") forw(" << (uint)B[0] << "," << (uint)B[1] << ") ";
      node next = back;
      stack<node> tmp;
      while((next=graph->getOutNode(next, 1)) != forw){
        Coord c = layout->getNodeValue(next);
        
        //        cerr << "push : c=(" << (uint)c[0] << "," << (uint)c[1] << ") ";
	tmp.push(next);
      }
      if (tmp.empty()) {
	Iterator<edge>*ite = graph->getOutEdges(back);
	edge e = ite->next();
	graph->delEdge(e);
	delete ite;
      }
      while(!tmp.empty()){
	graph->delNode(tmp.top());
	tmp.pop();
      }
      start = graph->addNode();
      graph->addEdge(back, start);
      graph->addEdge(start, forw);
      layout->setNodeValue(start, p);
    }
//     else {
//       cerr << " failed test ";
//     }
    //    cerr <<endl;
  }
  
private:
  SuperGraph *graph;
  LayoutProxy *layout;
  node start;
};

namespace tlprender
{
  Shape::Shape() : valid(false), avgZ(0.0) {}
  Shape::~Shape() {}

  bool Shape::isValid() const {return (vertices.size() > 2);}
  
  void Shape::begin() {valid=false; vertices=vector<Point>();}
  
  void Shape::add(const int x, const int y, const GLfloat z) {
    add(Point(x,y,z));
  }

  bool Shape::end() {
      int count=0;
      avgZ=0.0;
      for (vector<Point>::iterator i = vertices.begin(); i != vertices.end(); ++i) {      
        vector<Point>::const_iterator pre = (i==vertices.begin() ? vertices.end() : i);
        vector<Point>::const_iterator next = i;      
        --pre; ++next; if (next==vertices.end()) next=vertices.begin();
        if ((*i==*pre) || (*i==*next)) {
          i=vertices.erase(i);
          if (i != vertices.begin()) --i;
        }
        else {
          ++count;
          avgZ += i->z;
        }
      }
    
      if (count != 0) avgZ /= count;
      valid = (vertices.size()>2);
      if (valid) {
        ConvexHull *convexHull = new ConvexHull();
        for (vector<Point>::const_iterator it = vertices.begin(); it != vertices.end(); ++it) {
          convexHull->addPoint(Coord(it->x, it->y, 0));
        }
        vertices.clear();
        list<Coord> l = convexHull->getConvexHull();
        for (list<Coord>::const_iterator it = l.begin(); it != l.end(); ++it) {
          vertices.push_back(Point((int)it->getX(), (int)it->getY(), it->getZ()));
        }
        delete convexHull;
      }
      return valid;
    }

  bool Shape::clip(const Shape &sh)
  {
    ConvexHull *convexHull = new ConvexHull();
    vertices.insert(vertices.end(), sh.vertices.begin(), sh.vertices.end());
    for (vector<Point>::const_iterator it = vertices.begin(); it != vertices.end(); ++it) {
      convexHull->addPoint(Coord(it->x, it->y, 0));
    }
    vertices.clear();
    list<Coord> l = convexHull->getConvexHull();
    for (list<Coord>::const_iterator it = l.begin(); it != l.end(); ++it) {
      vertices.push_back(Point((int)it->getX(), (int)it->getY(), it->getZ()));
    }
    delete convexHull;
    return true;
  }
  
  ostream& operator<<(ostream &os, const Shape &p)
  {
    for (vector<Point>::const_iterator i=p.vertices.begin(); i != p.vertices.end();) {
    os << i->x << "," << i->y;
    if (++i != p.vertices.end()) os << ",";
    }
    return os;
  }

  ostream& operator<<(ostream &os, const Point &p) {
    os << "(" << p.x << "," << p.y << ")";
    return os;
  }
}

bool less<tlprender::Shape *>::operator() (tlprender::Shape *&p1, tlprender::Shape *&p2) {
  return (*p2 <= *p1);
}

bool less<tlprender::Shape>::operator()(const tlprender::Shape &p1, const tlprender::Shape &p2){
  return (p2 <= p1);
}