#include "FMPointCloud.h"
#include "FM1PC.h"
#include "FMPCField.h"
#include <limits>

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;
  }
}

// get distance on cylinder with plane from edge x = 1
double labysmoothPlane2(double x, double y, double z){
  const double R = 1.;
  double theta = acos(x/R);

  return R*theta;
}

// get distance on disk with central point source
double circleReal(double x, double y, double z){
  const npoint3 center(0,0,0);
  npoint3 pt(x,y,z);

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

// get distance on sphere from pole
double sphereReal(double x, double y, double z){
  const double R = 1;

  double theta = acos(z/R);

  return R*theta;
}

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

  std::string PCfile;
  double lc, distRef;
  // int nbNeib = 10; // carre
  int nbNeib = 13;
  // int nbNeib = 13;
  // int nbNeib = 9;
  if(!getParameters(argc,argv,&PCfile,&lc,&distRef)){
    std::cerr << "Unable to read given parameters !" << std::endl;
    return 1;
  }
  
  // std::vector<npoint3> seeds{npoint3(-.5,-.5,0.),npoint3(.5,.5,0.)}; // "carre" case
  // std::vector<npoint3> seeds{npoint3(1.,0.,0.),npoint3(-1,0.,5.)}; // "labysmooth" case
  // std::vector<npoint3> seeds{npoint3(0.,0.,0.)}; // disk
  // std::vector<npoint3> seeds{npoint3(0.,0.,1.), npoint3(sqrt(2)/2,sqrt(2)/2,0.)}; // sphere
  
  double scale = 1.;
  std::vector<npoint3> seeds{npoint3(scale,0.,0.), npoint3(-scale,0.,3.141592653589793*scale)}; // scale case A
  // std::vector<npoint3> seeds{npoint3(scale,0.,0.), npoint3(0,scale,3.141592653589793*scale/2)}; // scale case B
  
  // Point Cloud
  FM::FMPointCloud PC;
  PC.readFromMSH(PCfile);
  PC.buildTree();
  PC.buildGraph(nbNeib);
  PC.applyNormCurvCylinder(npoint3(0,0,1),npoint3(0,0,0),1.); // "labysmooth" case
  // PC.applyNormCurvPlane(npoint3(0.,0.,1),npoint3(1.,0.,0.)); // "carre" case
  // PC.applyNormCurvSphere(npoint3(0.,0.,0.),npoint3(0.,0.,1.),1.); // sphere
  
  // Distance field
  FM::FMPCField dataField(&PC);
  
  // Algo
  FM::FM1PC algo(&PC,&dataField,!true);
  algo.initiateFM(seeds[0]);
  // algo.initiateFM(seeds[0],npoint3(0,1,0));
  algo.propagateFM();
  
  // Results
  // double dist1 = dataField.distanceAtPoint1(seeds[1]);
  double dist1 = algo.interpolate(seeds[1]);

  int idxEnd;
  PC.findClosest(seeds[1],idxEnd);
  // double dist1 = dataField.getDistance(idxEnd);
  double error = (dist1-distRef)/distRef;

  // std::vector<int> ptsEdge;
  // straight line
  // npoint3 ptEdge1(-1,0,-0.1), ptEdge2(-1,0,5.1);
  // PC.findNodesEdge(ptEdge1,ptEdge2,lc/3,ptsEdge);
  // double rmsEdge = dataField.rmsErrorAbs(labysmoothPlane2,ptsEdge);

  // PC.findNodesGeo(FM::make_circle(seeds[0],npoint3(0,0,1),1.,lc/3),ptsEdge); // circle
  // PC.findNodesGeo(FM::make_circle(seeds[1],npoint3(0,0,1),1.,lc/3),ptsEdge); // sphere
  
  // double rmsEdge = dataField.rmsErrorAbs(circleReal,ptsEdge); // circle
  // double rmsEdge = dataField.rmsErrorAbs(sphereReal,ptsEdge); // sphere

  // double maxEdge = dataField.maxErrorAbs(circleReal,ptsEdge); // circle
  // double maxEdge = dataField.maxErrorAbs(sphereReal,ptsEdge); // sphere
  
  std::cout << PC.getNbrNodes();
  std::cout << setprecision(numeric_limits<double>::max_digits10);
  std::cout << " " << distRef/lc << " " << error << std::endl;
  // std::cout << " " << distRef/lc << " " << rmsEdge << std::endl;
  // std::cout << " " << distRef/lc << " " << rmsEdge << " " << maxEdge << std::endl;
  
  return 0;
}