#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;
    std::vector<SPoint3> seeds;
    // seeds.push_back(SPoint3(-0.5,-0.5,0));
    // seeds.push_back(SPoint3(.5,.5,0));
    seeds.push_back(SPoint3(1.,0.,0.));
    seeds.push_back(SPoint3(-1.,0.,5.));

    bool LS2 = true;

    if(!getParameters(argc,argv,&fileMesh,&lc,&valReal)){
        std::cout<<"\nWrong arguments."<<std::endl;
        return 1;
    }

    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;

    InitPt(model,seeds[0],initvals,bndinit);
    fastM.Init(initvals,bndinit);
    fastM.Propagate();

    FMField field;
    field.Import(fastM);

    double dist;

    if(LS2){
      map<FM::gmshNode,FastMarch_c::FieldPair> initvals;
      set<FM::gmshElement> bndinit;

      InitPt(model,seeds[1],initvals,bndinit);
      fastM.Init(initvals,bndinit);
      fastM.Propagate();
      FMField field2;
      field2.Import(fastM);

      FindMiniPath mPath(field,field2,model);

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

      dist = mPath.getLength(seeds[0],seeds[1],&val1,&val2,false,&valMid1,&valMid2,midPoint);
    } else {
      dist = field(seeds[1]).first;
    }
    //double valReal = sqrt(seeds[1].x()*seeds[1].x() + seeds[1].y()*seeds[1].y() + seeds[1].z()*seeds[1].z());
    // double valReal = seeds[0].distance(seeds[1]);

    double endVal = (dist -valReal)/valReal;

    std::cout << FM::FMModel::Ptr->v_to_ele.size() << " ";
    std::cout << setprecision(numeric_limits<double>::max_digits10);
    std::cout << " " << lc << " " << valReal/lc << " "<< endVal << std::endl;
}