#include "FMPointCloud.h"
// #include "FM0PC.h"
#include "FM1PC.h"
// #include "FM2PC.h"
#include "FMPCField.h"
#include "ndisplay.h"


bool getParameters(int argc, char* argv[], std::string* ptsFile, double* lc_, double* distRef_){
  if(argc > 3){
    *ptsFile = argv[1];
    *lc_ = atof(argv[2]);
    *distRef_ = atof(argv[3]);
    //*nbNeib_ = atoi(argv[3]);
    return true;
  } else {
    *ptsFile = "";
    *lc_ = 0;
    *distRef_ = 0;
    //*nbNeib_ = 0;
    return false;
  }
}

double carreReal(double x, double y, double z){
  npoint3 pt(x,y,z);
  npoint3 pt0(-.5,-.5,0.);
  // npoint3 pt0(-.5,0.,-.5);
  return (pt-pt0).norm();
}

double carreRealPlane(double x, double y, double z){
  npoint3 pt(x,y,z), pt0(-.5,0,0.), Nplane(1,0,0);
  // npoint3 pt(x,y,z), pt0(0,0,0.); // disk plane centered
  // dist 
  // return (pt-pt0)*Nplane;
  return abs(pt[0]-pt0[0]);
}

double carreRealPlane2(double x, double y, double z){
  npoint3 pt(x,y,z), pt0(-1,0,0.), Nplane(1,0,0);
  // npoint3 pt(x,y,z), pt0(0,0,0.); // disk plane centered
  // dist 
  // return (pt-pt0)*Nplane;
  return abs(pt[0]-pt0[0]);
}

// point source
double labysmoothReal(double x, double y, double z){
  const double R = 1;
  const npoint3 pt1(1.,0.,0.);
  // const npoint3 pt1(-1.,0.,5.);
  npoint3 ptP(x,y,0.);

  double dDist = (ptP-pt1).norm();
  double theta = (dDist*dDist/(2*R*R) < 1e-3) ? 2.*asin(dDist/2/R): acos(1-dDist*dDist/(2*R*R));

  // double theta = atan2(y,x) - atan2(pt1[1],pt1[0]);

  npoint3 pPlan(R*theta,z,0.);

  return pPlan.norm();
}

// plane along z
double labysmoothPlane(double x, double y, double z){
  //return z; // labysmooth
  return y+1; // bend square
}

// plane from center
double labysmoothPlane1(double x, double y, double z){
  // const npoint3 pt0(0,1,0);
  const double R = 1.;

  // double theta = 3.14159265358979323846/2.;
  // if(y>0) theta = atan(abs(x)/y);

  double theta = asin(abs(x)/R);

  return R*theta;
}

// plane from border
double labysmoothPlane2(double x, double y, double z){
  const double R = 1.;
  const npoint3 center(0,0,0), axis(0,0,1), start(R,0,0);

  npoint3 pt(x,y,z);
  npoint3 ptRef(pt-center);
  npoint3 vecP(ptRef - (ptRef*axis)*axis);

  vecP.normalize();
  npoint3 vecS = (start-center);
  vecS.normalize();

  double theta = acos(vecP*vecS);

  // double theta = acos(x/R);

  return R*theta;
}

// source point pole
double sphereReal(double x, double y, double z){
  const double R = 1;

  double theta = acos(z/R);

  return R*theta;
}

// source plane equator
double sphereReal2(double x, double y, double z){
  const double R = 1;

  double theta = asin(z/R);

  return R*theta;
}

// source plane XY pole
double sphereReal3(double x, double y, double z){
  const double R = 1;

  double theta = asin(abs(x)/R);

  return R*theta;
}

double circleReal(double x, double y, double z){
  const npoint3 center(0,0,0);
  npoint3 pt(x,y,z);

  return (pt-center).norm();
}

double LcornerReal(double x, double y, double z){
  const npoint3 corner(0,0,0);
  const npoint3 p0(-.5,-.5,0);
  // const npoint3 p0(.5,0,0);
  // const npoint3 p0(-1,1,0);
  npoint3 p1(x,y,z);

  npoint3 vec(corner-p0), vecY;
  double offsetCorner = vec.normalize();
  bool noShock = false;

  if((p0.x()<corner.x() && p0.y()<corner.y()) || (p0.x()==corner.x() && p0.y()<corner.y()))
    vecY = npoint3(vec.y(),-1*vec.x(),0);
  else if((p0.x()>corner.x() && p0.y()>corner.y()) || (p0.x()>corner.x() && p0.y()==corner.y()))
    vecY = npoint3(-1*vec.y(),vec.x(),0);
  else
    noShock = true;

  double dist = 0.;
  if(!noShock){
    double xp = (p1-corner)*vec;
    double yp = (p1-corner)*vecY;

    dist = (xp >= 0 && yp > 0) ? offsetCorner + (p1-corner).norm() : (p1-p0).norm();
  } else {
    dist = (p1-p0).norm();
  }
  
  return dist;
}

double bendDisk1(double x, double y, double z){
  double const R = 1;
  npoint3 const center(0,0,R);
  npoint3 const axis(0,1,0);
  npoint3 const vecRef(0,0,-R);

  npoint3 pt(x,y,z);

  double newY = (pt-center)*axis;

  npoint3 ptP = (pt-center) - newY*axis;
  double theta = acos(ptP*vecRef /R /R);

  double dist = sqrt((theta*R)*(theta*R) + newY*newY);

  return dist;
}

bool roundCorner(double x, double y, double z){
  if(y < -1 + EPS_NUM || x < -1 +EPS_NUM) return true;
  else if(x < 0 && y < 0 && sqrt(x*x + y*y) > 1 - EPS_NUM) return true;
  else return false;
}

npoint3 roundCornerGrad(double x, double y, double z){
  if(y < -1 + EPS_NUM){ return npoint3(0,1,0); }
  else if (x < -1 +EPS_NUM){ return npoint3(1,0,0);}
  else if(x < 0 && y < 0 && sqrt(x*x + y*y) > 1 - EPS_NUM){
    npoint3 gradI = npoint3(-x,-y,0);
    gradI.normalize();
    return gradI;
  }
  else { return npoint3(); }
}

double roundCornerReal(double x, double y, double z){
  if(x <= 0 && y <= 0){
    return 1 - sqrt(x*x + y*y);
  } else if ( y < x ) {
    return y + 1;
  } else {
    return x + 1;
  }
}

bool round2Corner(double x, double y, double z){
  // source 1
  bool s1 = (x < -3 + EPS_NUM && y < -1 + EPS_NUM);

  // source 2
  double distC2X = x + 3;
  double distC2Y = y + 2;
  bool s2 = (x > -3 - EPS_NUM && y > -2 - EPS_NUM) && (sqrt(distC2X*distC2X + distC2Y*distC2Y) < 1 + EPS_NUM);

  // source 3
  bool s3 = (x > -2 - EPS_NUM && y > -3 - EPS_NUM) && abs(x+2) < EPS_NUM && y < -1; 

  // source 4
  double distC4X = x + 1;
  double distC4Y = y + 3;
  bool s4 = (x < -1 + EPS_NUM && y < -3 + EPS_NUM) && (sqrt(distC4X*distC4X + distC4Y*distC4Y) > 1 - EPS_NUM);

  // source 5
  bool s5 = (x > -1 - EPS_NUM && y < -4 + EPS_NUM);

  return s1 || s2 || s3 || s4 || s5;
}

npoint3 round2CornerGrad(double x, double y, double z){
  double distC2X = x + 3;
  double distC2Y = y + 2;
  double distC4X = x + 1;
  double distC4Y = y + 3;

  if ((x < -3 + EPS_NUM && y < -1 + EPS_NUM) || (x > -1 - EPS_NUM && y < -4 + EPS_NUM)){
    return npoint3(0,1,0);
  } else if ((x > -2 - EPS_NUM && y > -3 - EPS_NUM) && abs(x+2) < EPS_NUM && y < -1){
    return npoint3(1,0,0);
  } else if ((x > -3 - EPS_NUM && y > -2 - EPS_NUM) && (sqrt(distC2X*distC2X + distC2Y*distC2Y) < 1 + EPS_NUM)){
    npoint3 gradI(distC2X,distC2Y,0);
    gradI.normalize();
    return gradI;
  } else if ((x < -1 + EPS_NUM && y < -3 + EPS_NUM) && (sqrt(distC4X*distC4X + distC4Y*distC4Y) > 1 - EPS_NUM)){
    npoint3 gradI(-distC4X,-distC4Y,0);
    gradI.normalize();
    return gradI;
  } else {
    return npoint3();
  }

}

double round2CornerReal(double x, double y, double z){
  if(x < -3){
    return y + 1;
  } else if (y >= -2 && x >= -3) {
    double dist2X = x + 3;
    double dist2Y = y + 2;
    return sqrt(dist2X*dist2X + dist2Y*dist2Y) - 1;
  } else if (y <= -3 && x <= -1) {
    double dist4X = x + 1;
    double dist4Y = y + 3;
    return 1 - sqrt(dist4X*dist4X + dist4Y*dist4Y);
  } else {
    if(x > -1 && y+2 <= x){
      return y + 4;
    } else {
      return x + 2;
    }
  }
}


int main(int argc, char* argv[]) {

  std::string PCfile;
  std::string outputFiles = "testX";
  double lc, distRef;
  // int nbNeib = 10; // cyl point
  // int nbNeib = 9; // bug avec plan
  // int nbNeib = 9;
  int nbNeib = 13; // 12
  // int nbNeib = 10; // cyl plane
  std::cout << "getting parameters" << std::endl;
  if(!getParameters(argc,argv,&PCfile,&lc,&distRef)){
    std::cerr << "Unable to read given parameters !" << std::endl;
    return 1;
  }

  // std::vector<npoint3> seeds;
  // seeds.push_back(npoint3(-.5,-.5,0.)); //square XY && 3D corner start
  // seeds.push_back(npoint3(.5,.5,0.));  // square XY end
  // seeds.push_back(npoint3(-.5,0.,-.5));  // square XY end
  // seeds.push_back(npoint3(.5,0.,.5));  // square XZ end
  // seeds.push_back(npoint3(.5,0.,-.5)); // 3D corner end
  // seeds.push_back(npoint3(0.5,0.25*sqrt(2),0.25*sqrt(2))); // 3D corner not right end

  // seeds.push_back(npoint3(1,0,0)); // start labysmooth
  // seeds.push_back(npoint3(-1,0,5)); // end labysmooth
  // seeds.push_back(npoint3(-1,0,1.5707963267)); // end labysmooth plane

  // seeds.push_back(npoint3(0,0,1)); // sphere point start
  // seeds.push_back(npoint3(1,0,0)); // sphere end
  // seeds.push_back(npoint3(sqrt(2)/2,sqrt(2)/2,0)); // sphere end

  // seeds.push_back(npoint3(1,0,0)); // sphere plane start
  // seeds.push_back(npoint3(0,0,1)); // sphere plane end

  // std::vector<npoint3> seeds{npoint3(0,0,0),npoint3(0,-1,0)}; // circle

  // seeds.push_back(npoint3(0,0,0)); // bended carre test

  // std::vector<npoint3> seeds{npoint3(1,0.,0.),npoint3(-1,0.,3.141592653589793)}; // cylinder scale 1
  std::vector<npoint3> seeds{npoint3(-.5,-.5,0.),npoint3(.5,.5,0.)}; // square
  // std::vector<npoint3> seeds{npoint3(0,0,1),npoint3(sqrt(2)/2,sqrt(2)/2,0)}; // sphere start pole

  // std::vector<npoint3> seeds{npoint3(-0.0795103,0.126002,0.0528073)}; // standford bunny CSV :  ~/Documents/standfordBunny/pc/bun_zipper.csv
  // std::vector<npoint3> seeds{npoint3(92.15879,1083.824,222.844)}; // aile : ../../../../../MeshMachine/FastMarching/devel/testVTP/aile1.0000.msh
  // std::vector<npoint3> seeds{npoint3(-.0821974,.128571,.0521036)}; // standford bunny MSH :  ~/Documents/standfordBunny/bun_zipper.msh

  FM::FMPointCloud test;
  data_container data;
  ndisplay display;


  // 0. load PC
  std::cout << "reading MSH/CSV file..." << std::endl;
  test.readFromMSH(PCfile);
  // test.readFromCSV(PCfile);
  // test.readFromCSV("cylinder1_0.025.csv");
  std::cout << test.getNbrNodes();

  // 1. Build graph

  std::cout << "Building tree" << std::endl;
  
  test.buildTree();
  // test.importGraphCSR("cylinder1_0.025.csr");
 
  std::cout << "Build graph" << std::endl;

  test.buildGraph(nbNeib);

  // npoint3 norm1(0,0,1), norm2(0,-1,1);
  // norm2.normalize();
  // npoint3 norm1(0,0,1), norm2(0,1,0);
  // test.quickCornerTopolCorrect(norm1,norm2);

  // std::cout << "Computing normals and curvatures" << std::endl;
  // test.applyNormCurvPlane(npoint3(0.,0.,1),npoint3(1.,0.,0.));
  // test.applyNormCurvCylinder(npoint3(0,0,1),npoint3(0,0,0),1.);
  // test.applyNormCurvSphere(npoint3(0,0,0),npoint3(0,0,1),1.);
  // test.applyNormCurvCylinder(npoint3(0,1,0),npoint3(0,0,10),10); // bend disk
  // test.applyNormCurvCylinder(npoint3(0,1,0),npoint3(0,0,1),1); // bend carre

  // test.computeNormCurv();
  // test.applyNormCurvCylinder(npoint3(0,0,1),npoint3(0,0,0),1.,10.);

  // 2. FM

  bool corrCurvOn = !true;
  
  FM::FMPCField dataField(&test);

  #ifdef FM0PC_H
    FM::FM0PC testAlgo(&test,&dataField,corrCurvOn);
  #elif defined(FM1PC_H)
    FM::FM1PC testAlgo(&test,&dataField,corrCurvOn);
  #elif defined(FM2PC_H)
    FM::FM2PC testAlgo(&test,&dataField,corrCurvOn);
  #endif

  // testAlgo.printLog();
 

  testAlgo.initiateFM(seeds[0]);
  // testAlgo.initiateFMNode(seeds[0]);
  // testAlgo.initiateFM(seeds[0],npoint3(1,0,0));
  // testAlgo.initiateFM(seeds[0],npoint3(0,1,0));
  // testAlgo.initiateFM(seeds[0],npoint3(0,0,1));
  // testAlgo.initiateFMplane1(npoint3(0,1,0),npoint3(0,-1,0));
  // testAlgo.initiateFM(seeds[0],npoint3(0,0,1));
  // testAlgo.initiateFM(npoint3(0,1,0),npoint3(1,0,0));
  // testAlgo.initiateFM(npoint3(-1,0,0),npoint3(1,0,0));

  // testAlgo.initiateFMplane2(seeds[0],npoint3(1,0,0));
  // testAlgo.initiateCurve(FM::make_line(npoint3(-1,1,0),npoint3(-1,-1,0),lc/3),npoint3(1,0,0));

  // testAlgo.initiateLine(FM::make_line(npoint3(0,1,0),npoint3(0,-1,0),lc/3),npoint3(1,0,0));
  // testAlgo.initiateFMplane2(npoint3(0,-1,0),npoint3(0,1,0));

  // testAlgo.initiateFMplane2(seeds[0],npoint3(1,0,0));

  // testAlgo.initiateCurve(FM::make_circle(npoint3(0,0,0),npoint3(1,0,0),1.,lc/3),npoint3(1,0,0));

  // testAlgo.initiateCurve(roundCorner,roundCornerGrad);

  // testAlgo.initiateCurve(round2Corner,round2CornerGrad);

  testAlgo.propagateFM();

 
  std::cout << "Displaying" << std::endl;
  // dataField.display(data);
  // test.displayCurvatures(data,true,1);
  // test.displayNodes(data);
  // test.displayTree(data, 100);
  // testAlgo.displayNormals(data);
  // test.displayTree(data);
  // testAlgo.displayComputedNormals(data,false);
  // testAlgo.displayApproximatedNormals(data,false);
  // testAlgo.displayApproximatedNormals(data);
  // testAlgo.displayComputedNormals(data);

  testAlgo.drawPathes(data);
  // testAlgo.displayFrontCurvature(data);

  int endVx;
  test.findClosest(seeds[1], endVx);
  // test.findClosest(npoint3(0.84220526956312614,0.5391570123072702,0.02975775654140329), endVx);

  // testAlgo.drawGeodesic(endVx,data);
  // testAlgo.drawNodeMethod(data);
  // testAlgo.drawElementsUsed2(endVx,data);

  int ptI;
  // test.findClosest(npoint3(.11045,-.889407,0),ptI);
  // test.displayNodeRelations(data,ptI);

  display.init_data(data);
  display.display();

  dataField.exportMSH(outputFiles + "_data.msh",true);
  // dataField.exportCSV();

  // test.exportGraph("square");

  dataField.exportErrorAbsMSH(carreReal);
  // dataField.exportErrorAbsMSH(carreRealPlane);
  // dataField.exportErrorAbsMSH(carreRealPlane2);
  // dataField.exportErrorAbsMSH(labysmoothReal);
  // dataField.exportErrorRelMSH(labysmoothReal);
  // dataField.exportErrorAbsMSH(labysmoothPlane1);
  // dataField.exportErrorAbsMSH(labysmoothPlane);
  // dataField.exportErrorRelMSH(labysmoothPlane1);
  // dataField.exportErrorAbsMSH(labysmoothPlane2);

  // dataField.exportErrorAbsMSH(sphereReal);
  // dataField.exportErrorRelMSH(sphereReal);
  // dataField.exportErrorAbsMSH(sphereReal2);
  // dataField.exportErrorAbsMSH(circleReal);

  // dataField.exportErrorAbsCSV(sphereReal2);
  // dataField.exportErrorAbsCSV(circleReal);

  // dataField.exportErrorAbsMSH(bendDisk1);

  // dataField.exportErrorAbsMSH(sphereReal3);

  // dataField.exportErrorAbsMSH(roundCornerReal);
  // dataField.exportErrorAbsMSH(round2CornerReal);

  // testAlgo.printNormals();

  /*
    RMS on edge
  */
  // std::vector<int> ptsEdge;
  // npoint3 ptEdge1(-1,0,-0.1), ptEdge2(-1,0,5.1);
  // test.findNodesEdge(ptEdge1,ptEdge2,lc/3,ptsEdge);
  // double rmsEdge = dataField.rmsErrorAbs(labysmoothPlane2,ptsEdge);

  // test.findNodesGeo(FM::make_circle(seeds[1],npoint3(0,0,1),1,lc/3),ptsEdge);

  // double rmsEdge = dataField.rmsErrorAbs(circleReal,ptsEdge);
  // double maxEdge = dataField.maxErrorAbs(circleReal,ptsEdge);
  // double rmsEdge = dataField.rmsErrorAbs(sphereReal,ptsEdge);
  // double maxEdge = dataField.maxErrorAbs(sphereReal,ptsEdge);

  /*
   section below for test, check at las seed point the distance
  */

  std::cout << setprecision(numeric_limits<double>::max_digits10);

  double dist1 = testAlgo.interpolate(seeds[1]);
  std::cout << "interpolate = " << dist1 << std::endl;

  // double dist2 = dataField.distanceAtPoint1(seeds[1]);
  // std::cout << "interpolate = " << dist2 << std::endl;
  // double valEnd = (dist1-distRef)/distRef;

  // std::cout << " " << lc;
  // 
  // // std::cout << " " << distRef/lc << " " << valEnd*100 << " % (" << dist1 << " / " << distRef << ")" << std::endl;
  // std::cout << " " << distRef/lc << " " << valEnd << " (" << dist1 << " / " << distRef << ")" << std::endl;

  // std::cout << "RMS EDGE :\nNodes ids :" << std::endl;
  // for(int i : ptsEdge) std::cout << " " << i;
  // std::cout << std::endl << " RMS = " << rmsEdge << std::endl;

  // std::cout << "MAX EDGE :" << std::endl;
  // std::cout << " MAX = " << maxEdge << std::endl;

  cout << "end code" << endl;

  return 0;
}