#include "MeshBuilder.hpp"

#include <queue>

using namespace std;

namespace mgx 
{


    int addVertex(const vertex &v, std::vector<vertex>& vtxs)
    {
      for(size_t i = 0; i<vtxs.size(); i++){
        vertex& n = vtxs[i];
        if(v == n) return i;
      }

      vtxs.push_back(v);

      return vtxs.size()-1;
    }

       // Get the index of a point, adding it if required 
    int MeshBuilder::getIndex(const vertex& v)
    {
      //double Epsilon = 1E-5;

      if(pMap.empty()) {
        // If empty just add the point
        pMap[v->pos] = 0;
        vtxVec.push_back(v);
        return 0;
      } else if(pMap.count(v->pos) > 0) {
        // If found, get the index
        return pMap[v->pos];
      } else {
        // See if close to existing point and if so just use it
        Point3d p = v->pos;
        p.x() -= Epsilon;
        std::map<Point3d, int>::iterator it = pMap.lower_bound(p);
        if(it == pMap.end())
          --it;
        int i = 0;
        while(it != pMap.end() and it->first.x() - v->pos.x() < Epsilon) {
          if(norm(v->pos - it->first) < Epsilon)
            return it->second;
          ++it;
        }
        i = int(vtxVec.size());
        pMap[v->pos] = i;
        vtxVec.push_back(v);
        return i;
      }
    } 


    // Add a triangle sharing points and vertices if they already exist
    void MeshBuilder::addTri(const vertex &a, const vertex &b, const vertex &c, bool keepAll)
    {
      // See if really a triangle
      if(norm(a->pos - b->pos) <= Epsilon)
        return;
      if(norm(b->pos - c->pos) <= Epsilon)
        return;
      if(norm(c->pos - a->pos) <= Epsilon)
        return;
    
      // Add triangle

      int aIdx;
      int bIdx;
      int cIdx;
      
      if(keepAll){
        aIdx = addVertex(a, vtxVec);
        bIdx = addVertex(b, vtxVec);
        cIdx = addVertex(c, vtxVec);
      } else {
        aIdx = getIndex(a);
        bIdx = getIndex(b);
        cIdx = getIndex(c);
        //std::cout << "dont keep" << std::endl;
      }
      //std::cout << aIdx << std::endl;
      triVec.push_back(Point3i(aIdx, bIdx, cIdx));

    }
/*
    // Add a triangle sharing points and vertices if they already exist
    void MeshBuilder::addTri(const Point3d &a, const Point3d &b, const Point3d &c)
    {
      // See if really a triangle
      if(norm(a - b) <= Epsilon)
        return;
      if(norm(b - c) <= Epsilon)
        return;
      if(norm(c - a) <= Epsilon)
        return;
    
      // Add triangle
      int aIdx = getIndex(a);
      int bIdx = getIndex(b);
      int cIdx = getIndex(c);
    
      triVec.push_back(Point3i(aIdx, bIdx, cIdx));

    }
  */  
    // Get the index of a point, adding it if required 
    int MeshBuilder::getIndex(Point3d pos, int label)
    {
      if(pMap.empty()) {
        // If empty just add the point
        pMap[pos] = 0;
        vertexLabelMap[0] = label;
        vertexLabels.push_back(label);
        pVec.push_back(pos);
        return 0;
      } else if(pMap.count(pos) > 0) {
        // If found, get the index
        return pMap[pos];
      } else {
        // See if close to existing point and if so just use it
        Point3d p = pos;
        p.x() -= Epsilon;
        std::map<Point3d, int>::iterator it = pMap.lower_bound(p);
        if(it == pMap.end())
          --it;
        int i = 0;
        while(it != pMap.end() and it->first.x() - pos.x() < Epsilon) {
          if(norm(pos - it->first) < Epsilon)
            return it->second;
          ++it;
        }
        i = int(pVec.size());
        pMap[pos] = i;
        vertexLabelMap[i] = label;
        vertexLabels.push_back(label);
        pVec.push_back(pos);
        return i;
      }
    }
  
  
    // Add a triangle sharing points and vertices if they already exist
    void MeshBuilder::addTri(const Point3d &a, const Point3d &b, const Point3d &c, Point3i label)
    {
      // See if really a triangle
      if(norm(a - b) <= Epsilon)
        return;
      if(norm(b - c) <= Epsilon)
        return;
      if(norm(c - a) <= Epsilon)
        return;
    
      // Add triangle
      int aIdx = getIndex(a, label.x());
      int bIdx = getIndex(b, label.y());
      int cIdx = getIndex(c, label.z());
    
      triVec.push_back(Point3i(aIdx, bIdx, cIdx));
    
    }

    void MeshBuilder::writeVertexVec(bool useLabelMap, int label)
    {
      for(uint i = 0; i < pVec.size(); ++i) {
        vertex v;
        v->pos = pVec[i];
        if(!useLabelMap)
          v->label = label;
        else
          v->label=vertexLabelMap[i];
        vtxVec.push_back(v);
      }
    }

    void MeshBuilder::writeVertexVec(std::map<Point3d, int> pointLabelMap)
    {

    //std::vector<vertex> vtxVecNew;
      for(uint i = 0; i < pVec.size(); ++i) {
        vertex v;
        v->pos = pVec[i];
        if(pointLabelMap[v->pos] > 0)
          v->label = pointLabelMap[v->pos];
        vtxVec.push_back(v);
      }
     //vtxVec = vtxVecNew;

    }

    void MeshBuilder::addToVec(MeshBuilder mb)
    {

          size_t sz = vtxVec.size();
          Point3i shift(sz, sz, sz);

          for(uint i = 0; i < mb.triVec.size(); ++i){
            mb.triVec[i] = mb.triVec[i] + shift;
            triVec.push_back(mb.triVec[i]);
          }
          for(uint i = 0; i < mb.vtxVec.size(); ++i){
            vtxVec.push_back(mb.vtxVec[i]);
          }

    }


    void MeshBuilder::addTriCheckOrient(const vertex &a, const vertex &b, const vertex &c, Point3d &nrml, bool keepAll)
    {
      Point3d triNrml = (b->pos-a->pos) % (c->pos-a->pos);
      if(triNrml * nrml > 0){
        addTri(a,b,c, keepAll);
      } else {
        addTri(b,a,c, keepAll);
      }

    }


    bool equalVtx(Point3i p1, Point3i p2)
    {

      if(p1.x() == p2.x() or p1.x() == p2.y() or p1.x() == p2.z()) return true;
      if(p1.y() == p2.x() or p1.y() == p2.y() or p1.y() == p2.z()) return true;
      if(p1.z() == p2.x() or p1.z() == p2.y() or p1.z() == p2.z()) return true;
      return false;
    }

    // find one connected component in a tri vector, return the component and the remaining tris (that are not part of the component)
    void findComponent(std::vector<Point3i>& remainingTris, std::vector<Point3i>& comp)
    {
      //std::cout << "find Comp" << std::endl;
      if(remainingTris.empty()) return;

      //start with first tri
      std::set<Point3i> remainingTrisSet, compSet;

      forall(Point3i p, remainingTris)
        remainingTrisSet.insert(p);

      std::queue<Point3i> frontierQ;
      frontierQ.push(remainingTris[0]);
      remainingTrisSet.erase(frontierQ.front());
      compSet.insert(frontierQ.front());

      while(!frontierQ.empty()){
        //std::cout << "round: " << frontierQ.size() << "/" << remainingTrisSet.size() << "/" << compSet.size() << std::endl;
        // take front element, add it to comp, find all its neighbors, add them to frontier and delete from remaining
        Point3i currentP = frontierQ.front();
        frontierQ.pop();
        
        forall(Point3i p, remainingTrisSet){
          if(equalVtx(p,currentP)){
            frontierQ.push(p);
            compSet.insert(p);
          }
          
        }
        forall(Point3i p, compSet){
          remainingTrisSet.erase(p);
        }
      }

      std::vector<Point3i> remainingTrisNew, compNew;

      forall(Point3i p, remainingTrisSet)
        remainingTrisNew.push_back(p);

      forall(Point3i p, compSet)
        compNew.push_back(p);

      remainingTris = remainingTrisNew;
      comp = compNew;

    }

    // find all connected components in a tri vector
    std::vector<std::vector<Point3i> > findAllComponents(std::vector<Point3i>& tris)
    {

      std::vector<std::vector<Point3i> > components;

      while(!tris.empty()){
        //std::cout << "left: " << remainingTris.size() << std::endl;
        std::vector<Point3i> component;
        findComponent(tris, component);
        //std::cout << "new: " << remainingTris.size() << "/" << component.size() << std::endl;
        components.push_back(component);
      }

      return components;
    }

    // check the orientation of the tri vec, assume that the center is inside of the body
    void MeshBuilder::correctOrientationsComponent(std::vector<Point3i>& tris)
    {
      std::set<int> vtxIds;

      // first get the center of current component
      forall(Point3i p, tris){
        vtxIds.insert(p.x());
        vtxIds.insert(p.y());
        vtxIds.insert(p.z());
      }
      Point3d center(0,0,0);
      forall(int idx, vtxIds){
        center+=vtxVec[idx]->pos;
      }
      center/=vtxIds.size();

      // now check orientation of first triangle


      // now run tri orient check



 
    }


    void MeshBuilder::eraseSmallComponents()
    {

      
      //std::cout << "eraseSmallComponents" << std::endl;
      std::vector<Point3i> remainingTris;

      forall(Point3i p, triVec){
        Point3i pNew (p.x(),p.y(),p.z());
        remainingTris.push_back(pNew);
      }
      std::vector<std::vector<Point3i> > components = findAllComponents(remainingTris);
     /* std::vector<std::vector<Point3i> > components;

      while(!remainingTris.empty()){
        //std::cout << "left: " << remainingTris.size() << std::endl;
        std::vector<Point3i> component;
        findComponent(remainingTris, component);
        //std::cout << "new: " << remainingTris.size() << "/" << component.size() << std::endl;
        components.push_back(component);
      }


      //std::cout << "components: " << components.size() << std::endl;
*/
      uint maxSize = 0;
      std::vector<Point3i> maxComponent;

      forall(std::vector<Point3i>& comp, components){
        //std::cout << "comp: " << comp.size() << "/" << maxSize << std::endl;
        if(comp.size() > maxSize){
          //std::cout << "new2: " << comp.size() << "/" << maxSize << std::endl;
          maxSize = comp.size();
          maxComponent = comp;
        }
        //std::cout << "comp2: " << maxSize << "/" << maxComponent.size() << "/" << comp.size() << std::endl;
      }
      //std::cout << "max: " << maxComponent.size() << std::endl;
      triVec = maxComponent;

    }

  Point3i sortPoint3i(Point3i p)
  {
    Point3i result = p;
    int s;

    if(result.x()>result.y()){
      s = result.y();
      result.y() = result.x();
      result.x() = s;
    }

    if(result.y()>result.z()){
      s = result.z();
      result.z() = result.y();
      result.y() = s;
    }

    if(result.x()>result.y()){
      s = result.y();
      result.y() = result.x();
      result.x() = s;
    }

    return result;
  }


  // return true if tris use identical vtxs
  bool trisIdentical(Point3i t1, Point3i t2)
  {
    Point3i t1Sorted = sortPoint3i(t1);
    Point3i t2Sorted = sortPoint3i(t2);

    if(t1Sorted == t2Sorted){
    	std::cout << "identical!  " << t1 << "/" << t2 << "//" << t1Sorted << "/" << t2Sorted << std::endl;
      return true;
    } 

    return false;
    

  }



  // erase tris using the same vtxs
  void MeshBuilder::eraseIdenticalTris()
  {

  std::vector<Point3i> trisToTest;

  std::vector<Point3i> triVecNew;

  forall(Point3i p, triVec){
  	trisToTest.push_back(p);
  }

  while(!trisToTest.empty()){
    // take the first tri
    std::vector<Point3i> trisToTestNew;
    std::cout << "while " << trisToTest.size() << std::endl;

    Point3i p0 = trisToTest[0];
    //Point3i p0 = (*it);

    if(trisToTest.size() == 1){
      triVecNew.push_back(p0);
      trisToTest = trisToTestNew;
      continue;
    }

    //int idx = -1;
    Point3i pi;
    std::cout << "pp " << p0 << "/" << std::endl;

    bool del = false;
    // check if duplicate in the rest
    for(size_t i = 1; i<trisToTest.size(); i++){
      pi = trisToTest[i];
      if(trisIdentical(p0,pi)){
        del = true;
      } else {
        trisToTestNew.push_back(pi);
      }
    }

    std::cout << "del " << p0 << "/" << pi << "/" << del << std::endl;

    if(!del){
      triVecNew.push_back(p0);
    }

    trisToTest = trisToTestNew;
    // when found erase both
    // if not add to new triVec
  }

  triVec = triVecNew;

  }




    // shoots a ray from p to the center of the first triangle in trivec, counts hit through other tris
    // if odd: p outside of mesh (return false), even: inside (return true)
    // requires a properly closed mesh
    // TODO not finished yet
    bool pointInVolume(Point3d p, std::vector<Point3i>& triVec, std::vector<vertex>& vtxVec)
    {

    if(triVec.size() < 1 or vtxVec.size() < 1) return false;

    Point3d triP = (vtxVec[triVec[0].x()]->pos + vtxVec[triVec[0].y()]->pos + vtxVec[triVec[0].z()]->pos) / 3.;

    int counter = 0;

    for(size_t i = 1; i<triVec.size(); i++){
      //Point3d triP = (vtxVec[triVec[0].x()]->pos + vtxVec[triVec[0].y()]->pos + vtxVec[triVec[0].z()]->pos) / 3.
      Point3d inters;
      if(rayTriangleIntersect(p, triP, vtxVec[triVec[i].x()]->pos, vtxVec[triVec[i].y()]->pos, vtxVec[triVec[i].z()]->pos, inters) == 1){
        counter++;
      }
    }

    if(counter % 2 == 0) return true;
    return false;

    }



}
  