#include <iostream>
#include <fstream>
#include <limits>
#include <iomanip>

#include "FastMarching.h"
#include "gmshNode.h"
#include "gmshElement.h"
#include "GmshGlobal.h"
#include "FMField.h"
#include "FindMiniPath.h"


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

int main(int argc, char* argv[]){
  std::string fileMesh;
  double lc, valReal;
  
  if(!getParameters(argc,argv,&fileMesh,&lc,&valReal)){
      std::cout<<"\nWrong arguments."<<std::endl;
      return 1;
  }
    
  // std::vector<SPoint3> seeds{SPoint3(-.5,-.5,0.),SPoint3(.5,.5,0.)}; // "carre" case
  std::vector<SPoint3> seeds{SPoint3(1.,0.,0.),SPoint3(-1,0.,5.)}; // "labysmooth" case
  
  GmshInitialize();
  FM::FMModel *model = new FM::FMModel();
  model -> readMSH(fileMesh);
  model -> buildAdj(1<<(model->getDim()-1));

  FMSpeedConstant speed;
  FastMarch_c fastM(speed);
  
  map<FM::gmshNode,FastMarch_c::FieldPair> initvals;
  set<FM::gmshElement> bndinit;

  // field seed 0
  InitPt(model,seeds[0],initvals,bndinit);
  fastM.Init(initvals,bndinit);
  fastM.Propagate();
  FMField field;
  field.Import(fastM);
  
  // field seed 1
  map<FM::gmshNode,FastMarch_c::FieldPair> initvals2;
  set<FM::gmshElement> bndinit2;

  InitPt(model,seeds[1],initvals2,bndinit2);
  fastM.Init(initvals2,bndinit2);
  fastM.Propagate();
  FMField field2;
  field2.Import(fastM);
  
  // results
  FindMiniPath mPath(field,field2,model);

  double val1, val2, valMid1, valMid2;
  SPoint3 midPoint;

  double dist = mPath.getLength(seeds[0],seeds[1],&val1,&val2,false,&valMid1,&valMid2,midPoint);
  double error = (dist -valReal)/valReal;
  
  std::cout << FM::FMModel::Ptr->v_to_ele.size() << " ";
  std::cout << setprecision(numeric_limits<double>::max_digits10);
  std::cout << " " << valReal/lc << " "<< error << std::endl;
  
  return 0;
}