//
// This file is part of MorphoGraphX - http://www.MorphoGraphX.org
// Copyright (C) 2012-2015 Richard S. Smith and collaborators.
//
// If you use MorphoGraphX in your work, please cite:
//   http://dx.doi.org/10.7554/eLife.05864
//
// MorphoGraphX is free software, and is licensed under under the terms of the 
// GNU General (GPL) Public License version 2.0, http://www.gnu.org/licenses.
// 
#include <Triangulate.hpp>
#include <Progress.hpp>

#include <PCA.hpp>

#include <ThirdParty/triangle.h>

namespace mgx 
{
  
  // check the orientation of all triangles and flip them if necessary
  bool triangleOrientationCheck(std::vector<Point3i>& triList, int idxRefTri)
  {
    std::cout << "orientation check" << std::endl;
    if(triList.size() == 0) return true;
    std::vector<bool> triListChecked;
    triListChecked.resize(triList.size(),false);
    typedef std::pair <int,int> edgeT;
    // set the first (or reference) triangle as valid
    std::vector<edgeT> checkedEdges;
    int idxRef = idxRefTri;
    if(idxRef == -1) idxRef = 0;

    triListChecked[idxRef] = true;
    // and also all of its 3 edges
    for(int i = 0; i < 3; ++i) {
      edgeT edge;
      edge.first = triList[idxRef][i];
      if(i==2)
        edge.second = triList[idxRef][0];
      else
        edge.second = triList[idxRef][i+1];

      checkedEdges.push_back(edge);
    }
  
    // check all other tris
    int remainingTris = 1;
    int remainingTrisLast = 2;
    int remainingTrisLast2 = 3; // count remaining tri in the last 3 steps to see whether there is progress
    int triNr = triList.size();
    //std::cout << "orientation check: main loop" << std::endl;
    while(remainingTris>0 and (remainingTrisLast != remainingTris and remainingTris !=remainingTrisLast2)){ // not all edges checked
      //std::cout << "orient  " << remainingTris << "/" << remainingTrisLast << "/" << remainingTrisLast2 << std::endl;
      //std::cout << "orientation check: " << remainingTris << std::endl;
      remainingTrisLast2 = remainingTrisLast;
      remainingTrisLast = remainingTris;
      remainingTris = 0;
      
      for(int i = 0; i < triNr; ++i) { // look for first tri that has an unresolved edge

        //cout << "tri" << triListChecked[i] << "/" << triList[i] << endl;

        if(!triListChecked[i]){ // check this triangle (not yet checked)
          remainingTris++; // count as unchecked, will cause another loop

          edgeT edge1, edge2, edge3;

          edge1.first = triList[i][0];
          edge1.second = triList[i][1];
          edge2.first = triList[i][1];
          edge2.second = triList[i][2];
          edge3.first = triList[i][2];
          edge3.second = triList[i][0];
          // get edges
          for(int j = 0; j < 3; ++j) {
            edgeT edge;
            if(j==0)
              edge = edge1;
            else if(j==1)
              edge = edge2;
            else if(j==2)
              edge = edge3;

            forall(const edgeT &validEdge, checkedEdges){
            // compare the current unchecked tri whether it neighbors an already checked one
              if(!triListChecked[i]){
                if(edge.first == validEdge.second and edge.second == validEdge.first){
                  // edge in current tri in opposite direction compared to checked tri -> everything fine
                  // set tri as checked
                  triListChecked[i] = true;
                  // set edges as valid
                  checkedEdges.push_back(edge1);
                  checkedEdges.push_back(edge2);
                  checkedEdges.push_back(edge3);
                } else if(edge == validEdge){
                  // edge in current tri in same direction compared to checked tri -> flip it
                  // flip entry in tri table
                  int h = triList[i][0];
                  triList[i][0] = triList[i][1];
                  triList[i][1] = h;
                  // set tri as checked
                  triListChecked[i] = true;
                  // flip all the edges                
                  int hh = edge.first;
                  edge.first = edge.second;
                  edge.second = hh;
                  edge1.first = triList[i][0];
                  edge1.second = triList[i][1];
                  edge2.first = triList[i][1];
                  edge2.second = triList[i][2];
                  edge3.first = triList[i][2];
                  edge3.second = triList[i][0];
                  checkedEdges.push_back(edge1);
                  checkedEdges.push_back(edge2);
                  checkedEdges.push_back(edge3);
                } else { // nothing found -> do nothing
                }
              }
            } // forall(const edgeT &validEdge, checkedEdges)
          }
        }
      }
    } //while(!doneChecking)
    if(remainingTris>0)
      std::cout << "some triangle orientations could be wrong!" << std::endl;
    return true;
  }
  
  mgx_EXPORT Point3d getPolygonNrml(std::vector<Point3d>& pointsPolygon, int& idxP3)
  {
    Point3d normal(0,0,0);

    // 3 or less points always planar
    if(pointsPolygon.size() < 3) return normal;
  
    // take first three points, calc normal
    Point3d p1 = pointsPolygon[0];
    Point3d p2 = pointsPolygon[1];
    Point3d p3;
  
    // make sure 3rd point is not in the same line
    for(uint i = 2; i<pointsPolygon.size(); i++){
      p3 = pointsPolygon[i];
      idxP3 = i;
      normal = (p2-p1) % (p3-p1);
      if(norm(normal)>0.01){
        i=pointsPolygon.size();
      }
    }
    normal /= norm(normal);
    return normal;
  }
  mgx_EXPORT Point3d getPolygonNrml(std::vector<Point3d>& pointsPolygon)
  {
    int i;
    return getPolygonNrml(pointsPolygon,i);
  }


  // test whether inputVertices are in one plane
  mgx_EXPORT bool isDataPlanar(std::vector<Point3d>& inputVertices)
  {
    // 3 or less points always planar
    if(inputVertices.size() < 3)
      return true;

    int idxP3;
    Point3d normal = getPolygonNrml(inputVertices, idxP3);
    Point3d p1 = inputVertices[0];
    Point3d p2 = inputVertices[1];
    Point3d p3 = inputVertices[idxP3];

  /*
    // take first three points, calc normal
    Point3d p1 = inputVertices[0];
    Point3d p2 = inputVertices[1];
    Point3d p3;
    Point3d normal;
  
    // make sure 3rd point is not in the same line
    for(int i = 2; i<inputVertices.size(); i++){
      p3 = inputVertices[i];
      normal = (p2-p1) % (p3-p1);
      if(norm(normal)>0.01){
        i=inputVertices.size();
        //std::cout << "normal " << p1 << "/" << p2 << "/" << p3 << "/" << normal << std::endl;
      }
    }
  
    if(normal == Point3d(0,0,0))
      return true;
  
  
    normal /= norm(normal);
  */
    //bool notPlan = false;
  
    // compare to all other points
    forall(const Point3d& p, inputVertices){
      if(p!=p1 and p!=p2 and p!=p3){
        Point3d currentP = (p-p1);
        currentP/=norm(currentP);
        float scalar = normal * currentP;
   
        if(scalar > 0.01 or scalar < -0.01){
          //notPlan = true;
          return false;
          //std::cout << "NOT PLANAR " << scalar << "/" << currentP << "/" << normal << std::endl;
        }
          //return false;
      }
    }
    //if(notPlan)
    //  return false;
  
    return true;
  }


  // copied from pdgs
  void inplaceMatrixMultiply(std::vector<Point3d>& P, const Matrix3d& M)
  {
    int nb_rows = P.size();
    for(int i = 0; i < nb_rows; ++i)
      P[i] = P[i] * M;
  }
  
  // takes a vector of points (ordered) of a polygon and subdivides the segments until no segment is greater maxLength
  // rewrites the point list and the segment list
  void subdivideBorderSegs(std::vector<Point2f> &pList, std::vector<Point2i> &segList, float maxLength)
  {
    float maxSegLength = 1E20;
  
    std::vector<Point2f> pListNew;
    pListNew = pList;
  
    while(maxSegLength > maxLength){
  
      maxSegLength = 0;
      std::vector<Point2f> pListTemp;
  
      // go through all segments, measure them, half them if necessary
      int pListSize = pListNew.size();
      Point2f last = pListNew[pListSize-1]; // always needs 2 points for a segment
      for(int i = 0; i < pListSize; i++){
        float segLength = norm(last - pListNew[i]);
  
        if(segLength > maxLength){ //do the subdivide
          Point2f subdividedPoint = (last + pListNew[i])/2.0;
          pListTemp.push_back(subdividedPoint);
        }
        pListTemp.push_back(pListNew[i]);
        //} else {
        //  //do nothing
        //  pListTemp.push_back(pListNew[i]);
  
  //      }
  
        if(maxSegLength < segLength) maxSegLength=segLength; // find the maximum remaining segLength
        last = pListNew[i];
      }
      pListNew = pListTemp;
  
  
    }
    // write the segList again
    std::vector<Point2i> segListNew;
    int pListSize = pListNew.size();
    for(int i = 0; i < pListSize; i++){
      if(i<pListSize-1)
        segListNew.push_back(Point2i(i,i+1));
      else
        segListNew.push_back(Point2i(i,0));
    }
  
    segList = segListNew;
    pList = pListNew;
  }

  // takes a set of points (the corner points of a polygon) and finds its principal components, i.e., a best-fit plane
  bool findPolygonPlane(std::vector<Point3d> &pointsPolygon, Point3d &planePos, Point3d &planeNrml)
  {
    std::map<int, Matrix3f> Correlations;
    Matrix3f totCorr;

    int nrPoints = pointsPolygon.size();

    planePos = Point3d(0,0,0);

    // first calculate the center of the polygon
    for(int i = 0; i<nrPoints; i++){
      planePos+=pointsPolygon[i];
    }
    planePos /= nrPoints;

    // now build the correlation matrix
    for(int i = 0; i<nrPoints; i++){
      Point3d grad = pointsPolygon[i] - planePos;

      Matrix3f corr;
      for(int k = 0; k < 3; k++)
        for(int l = 0; l < 3; l++)
          corr(k,l) = grad[k] * grad[l];

      Correlations[i] += corr;
      totCorr+=corr;
    }

    Point3d pd1, pd2;

    // Do the PCA
    PCA pca;
    if(!pca(totCorr)){
      std::cout << "Error performing PCA"; return false;}
      
    pd1 = Point3d(pca.p1);
    pd2 = Point3d(pca.p2);


    planeNrml = pd1 % pd2;

    return true;
  }

  // project a vector of points onto a plane
  bool projectPointsOnPlane(std::vector<Point3d> &points, Point3d planePos, Point3d planeNrml)
  {
    std::vector<Point3d> pointsPolygon;

    forall(const Point3d &p, points) {
      double sN;
      Point3d intersectN;
      planeLineIntersect(planePos, planeNrml, p, p-planeNrml, sN, intersectN);
      pointsPolygon.push_back(intersectN);
    }
  
    points = pointsPolygon;

    return true;
  }

  Matrix3d calcRotMatrix(Point3d nrmlStart, Point3d nrmlEnd, Point3d rot_axis)
  {
    if(norm(rot_axis) < 0.0001) rot_axis = nrmlStart % nrmlEnd;

    double cos_angle, sin_angle;
    sin_angle = norm(rot_axis);
    cos_angle = nrmlStart * nrmlEnd;
    double angle_rot = atan2(sin_angle, cos_angle);


    if(fabs(sin_angle) < 1e-8)
      rot_axis = Point3d(1, 0, 0);
    else
      rot_axis.normalize();

    return Matrix3d::rotation(rot_axis, angle_rot);
  }

  // // // Rotate polygon into xy-plane, code copied from pdgs // // //
  void rotatePointsIntoXY(Point3d polygonNormal, std::vector<Point3d>& pointsPolygon, Matrix3d& inv_rot_polygon_plane)
  {
    
    // Rotate polygon plane to align it with (x,y) plane
    Point3d z_axis(0, 0, 1);

    // get the rotation matrix
    inv_rot_polygon_plane = calcRotMatrix(z_axis, -polygonNormal);

    // do the rotation
    inplaceMatrixMultiply(pointsPolygon, inv_rot_polygon_plane);

  }

   // takes a vector of 3d points of a CONVEX polygon in space (otherwise the segments have to be in order!), rotates it into the XY-plane, triangulates it, and rotates it back
   // check hpp for more documentation
  bool triangulatePolygon3D(float maxArea, Point3d polygonNormal, std::vector<Point3d> &pointsPolygon, 
    std::vector<Point3i> &triList, std::vector<Point3d> &ptList, bool boundary, bool inOrder, bool pointsCenter)
  {
      //std::cout << "tri3d " << pointsPolygon.size() << "/" << !isDataPlanar(pointsPolygon) << "/" << std::endl;
    if(pointsPolygon.size() < 3 or !isDataPlanar(pointsPolygon) or maxArea<=0)
      return false;
  
    Matrix3d inv_rot_polygon_plane1;
    rotatePointsIntoXY(polygonNormal, pointsPolygon, inv_rot_polygon_plane1);
  /* this is the old rotation code, now separated out in rotatePointsIntoXY and calcRotMatrix
    // // // Rotate polygon into xy-plane, code copied from pdgs // // //
    Point3d normal_polygon1 = -polygonNormal;
    std::vector<Point3d> polygon_proj1 = pointsPolygon;
  
    // Rotate polygon plane to align it with (x,y) plane
    Point3d z_axis(0, 0, 1);
    double cos_angle1, sin_angle1, angle_rot_plane1;
    Point3d rot_axis_plane1 = normal_polygon1 % z_axis;   // cross product,  ||normalxZaxis||=sin(angle rotation)
    sin_angle1 = norm(rot_axis_plane1);
    cos_angle1 = normal_polygon1 * z_axis;   // dot product,  normal.Zaxis=cos(angle rotation)
    angle_rot_plane1 = atan2(sin_angle1, cos_angle1);
  
    // which means: if rot_axis_plane is perfectly aligned with Z axis
    if(fabs(sin_angle1) < 1e-8)
      rot_axis_plane1 = Point3d(1, 0, 0);
    else
      rot_axis_plane1.normalize();
  
    // with angle= angle between normal and z axis
    Matrix3d inv_rot_polygon_plane1 = Matrix3d::rotation(rot_axis_plane1, -angle_rot_plane1);
    Matrix3d rot_polygon_plane1 = Matrix3d::rotation(rot_axis_plane1, angle_rot_plane1);
    inplaceMatrixMultiply(polygon_proj1, inv_rot_polygon_plane1);
  */
    std::vector<Point2f> pList(pointsPolygon.size()); // for triangulation
    std::vector<Point2f> pListCorrected(pointsPolygon.size()); // for triangulation
    std::vector<Point2i> segList; // for triangulation
  
    double pListZ = 0.;
  
  
    // calc polygon center
    int polySize = pointsPolygon.size();
    Point2f polyCenter (0.0,0.0);
  
    for(int i = 0; i < polySize; ++i) {
      pList[i] = Point2f(pointsPolygon[i].x(), pointsPolygon[i].y());
      pListCorrected[i] = pList[i];
      pListZ = pointsPolygon[i].z();
      polyCenter += pList[i];
      segList.push_back(Point2i(i, i+1));
      //std::cout << "triangulatehpp " << pList[i] << std::endl;
    }
    polyCenter /= polySize;
    segList[polySize-1].y() = 0;
  
    if(!inOrder){ // not in order
      // now order the segments
      std::vector<Point2i> segListNew;

      // now calculate the angle between the points and the center
      Point2f yAxis (0.0,1.0);
      Point2f xAxis (1.0,0.0);
  
      std::map<double, Point2f> posSin, negSin;
      typedef std::pair<double, Point2f> DoubleP2f;
  
      for(int i = 0; i < polySize; ++i) {
  
        Point2f currentVec = pList[i] - polyCenter;
        double cosAngle = (currentVec * yAxis)/norm(currentVec);
        double sinAngle = (currentVec * xAxis)/norm(currentVec);
  
        if(sinAngle >= 0){
          posSin[cosAngle] = pList[i];
        } else {
          negSin[-cosAngle] = pList[i];
        }
  
      }
  
      int count = 0;
    
      // now walk around the ordered sine values of the points and add them to the pList and segLists
      forall(const DoubleP2f &p, posSin){ // first the positive sine values
        segListNew.push_back(Point2i(count, count+1));
        pListCorrected[count] = p.second;
        count++;
      } 
      forall(const DoubleP2f &p, negSin){ // now the negative sine values
        segListNew.push_back(Point2i(count, count+1));
        pListCorrected[count] = p.second;
        count++;
      }
      // change last segment to connect to the first
      segListNew[segListNew.size()-1].y() = 0;

      segList = segListNew;

    } // end if !inOrder

    if(boundary) // subdivide the border segments if they are too long
      subdivideBorderSegs(pListCorrected, segList, sqrt(maxArea*2.0));


    // triangulate the polygon
    std::vector<Point2f> ptList2;
    std::vector<Point2i> segtList;

    //std::cout << "triangulate " << pListCorrected.size() << std::endl;
  
    if(pListCorrected.size()>2)
      triangulatePolygon(maxArea, pListCorrected, segList, ptList2, triList, segtList, false /*boundary*/, pointsCenter /*add points center*/);
  
    //std::cout << "triangulated " << ptList2.size() << std::endl;

    std::vector<Point3d> ptList3;
    std::vector<Point3d> ptListBoundary;
  
    // rotate back
    forall(const Point2f &pt, ptList2) {
      Point3d p (pt.x(), pt.y(), pListZ);
      p=p*inverse(inv_rot_polygon_plane1);
      ptList3.push_back(p);
    }
    forall(const Point2f &pt, pListCorrected) {
      Point3d p (pt.x(), pt.y(), pListZ);
  
      p=p*inverse(inv_rot_polygon_plane1);
  
  
      ptListBoundary.push_back(p);
    }
  
    ptList = ptList3;
    pointsPolygon = ptListBoundary;
    return true;
  
  } 


  // swap two elements with index i and j
  void swapElements(std::vector<std::pair<Point3d,Point3d> > &polygonSegs, int i, int j)
  {
    std::pair<Point3d,Point3d> p;
    p = polygonSegs[i];
    polygonSegs[i] = polygonSegs[j];
    polygonSegs[j] = p;
  }

  // swap the two elements of a pair
  void swapPair(std::pair<Point3d,Point3d> &pair)
  {
    Point3d p;
    p = pair.first;
    pair.first = pair.second;
    pair.second = p;
  }

  // order segments so that they connect
  std::vector<Point3d> orderPolygonSegs(std::vector<std::pair<Point3d,Point3d> > &polygonSegs, bool keepPolySegs)
  {
    std::vector<Point3d> result;

    Point3d startSeg;
    int stop = 0;
    double eps = 0.00001;

    if(polygonSegs.size() < 1) return result;
    startSeg = polygonSegs[0].first;

    // order the polygon segs, start with the first and look for connections
    for(uint i = 0; i<polygonSegs.size(); i++){
      for(uint j = i+1; j<polygonSegs.size(); j++){
        if(norm(polygonSegs[i].second - polygonSegs[j].first)<eps){ // connection found
          swapElements(polygonSegs, i+1, j);
          j = polygonSegs.size();
        } else if(norm(polygonSegs[i].second - polygonSegs[j].second)<eps){ // turn segment around, connection found
          swapPair(polygonSegs[j]);
          swapElements(polygonSegs, i+1, j);
          j = polygonSegs.size();
        }
      }
      stop = i+1;
      //std::cout << polygonSegs[i].first << "/" << polygonSegs[i].second << std::endl; // debug output
      if(norm(startSeg - polygonSegs[i].second)<eps){ // abort, circle found
        i = polygonSegs.size();
      } 
    }

    //result.push_back(polygonSegs[0].first);
    for(int i = 0; i<stop; i++){
      result.push_back(polygonSegs[i].second);
    }

    std::vector<std::pair<Point3d,Point3d> > polygonSegsRemain;
    for(uint i = stop; i<polygonSegs.size(); i++){
      polygonSegsRemain.push_back(polygonSegs[i]);
    }
    if(!keepPolySegs)
      polygonSegs = polygonSegsRemain;

    return result;
  }


  // order segments so that they connect
  // find multiple parts
  std::vector<std::vector<Point3d> > orderPolygonSegsMulti(std::vector<std::pair<Point3d,Point3d> > &polygonSegs)
  {
    std::vector<std::vector<Point3d> > result;

    std::vector<std::pair<Point3d,Point3d> > polygonSegsLocal;

    for(size_t i = 0; i<polygonSegs.size(); i++){
      std::pair<Point3d,Point3d> p = std::make_pair(polygonSegs[i].first, polygonSegs[i].second);
      polygonSegsLocal.push_back(p);
    }

    while(!polygonSegsLocal.empty()){
      std::vector<Point3d> currentPolygon;
      currentPolygon = orderPolygonSegs(polygonSegsLocal, false);
      result.push_back(currentPolygon);
    }
    return result;
  }


  //calculate a orthogonal vector to a given input vector

  Point3d orthogonalVector(Point3d vec)
  {
    double eps = 0.001;

    double n = 0.;
    Point3d orth (0.,0.,0.);
    if(std::abs(vec.z())>eps){
      n = (-vec.x()-vec.y())/vec.z();
      orth = Point3d(1.,1.,n);
      //std::cout << "b1 " << n << "/" << orth << std::endl;
    } else if(std::abs(vec.y())>eps){
      n = (-vec.x()-vec.z())/vec.y();
      orth = Point3d(1.,n,1.);
      //std::cout << "b2 " << n << "/" << orth << std::endl;
    } else if(std::abs(vec.x())>eps){
      n = (-vec.z()-vec.y())/vec.x();
      orth = Point3d(n,1.,1.);
      //std::cout << "b3 " << n << "/" << orth << std::endl;
    }

    orth /= norm(orth);

    if(abs(orth*vec)>eps) Information::out << "Problem in orthogonalVector (Triangulate.cpp)" << endl;

    return orth;
  }

  Point3d calcNearestPointOnBezierGrid(const Point3d& targetPoint, std::vector<std::vector<Point3d> >& bezier, Point2i& bezIdx)
  {

  Point3d result (0,0,0);

  double minDis = HUGE_VAL;
  for(uint i = 0; i<bezier.size(); i++){
    for(uint j = 0; j<bezier[i].size(); j++){
      double currentDistance = norm(bezier[i][j]-targetPoint);
      if(currentDistance < minDis){
        minDis = currentDistance;
        bezIdx = Point2i(i,j);
        result = bezier[i][j];
      }
    }
  }

  return result;
  }


// copied from cellmakertools

  // this comes from MeshProcessPDG.cpp
  // project Point3d on plane:
  // P=Point to project (Point3d), Q=Point of the plane(Point3d),N=Normal to the plane(point3d), norm(N)=1
  //Point3d projectPointOnPlane(const Point3d& P, const Point3d& Q, const Point3d& N) {
  //  return P - ((P - Q) * N) * N;
  //}



Matrix3d calcProjectedGrowthTensor(Matrix3d tensor, Point3d planeNrml){

  Matrix3d rotMat;

  Point3d vec = orthogonalVector(planeNrml);
  Point3d vec2 = planeNrml % vec;

  rotMat[0][0] = vec.x();
  rotMat[0][1] = vec.y();
  rotMat[0][2] = vec.z();

  rotMat[1][0] = vec2.x();
  rotMat[1][1] = vec2.y();
  rotMat[1][2] = vec2.z();

  rotMat[2][0] = planeNrml.x();
  rotMat[2][1] = planeNrml.y();
  rotMat[2][2] = planeNrml.z();

  //std::cout << "rot " << cellLabel << "/" << vec << "/" << vec2 << "/" << nrml << std::endl;

  // rotate the tensor
  tensor = rotMat * tensor * transpose(rotMat);

  // get rid of the z-dimension as we only keep the information relevant for the cell plane
  tensor[0][2] = 0;
  tensor[1][2] = 0;
  tensor[2][0] = 0;
  tensor[2][1] = 0;
  tensor[2][2] = 0;

  // rotate it back into the max/min directions
  tensor = transpose(rotMat) * tensor * rotMat;

  return tensor;
}

// implementation of point in polygon
// source: https://www.geeksforgeeks.org/how-to-check-if-a-given-point-lies-inside-a-polygon/

// To find orientation of ordered triplet (p, q, r). 
// The function returns following values 
// 0 --> p, q and r are colinear 
// 1 --> Clockwise 
// 2 --> Counterclockwise 
int orientation(Point3d p, Point3d q, Point3d r) 
{ 
    int val = (q.y() - p.y()) * (r.x() - q.x()) - 
              (q.x() - p.x()) * (r.y() - q.y()); 
  
    if (val == 0) return 0;  // colinear 
    return (val > 0)? 1: 2; // clock or counterclock wise 
} 

// Given three colinear points p, q, r, the function checks if 
// point q lies on line segment 'pr' 
bool onSegment(Point3d p, Point3d q, Point3d r) 
{ 
    if (q.x() <= max(p.x(), r.x()) and q.x() >= min(p.x(), r.x()) and q.y() <= max(p.y(), r.y()) and q.y() >= min(p.y(), r.y())) 
      return true; 
    return false; 
} 

 
// The function that returns true if line segment 'p1q1' 
// and 'p2q2' intersect. 
bool doIntersect(Point3d p1, Point3d q1, Point3d p2, Point3d q2) 
{ 
    // Find the four orientations needed for general and 
    // special cases 
    int o1 = orientation(p1, q1, p2); 
    int o2 = orientation(p1, q1, q2); 
    int o3 = orientation(p2, q2, p1); 
    int o4 = orientation(p2, q2, q1); 
  
    // General case 
    if (o1 != o2 and o3 != o4) 
        return true; 
  
    // Special Cases 
    // p1, q1 and p2 are colinear and p2 lies on segment p1q1 
    if (o1 == 0 and onSegment(p1, p2, q1)) return true; 
  
    // p1, q1 and p2 are colinear and q2 lies on segment p1q1 
    if (o2 == 0 and onSegment(p1, q2, q1)) return true; 
  
    // p2, q2 and p1 are colinear and p1 lies on segment p2q2 
    if (o3 == 0 and onSegment(p2, p1, q2)) return true; 
  
     // p2, q2 and q1 are colinear and q1 lies on segment p2q2 
    if (o4 == 0 and onSegment(p2, q1, q2)) return true; 
  
    return false; // Doesn't fall in any of the above cases 
} 
  
// // Returns true if the point p lies inside the polygon[] with n vertices 
// bool isInside(Point polygon[], int n, Point p) 
// { 
   
// } 

  bool pointInPolygon(Point3d pointToTest, std::vector<Point3d>& polygon, bool debug)
    {

 // There must be at least 3 vertices in polygon[] 
    if (polygon.size() < 3)  return false; 

    pointToTest.z() = 0;
  
    double minX = 1E20;
    double maxX = -1E20;

    forall(Point3d p, polygon){
      if(minX > p.x()) minX = p.x();
      if(maxX < p.x()) maxX = p.x();
    }

    // quick check
    if(minX > pointToTest.x() or maxX < pointToTest.x()) return false;


    // Create a point for line segment from p to infinite 
    Point3d extreme(pointToTest.x()+maxX-minX, pointToTest.y(),0); 
  
    // Count intersections of the above line with sides of polygon 
    int count = 0; 

    Point3d seg1 = polygon[polygon.size()-1];
    for(uint i = 0; i < polygon.size(); i++){
      Point3d seg2 = polygon[i];

      if(debug){
        double s, u;
        Point3d in;
        lineLineIntersect(seg1, seg2, pointToTest, extreme, s, u, in);
        std::cout << "deb2 " << seg1 << "/" << seg2 << "/" << pointToTest << "/" << extreme << std::endl;
        std::cout << "c " << count << std::endl;
      
        std::cout << "deb " << lineLineIntersect(seg1, seg2, pointToTest, extreme, s, u, in) << "/" << in  << "/" << s << "/" << u << "/" << doIntersect(seg1, seg2, pointToTest, extreme) << "/" << orientation(seg1, pointToTest, seg2) << "/" << onSegment(seg1, pointToTest, seg2) << std::endl;
      }

      double s=0, u=0;
      Point3d in(0,0,0);
      bool r = lineLineIntersect(seg1, seg2, pointToTest, extreme, s, u, in);

      if (r and s <= 1 and s >= 0 and u <= 1 and u >= 0){
        //If the point 'p' is colinear with line segment 'i-next', 
        // then check if it lies on segment. If it lies, return true, 
        // otherwise false 
        
        //if (orientation(seg1, pointToTest, seg2) == 0) 
        //  return onSegment(seg1, pointToTest, seg2); 
        
        count++;
      }

      seg1 = seg2;
    }

    if(debug) std::cout << "c " << count << std::endl;

    return (count%2 == 1);
    }


bool lloydKMeans(std::vector<std::vector<double> >& inputClusters, 
  std::vector<AttrMap<int, double> >& dataPoints, int maxStep, double convergenceThreshold)
{

  std::vector<std::vector<double> > outputClusters;

  double dimensions = 0;

  if(inputClusters.size() < 1) return false;
  dimensions = inputClusters[0].size();

  // initialize clusters
  for(uint i = 0; i<inputClusters.size(); i++){
    if(dimensions != inputClusters[i].size()) return false;
    outputClusters.push_back(inputClusters[i]);
  }
  // check if dimensions match
  if(dataPoints.size() != dimensions) return false;

  // get all Labels from the first data map
  std::set<int> allLabels;
  typedef std::pair<int, double> IntDoubleP;
  forall(IntDoubleP p, dataPoints[0]){
    allLabels.insert(p.first);
  }


  std::map<int, int> labelClusterIdxMap;   // map from cell label to cluster number to keep track of changing ownerships
  int steps = 0;
  int changed = dataPoints.size();

  // loop to update clusters
  while(changed > dataPoints.size()*convergenceThreshold and maxStep!=0) { // stop when 0 steps left or until at least 99% of cells do not change ownership (if maxStep=-1)

    changed = 0;
    std::map<int, std::pair<std::vector<double>, int> > clusterIdxNewPointMap;

    //    std::cout << "lloyd41" << std::endl;

    // calc cluster for all points
    // go through all cell labels
    forall(int label, allLabels){

      //int oldIdx = labelClusterIdxMap[label];
      
//  std::cout << "lloyd411 " << label << "/" << dimensions << "/" << outputClusters.size() << std::endl;
    // find nearest cluster for all points
      double minDis = 1E20;
      int minClusterIdx = -1;
      for(size_t k=0; k<outputClusters.size(); k++){
        double dis = 0;
        for(size_t j=0; j<dimensions; j++){
          double v = dataPoints[j][label] - outputClusters[k][j];
          dis += v*v;
           // std::cout << "l " << dataPoints[j][label] << "/" << outputClusters[k][j] << std::endl;
        }
        //dis = sqrt(dis);
        if(dis < minDis){
          minDis = dis;
          minClusterIdx = k;
        }
      }
 // std::cout << "lloyd412 " << dimensions << "/" << outputClusters.size() << std::endl;
      if(minClusterIdx != labelClusterIdxMap[label]){
        labelClusterIdxMap[label] = minClusterIdx;
        changed++;
      }

      // update clusters with their points
      if(clusterIdxNewPointMap[minClusterIdx].second == 0){
        std::vector<double> v(dimensions);
        clusterIdxNewPointMap[minClusterIdx].first = v;
      }

      for(size_t j=0; j<dimensions; j++){
        clusterIdxNewPointMap[minClusterIdx].first[j]+=dataPoints[j][label];
      }
      clusterIdxNewPointMap[minClusterIdx].second++;
    }
 // std::cout << "lloyd42" << std::endl;
    // update cluster values
    std::vector< std::vector<double> > outputClustersNew;
    for(size_t i =0; i<outputClusters.size(); i++){
      if(clusterIdxNewPointMap[i].first.size() == 0) continue;
      std::vector<double> newClusterValue;

      for(size_t j=0; j<dimensions; j++){
        newClusterValue.push_back(clusterIdxNewPointMap[i].first[j]/clusterIdxNewPointMap[i].second);
      }
       // std::cout << "lloyd421 " << newClusterValue.size() << "/" << newClusterValue[0] << "/" << newClusterValue[1] << "//" << outputClusters[i].size() << std::endl;
      outputClusters[i] = newClusterValue;
      //outputClustersNew.push_back(newClusterValue);
     //   std::cout << "lloyd422" << std::endl;
    }
    //outputClusters = outputClustersNew;
 // std::cout << "lloyd43" << std::endl;
    if(maxStep>=0) maxStep--;
    steps++;
  }
  
  std::cout << "lloyd finished after " << steps << " optimization steps" << std::endl;
  inputClusters = outputClusters;
  return true;
}

std::vector<Point2d> lloydKMeans2D(std::vector<Point2d>& clusters, std::vector<Point2d>& data, int maxStep)
{
//cout << "lloyd1" << endl;
  std::vector<Point2d> clustersNew;

  // initialize clusters
  for(uint i = 0; i<clusters.size(); i++){
    clustersNew.push_back(clusters[i]);
  }

  std::map<int, int> pointClusterIdxMap;

  int changed;
//cout << "lloyd2" << endl;
  // loop to update clusters
  do {
  //  cout << "lloyd do" << endl;
    changed = 0;
    std::map<int, std::pair<Point2d, int> > clusterIdxNewPointMap;

    for(size_t i =0; i<data.size(); i++){
      Point2d p = data[i];
      int oldIdx = pointClusterIdxMap[i];
    // find nearest cluster for all points
      double minDis = 1E20;
      int minClusterIdx = -1;
      for(size_t j =0; j<clustersNew.size(); j++){
        Point2d pc = clustersNew[j];
        double dis = norm(p-pc);
        if(dis < minDis){
          minDis = dis;
          minClusterIdx = j;
        }
      }
      if(minClusterIdx != oldIdx){
        pointClusterIdxMap[i] = minClusterIdx;
        changed++;
      }
      // update clusters with their points
      clusterIdxNewPointMap[minClusterIdx].first+=p;
      clusterIdxNewPointMap[minClusterIdx].second++;
    }
    std::vector<Point2d> newClusters;
    for(size_t i =0; i<clustersNew.size(); i++){
      //Point2d p = clustersNew[i];
      newClusters.push_back(clusterIdxNewPointMap[i].first/clusterIdxNewPointMap[i].second);
    }
    clustersNew = newClusters;
    maxStep--;
  } while(changed > data.size()/100. and maxStep>0);


  return clustersNew;

}


// interpolates the apropriate array index for the landscape function which has a resolution of newRange+1 
double interpolateArrayIndex(double value, double min, double max, double newRange)
{
  if(value >= max){
    return newRange;
  } else if(value <= min){
    return 0;
  } else {
    return ((value - min)/(max - min) * newRange);
  }
}
  
// draw a cross according to the parameters (used for generating the heatmap)
void drawCross(QImage& image, int x, int y, int size, int crossSize, QRgb color)
  {
    for(int i = -crossSize; i <= crossSize; i++) {
      image.setPixel(std::min(std::max(0,x+i),size-1), y, color);
      image.setPixel(x, std::min(std::max(0,y+i),size-1), color);
    }
  }

// draw a circle according to the parameters (used for generating the heatmap)
void drawCircle(QImage& image, int x, int y, int size, QRgb color)
  {
    int circleSize = 6;
    Point2d currentPoint(x,y);
    for(int i = -circleSize; i <= circleSize; i++) {
      for(int j = -circleSize; j <= circleSize; j++) {
        Point2d testPoint(i+x,j+y);
        if((int)sqrt(i*i+j*j)==circleSize)
          image.setPixel(std::min(std::max(0,x+i),size-1), std::min(std::max(0,y+j),size-1), color);
      }
    }
  }

// calculates the value of a two dimensional gaussian function at location (x,y)
double gauss2D(double x, double y, double muX, double muY, double sigmaX, double sigmaY)
{
  //return (1.0/(2.0*3.14159*sigmaX*sigmaY)*exp(-0.5*((x-muX)*(x-muX)/sigmaX/sigmaX+(y-muY)*(y-muY)/sigmaY/sigmaY)));
  return (exp(-0.5*((x-muX)*(x-muX)/sigmaX/sigmaX+(y-muY)*(y-muY)/sigmaY/sigmaY)));
}



// function to automatically assign the colours in the heatmap
QRgb calcRGB(double maxValue, double currentValue)
  {
    if(maxValue < 1E-20) maxValue = 1;
    QRgb value;
    int r = 0, b = 0, g = 0, a = 0;
    int red = 0, green = 0, blue = 0;

    if(currentValue<maxValue/5.0){
      blue = (currentValue)/maxValue*5.0*255.0;
    } else if(currentValue<maxValue*2.0/5.0){
      blue = 255;
      green = (currentValue-maxValue*1.0/5.0)/maxValue*5.0*255.0;
    } else if(currentValue<maxValue*3.0/5.0){
      blue = (maxValue*3.0/5.0-currentValue)/maxValue*5*255.0;
      green = 255;
    } else if(currentValue<maxValue*4.0/5.0){
      green = 255;
      red = (currentValue-maxValue*3.0/5.0)/maxValue*5.0*255.0;
    } else {
      green = (maxValue-currentValue)/maxValue*5.0*255.0;
      red = 255;
    }
    // assure the range from 0 to 255
    r = std::max(0,std::min(255,red));
    g = std::max(0,std::min(255,green));
    b = std::max(0,std::min(255,blue));
    a = 255;

    value = qRgba(r, g, b, a);
    return value;
  }

QRgb getColorFromLabel(int label)
{
  QRgb col = qRgba(255,255,255,255);
  int r,g,b;
//   for(size_t i = 0; i < ImgData::LabelColors.size(); ++i)
//    cout << "Color: " << ImgData::LabelColors[i] << endl;
  if((uint)label < ImgData::LabelColors.size() and label > 0){
    r = ImgData::LabelColors[label].r() * 255;
    b = ImgData::LabelColors[label].b() * 255;
    g = ImgData::LabelColors[label].g() * 255;
    col = qRgba(r,g,b,255);
  }


  return col;
}

 double angleVectors(Point3d v1, Point3d v2, bool directed)
 {
  double result = 0;

  double normV1 = norm(v1);
  double normV2 = norm(v2);

  if(normV1 < 1E-5 or normV2 < 1E-5) return -1;

  result = acos((v1*v2)/normV1/normV2) * 180./M_PI;

  if(!directed and result < 0) result = -result;
  if(!directed and result > 90) result = 180-result;
  return result;
 }


  Point3d calcNearestPointOnBezierLine(const Point3d& targetPoint, std::vector<Point3d>& bezierVec, int& bezIdx, double& bezWeight)
  {

  Point3d result (0,0,0);
  int bezSize = bezierVec.size();

  double minDis = HUGE_VAL;
  for(int j=1; j<bezSize; j++){
    double currentDistance = distLinePoint(bezierVec[j-1], bezierVec[j], targetPoint, true);
    if(currentDistance < minDis){
      minDis = currentDistance;
      // interpolate the discretized bezier for more accurate arclength
      double a = norm(bezierVec[j-1] - targetPoint);
      double b = currentDistance;
      bezWeight = 0;
      double aabb = std::max(0.,a*a-b*b);
      bezIdx = j;
      bezWeight = (double)(std::sqrt(aabb))/(double)(norm(bezierVec[j]-bezierVec[j-1]));
      if(bezWeight > 1) bezWeight = 1;
    }
  }
  result = (1-bezWeight) * bezierVec[bezIdx-1] + bezWeight * bezierVec[bezIdx];

  return result;
  }


  Point3d calcNearestPointOnBezierLine(const Point3d& targetPoint, std::map<int, Point3d>& bMap, int& bezIdx, double& bezWeight)
  {

  Point3d result (0,0,0);
  int bezSize = bMap.size();

  double minDis = HUGE_VAL;
  for(int j=1; j<bezSize; j++){
    double currentDistance = distLinePoint(bMap[j-1], bMap[j], targetPoint, true);
    if(currentDistance < minDis){
      minDis = currentDistance;
      // interpolate the discretized bezier for more accurate arclength
      double a = norm(bMap[j-1] - targetPoint);
      double b = currentDistance;
      bezWeight = 0;
      double aabb = std::max(0.,a*a-b*b);
      bezIdx = j;
      bezWeight = (double)(std::sqrt(aabb))/(double)(norm(bMap[j]-bMap[j-1]));
      if(bezWeight > 1) bezWeight = 1;
    }
  }
  result = (1-bezWeight) * bMap[bezIdx-1] + bezWeight * bMap[bezIdx];

  return result;
  }


  Point3d calcNearestPointOnBezierLine(const Point3d& targetPoint, std::map<int, Point3d>& bMap)
  {
    int bezIdx = 0;
    double bezWeight = 0.;
    return calcNearestPointOnBezierLine(targetPoint, bMap, bezIdx, bezWeight);
  }



}
