#include "DivisionAnalysisData.hpp"

#include <triangle.h>
#include <Triangulate.hpp>

using namespace std;

namespace mgx
{

// out1: shortestPlaneNrml
// out2: shortesPlaneDispl

// in1 : vvGraph
// in2 : cell vertex
// in3 : samplingDirections
 // void simulateDivision2D(const vvGraph& S, const vertex& vc, std::vector<P3dDouble>& samplingDirections, Point3d& shortestPlaneNrml, double& shortesPlaneDispl)
 // {
 //    Point3d cpos = vc->pos;


 //    double mindis = HUGE_VAL;
 //    vertex p_u1, p_u2, ttu1, ttu2;
 //    Point3d minu, uu;
 //    for(size_t i = 0; i < samplingDirections.size(); ++i) {
 //      int count = 0;
 //      double dis = 0;
 //      forall(const vertex &tu1, S.neighbors(vc)) {
 //        vertex tu2 = S.nextTo(vc,tu1);
 //        Point3d u, u1 = tu1->pos, u2 = tu2->pos;
 //        double s;
 //        if(planeLineIntersect(cpos+samplingDirections[i].first*samplingDirections[i].second, samplingDirections[i].first, u1, u2, s, u))
 //          if(s >= 0 and s <= 1.0) {
 //            ttu1 = tu1;
 //            ttu2 = tu2;
 //            uu = u;
 //            count++;
 //            dis += norm(u - cpos);
 //          }
 //      }
 //      if(count == 2 and dis < mindis) {
 //        mindis = dis;
 //        minu = uu;
 //        p_u1 = ttu1;
 //        p_u2 = ttu2;
 //        shortestPlaneNrml = samplingDirections[i].first;
 //        shortesPlaneDispl = samplingDirections[i].second;
 //      }
 //    }

 // }

  double signedTriArea(const Point3d &p1, const Point3d &p2, const Point3d &p3)
  {
    Point3d d21 = p2-p1;
    Point3d d32 = p3-p2;
    return (d21.x()*d32.y() - d21.y()*d32.x())/2.0;
  }

  double calcArea(std::vector<Point3d>& cellContourOrdered)
  {
    double result = 0;

    if(cellContourOrdered.empty()) return -1;

    Point3d avgPos;

    // find center point
    forall(Point3d p, cellContourOrdered){
      avgPos += p;
    }
    avgPos /= cellContourOrdered.size();

    Point3d pLast = cellContourOrdered[cellContourOrdered.size()-1];

    // walk around contour and sum up area
    for(int i=0; i<cellContourOrdered.size(); i++){
      if(i > 0) pLast = cellContourOrdered[i-1];
      result += signedTriArea(avgPos, pLast, cellContourOrdered[i]);
    }

    return result;
  }


 double calcAreaRatio(std::vector<vertex>& cellContourOrdered, std::pair<vertex,vertex> cutEdge1, Point3d cutP1, std::pair<vertex,vertex> cutEdge2, Point3d cutP2)
 {
  vertex pLast = cellContourOrdered[cellContourOrdered.size()-1];

  std::vector<Point3d> cell1, cell2;
  bool cell1Active = true;
  cell1.push_back(pLast->pos);

  // go through all edges
  for(int i=0; i<cellContourOrdered.size(); i++){
    if(i > 0) pLast = cellContourOrdered[i-1];

    std::pair<vertex, vertex> currentEdge = std::make_pair(pLast, cellContourOrdered[i]);

    if(currentEdge == cutEdge1){
      cell1.push_back(cutP1);
      cell2.push_back(cutP1);
      if(cell1Active){
        cell2.push_back(cellContourOrdered[i]->pos);
      } else {
        cell1.push_back(cellContourOrdered[i]->pos);
      }

      cell1Active = !cell1Active;
    } else if(currentEdge == cutEdge2){
      cell1.push_back(cutP2);
      cell2.push_back(cutP2);

      if(cell1Active){
        cell2.push_back(cellContourOrdered[i]->pos);
      } else {
        cell1.push_back(cellContourOrdered[i]->pos);
      }

      cell1Active = !cell1Active;
    } else {
      if(cell1Active){ // add edge to cell1
        cell1.push_back(cellContourOrdered[i]->pos);
      } else{ // add edge to cell2
        cell2.push_back(cellContourOrdered[i]->pos);
      }
    }

  }

  double a1 = norm(calcArea(cell1));
  double a2 = norm(calcArea(cell2));

  //std::cout << "a " << a1 << "/" << a2 << "/" << cutP1 << "/" << cutP2 << std::endl;
  if(a1 > 0 and a2 > 0) return max(a1,a2)/min(a1,a2);


  return 0;
 }

 void simulateDivision2D(std::vector<vertex>& cellContour, Point3d& divP, Point3d divNormal, std::vector<P3dDouble>& samplingDirections,
  std::vector<CellDivision>& allDivisions, Point3d& shortestPlaneNrml, double& shortesPlaneDispl)
 {
    double mindis = HUGE_VAL;

    std::vector<CellDivision> allDivisionsNew;



    // test each div plane
    for(size_t i = 0; i < samplingDirections.size(); ++i) {

      std::vector<std::pair<vertex, vertex> > cuttingEdges;
      std::vector<Point3d> cuttingPoints;

      int count = 0;
      double dis = 0;

      // test each contour segment whether it cuts the plane or not
      vertex v1 = cellContour[cellContour.size()-1];
      Point3d seg1 = cellContour[cellContour.size()-1]->pos;
      for(int j = 0; j<cellContour.size(); j++){
        vertex v2 = cellContour[j];
        if(j>0) v1 = cellContour[j-1];

        Point3d seg1 = v1->pos;
        Point3d seg2 = v2->pos;

        Point3d u;
        double s;
        if(planeLineIntersect(divP+samplingDirections[i].first*samplingDirections[i].second, samplingDirections[i].first, seg1, seg2, s, u))
          if(s >= 0 and s <= 1.0) {
            count++;
            dis += norm(u - divP);
            cuttingEdges.push_back(std::make_pair(v1,v2));
            cuttingPoints.push_back(u);
          }
      }
      if(count == 2){ // hits side walls exactly two times
        CellDivision cd;
        cd.divPlane = DivPlane(samplingDirections[i].first, divP+samplingDirections[i].first*samplingDirections[i].second, samplingDirections[i].second);
        cd.divPlane.area = dis;
        cd.divPlane.label = i+1;
        cd.cellNormal = divNormal;
        cd.volumeRatio = calcAreaRatio(cellContour, cuttingEdges[0], cuttingPoints[0], cuttingEdges[1], cuttingPoints[1]);
        allDivisionsNew.push_back(cd);
      }
      if(count == 2 and dis < mindis) {
        mindis = dis;
        shortestPlaneNrml = samplingDirections[i].first;
        shortesPlaneDispl = samplingDirections[i].second;
      }
    }
    std::cout << "simDiv2 " << allDivisionsNew.size() << std::endl;
    allDivisions = allDivisionsNew;

 }



 // takes a DivisionPlane and draws it
 void drawPlane(const Process& process, Mesh *m, DivisionPlane& vtxTris, Point3d scalingNormal, double scalingFac)
 {

  AttrMap<int, Point3d>& planePositions = m->attributes().attrMap<int, Point3d>("Circular Planes Pos");
  AttrMap<int, Point3d>& planeNormals = m->attributes().attrMap<int, Point3d>("Circular Planes Nrml");

  std::set<vertex> vtxExists;
  vvGraph& S = m->graph();

  forall(const vertex &v, S){
    vtxExists.insert(v);
  }

  int label = 0;

  MakeMeshCircleDA ext(process);
  ext.run(m, vtxTris.size, vtxTris.size, 20, 360, 0, 0, true, 100.0, false);
  Matrix3d mat = calcRotMatrix(Point3d(0,0,1), vtxTris.planeNrml);
  forall(const vertex &v, S){
    if(vtxExists.find(v) == vtxExists.end()){
      v->pos = mat*v->pos + vtxTris.planePos;
      v->label = vtxTris.label;
      label = vtxTris.label;

      if(norm(scalingNormal) < 1E-5) continue;

      Point3d pOnPlane = projectPointOnPlane(v->pos, vtxTris.planePos, scalingNormal);
      v->pos = v->pos + (pOnPlane - v->pos)*scalingFac;
    }

  }



      planePositions[label] = vtxTris.planePos;
      planeNormals[label] = vtxTris.planeNrml;

 }

   // Create circular mesh without cells/labels
   bool MakeMeshCircleDA::run(Mesh *mesh, double radiusX, double radiusY, int edgePoints, double meshAngle,
    double shiftX, double shiftY, bool keep, double triSize, bool addEdgePoints)
    {
      MeshBuilder mb;

      // circle segment must be between 0 and 360 deg
      if(meshAngle <=0) return false;
      if(meshAngle > 360) meshAngle = 360;

      if(radiusX <= 0 or radiusY <= 0 or triSize <= 0 or edgePoints < 3)
        return setErrorMessage("Invalid Parameters");

      // init variables
      vvGraph& S = mesh->graph();

      std::vector<Point2f> pList; // for triangulation
      std::vector<Point2i> segList; // for triangulation

      double angleOld = 0;
      double angle = 90;
      int count = 0; // segment counter
      double edgePointIt = edgePoints;

      // if not full circle then it has to be closed with two lines (first one here)
      if(meshAngle < 360){
        for(int i=0; i<edgePointIt/2.; i++){
          Point2f p (0.0, 0.0);
          double step = radiusX/edgePoints*2.;
          p.x()+=step*i;
          pList.push_back(p);//rotatePoint2D(p, rotation));
          segList.push_back(Point2i(count, count+1));
          count++;
         }

       }

      // create a circular/ellipsoid polygon
       Point2f pLast;
      for(int i=0; i<edgePointIt; i++){
        angleOld = angle;
        double step=meshAngle/edgePoints;
        angle+=step;
        double radAngle = M_PI*angleOld/180.0;
        Point2f p (sin(radAngle), cos(radAngle));
        p.x() *= radiusX;
        p.y() *= radiusY;
        pList.push_back(p);//rotatePoint2D(p, rotation));
        segList.push_back(Point2i(count, count+1));
        count++;
        pLast = p;
      }

      // if not full circle then it has to be closed with two lines (the second one here)
      if(meshAngle < 360){
        // add the last point
        Point2f p;
        p.x() = radiusX * cos(-meshAngle/180.*M_PI);
        p.y() = radiusY * sin(-meshAngle/180.*M_PI);
        pList.push_back(p);//rotatePoint2D(p, rotation));
        segList.push_back(Point2i(count, count+1));
        count++;
        pLast = p;

        for(int i=1; i<edgePointIt/2.; i++){
          Point2f p = pLast;
          double fac = 1. - i/edgePointIt*2.;
          p=fac*p;
          pList.push_back(p);//rotatePoint2D(p, rotation));
          segList.push_back(Point2i(count, count+1));
          count++;
         }

       }
      segList[segList.size()-1].y() = 0; // close the segments

      // special mode for 4 edge points: create a rectangular with the specified side length
      if(edgePoints == 4){
        std::vector<Point2f> pListNew; // for triangulation
        std::vector<Point2i> segListNew; // for triangulation

        pListNew.push_back(Point2f(-radiusX, -radiusY));
        pListNew.push_back(Point2f(-radiusX, radiusY));
        pListNew.push_back(Point2f(radiusX, radiusY));
        pListNew.push_back(Point2f(radiusX, -radiusY));

        segListNew.push_back(Point2i(0, 1));
        segListNew.push_back(Point2i(1, 2));
        segListNew.push_back(Point2i(2, 3));
        segListNew.push_back(Point2i(3, 0));

        pList = pListNew;
        segList = segListNew;

      }

      // triangulate the polygon
      std::vector<Point3i> triList;
      std::vector<Point2f> ptList;
      std::vector<Point2i> segtList;

      triangulatePolygon(triSize, pList, segList, ptList, triList, segtList, addEdgePoints, true);

      std::vector<Point3d> ptList3;

      Point3d shift(shiftX, shiftY, 0);

      forall(const Point2f &pt, ptList) { // transfer from 2d to 3d
        Point3d p (pt.x(), pt.y(), 0.0);
        ptList3.push_back(p+shift);
      }

      // create triangle vector
      for(uint i = 0; i< triList.size(); i++) {
        Point3d a(ptList3[triList[i].x()].x(), ptList3[triList[i].x()].y(), ptList3[triList[i].x()].z());
        Point3d b(ptList3[triList[i].z()].x(), ptList3[triList[i].z()].y(), ptList3[triList[i].z()].z());
        Point3d c(ptList3[triList[i].y()].x(), ptList3[triList[i].y()].y(), ptList3[triList[i].y()].z());
        mb.addTri(a,c,b);
      }
      mb.writeVertexVec(false);

      vvGraph Temp = S;
      if(!keep) mesh->reset();

      if(!Mesh::meshFromTriangles(S, mb.vtxVec, mb.triVec)) {
        S = Temp;
        throw(QString("Error, cannot add all the triangles"));
      }

      mesh->updateAll();
      return true;

    }
    REGISTER_PROCESS(MakeMeshCircleDA);


   // Create circular mesh without cells/labels
   bool MakeMeshTetraDA::run(Mesh *mesh, double length, double triSize, bool keep, bool addEdgePoints)
    {
      MeshBuilder mb;

      vvGraph& S = mesh->graph();

      double h = sqrt(3.)/2.;
      double h2 = sqrt(6.)/3.;

      Point3d a(0,0,0);
      Point3d b(1,0,0);
      Point3d c(0.5,h,0);
      Point3d d(0.5,1/3.,h2);

      a*=length;
      b*=length;
      c*=length;
      d*=length;

      mb.addTri(a,b,c);
      mb.addTri(a,b,d);
      mb.addTri(b,c,d);
      mb.addTri(a,b,d);

      mb.writeVertexVec(false);


      vvGraph Temp = S;
      if(!keep) mesh->reset();

      if(!Mesh::meshFromTriangles(S, mb.vtxVec, mb.triVec)) {
        //S = Temp;
        throw(QString("Error, cannot add all the triangles"));
      }

      mesh->updateAll();
      return true;

    }
    REGISTER_PROCESS(MakeMeshTetraDA);


}
