#include "FMPointCloud.h"
#include "FM0PC.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 = 12; //carre
  // int nbNeib = 9;
  int nbNeib = 10;
  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)}; // circle
  // std::vector<npoint3> seeds{npoint3(0.,0.,1.),npoint3(0.,0.,0.)}; // sphere

  // 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 FM0
  FM::FM0PC algo(&PC,&dataField,!true);
  algo.initiateFM(seeds[0]);
  algo.propagateFM();
  
  // Results
  // double dist1 = dataField.distanceAtPoint1(seeds[1]);
  double dist1 = algo.interpolate(seeds[1]);
  double error = (dist1-distRef)/distRef;

  // RMS
  // std::vector<int> 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 << " " << maxEdge << std::endl;
  
  return 0;
}