#include "FindMiniPath.h"



double FindMiniPath::getLength(SPoint3 dep,SPoint3 arr,double *val1_,double *val2_, bool singular,double *valmid1_,double *valmid2_,SPoint3 &pos)
{
  ExportMSH ef ( Model );
  std::list<std::pair<SPoint3,int>> lpt;//List of point where the FMM is generated from. List of pair <point, field>. Field is the distance field from point.
  map<FM::gmshNode,FastMarch_c::FieldPair> initvals;
  set<FM::gmshElement> bndinit;
   
  lpt.push_back(std::pair<SPoint3,int>(dep,0));// create and add to list point lpt the pair <pt departure, field 0>
  lpt.push_back(std::pair<SPoint3,int>(arr,1));// " <pt arrival, field 1>
  for (std::list<std::pair<SPoint3,int> >::iterator it=lpt.begin();it!=lpt.end();++it) // loop on two points
  {
    InitPt(Model,it->first,initvals,bndinit ); //Init narrow band
    fastM.Init ( initvals,bndinit ); // with narrow band info init fast marching
    fastM.Propagate(); // march
    addField(FMField()); // add empty field
    fields[it->second].Import(fastM); // fields update with band propagated - at the index from lpt (eg:fields[0] for dep point) 
    fastM.Clear(); // clear fast marching for next point
    initvals.clear();
    bndinit.clear();
//    fields[it->second].SetExportFunctor(ef);
//    fields[it->second].Export("lsv0");
  }
  // add field (2) which is fields[0] - fields[1]
  addField(FMField());
  fields[fields.size()-1].Subtract(fields[0],fields[1]);
  // add field (3) which is fields[0] + fields[1]
  addField(FMField());
  fields[fields.size()-1].Add(fields[0],fields[1]);
  // export fields[0] in gmsh "ls0" files
  // fields[0] is the field distance from dep point
  fields[0].SetExportFunctor(ef);
  fields[0].Export("ls0");
  // export fields[1] in gmsh "ls1" files
  // fields[1] is the field distance from arr point
  fields[1].SetExportFunctor(ef);
  fields[1].Export("ls1");
  // export fields[2] in gmsh "lsdiff" files
  // fields[2] = fields[0] - fields[1]
  fields[2].SetExportFunctor(ef);
  fields[2].Export("lsdiff");
  // export fields[3] in gmsh "lsadd" files
  // fields[3] = fields[0] + fields[1]
  fields[3].SetExportFunctor(ef);
  fields[3].Export("lsadd");
  // add field which is the curvature from dep point and export it in gmsh "lsCRV0" files
  addField(FMField());
  fields[4].RCurvature(fields[0]);
  fields[4].SetExportFunctor(ef);
  fields[4].Export("lsCRV0");
  // add field which is the curvature from arr point and export it in gmsh "lsCRV1" files
  addField(FMField());
  fields[5].RCurvature(fields[1]); 
  fields[5].SetExportFunctor(ef);
  fields[5].Export("lsCRV1");
  
  // loop if points inserted due to singularity (bool inserted_sing)
  bool inserted_sing=false;
  double val;
  bool firstfirst=true;
  do
  {
    inserted_sing=false;
    val=0;//Length
    bool first=true;
    for (std::list<std::pair<SPoint3,int> >::iterator it=lpt.begin();it!=lpt.end();it++)
    {
      std::list<std::pair<SPoint3,int> >::iterator it2=it;it2++; // pointer to next value
      if (it2!=lpt.end())
      {
        double valdep,valarr;
        valdep=fields[it2->second](it->first).first; // distance of it in field of it2 (in field of it2, look for point of it, get FieldPair linked to it and get 1st element)
        valarr=fields[it->second](it2->first).first; // distance of it2 in field of it
        if (firstfirst) { if (val1_) *val1_=valdep; firstfirst=false;  }
        if (val2_) *val2_=valarr;
        double val1,val2;
        // need to act if there are singular points in between.
        SPoint3 pt;
        if (getMiniPoint(valdep,valarr,val1,val2,pt,it->second,it2->second,singular) )
        {
          // Goes here if singularity
          // SPoint3 pt (determined by getMiniPoint) inserted in lpt, restart loop
 //         std::cout << "singular point !" << std::endl;
          std::cout << " ("<< pt[0] << "," << pt[1] << "," << pt[2] << ") ";
          lpt.insert(it2,std::pair<SPoint3,int>(pt,it2->second));
          lpt.insert(it2,std::pair<SPoint3,int>(pt,it->second));
          inserted_sing=true;
          break;
        }
  //  std::cout << val1 << " " << val2 <<  " d1= " << (valdep.first )  << "  d2= " << (valarr.first ) << (valdep.first+valarr.first - val1-val2 ) << std::endl;
        val+=(valdep+valarr - val1-val2 );
        *valmid1_=val1;
        *valmid2_=val2;
        pos=pt;
        it=it2;
      }
    }
  }
  while (inserted_sing);
  Clear();
  return val;
}

int FindMiniPath::getSingularPoint(double valdep,double valarr,double& val1,double& val2,SPoint3 &pos,int f1, int f2,double crv)
{
  // TODO : check for singular points in between, return code and position of singular point.
  // 1- find area of high curvature
  // 2- for each of them, compute both distances d1p,d2p
  // 3- check if it is minimal, i.e. check that the iso-0 of (d1-d2) -(d1p-d2p) is a minimal value of d1+d2.
  // 4- return the point with the minimal d1+d2 if it does not already exist
  double crvmax=0.0;//Highest curvature computed
  FM::gmshNode pmax;//Node with highest curvature
  double dist1max,dist2max;//Maximum distance in field (1 of dep,2 of arriv) of vertex vv
  bool first=true;
  bool found=false;
  for (std::map<MVertex *,std::vector<MElement*> >::iterator it=FM::FMModel::Ptr->v_to_ele.begin(); it!=FM::FMModel::Ptr->v_to_ele.end(); ++it)
  {
    MVertex *v=it->first;
    FM::gmshNode vv(v);
    double crv=fields[4](vv).first/it->second.size();//relative curvature at vertex vv
    double dist1=fields[0](vv).first;//distance of vertex vv from dep point
    double dist2=fields[1](vv).first;//distance of vertex vv form arr point
    if (dist1>0.05*valarr && dist1<0.95*valarr && dist2>0.05*valdep && dist2<0.95*valdep)
    {
      if (first) // 1st vertex
      {
        crvmax=crv;
        pmax=vv;
        found=true;
        first=false;
        dist1max=dist1;
        dist2max=dist2;
      }
      else
      {
        if (crvmax<crv) // if biggest curvature so far
        {
          crvmax=crv;
          pmax=vv;
          dist1max=dist1;
          dist2max=dist2;
        }
      }
    }
  }
  if (found)
  {
    // check value of dist1-dist2, find minima of dist1+dist2 on dist1-dist2=constant ...
    double dval = dist1max-dist2max;
    double minv;//Smallest value of dist1+dist2
    SPoint3 minp;//Point at minv
    bool first=true;
//    std::cout << crvmax << " " << dval << " "  << dist1max+dist2max << " " << dist1max << " " << dist2max << " " << pmax.GetVertex()->x() << " " << pmax.GetVertex()->y() << " "<< pmax.GetVertex()->z() << std::endl;
    for ( int i=0;i<FM::FMModel::Ptr->Els.size();++i) //loop on element
    {
      FM::gmshElement e(FM::FMModel::Ptr->Els[i]); //ith element
      std::vector<FM::gmshNode> vec;//Vector of nodes of e (the ith element)
      e.GetNodes(vec);
      std::vector<std::pair<double,double> > vecf(vec.size());//Vector of (values in field f1, values in field f2) for each nodes.
      std::vector<double > vecfd(vec.size());//Vector of the differences between dval and the differences in the pairs of vecf
      bool pos=false;
      bool neg=false;
      for (int j=0;j!=vec.size();++j) // loop on node of element
      {
        vecf[j].first=fields[f1](vec[j]).first;
        vecf[j].second=fields[f2](vec[j]).first;
        vecfd[j]=(vecf[j].first-vecf[j].second)-dval;
        if (vecfd[j]>0.) pos=true;
        else neg=true;
        if (vecfd[j]==0.) {pos=true;neg=true;}
      }
      
      if (pos&&neg) //if there is a node with value field 1 - value field 2 = dist1max - dist2max 
      {
//        std::cout << "element num " << e.GetBase()->getNum() <<std::endl ;
//        for (int j=0;j!=vec.size();++j)
//        {
//         std::cout << vecf[j].first << " " << vecf[j].second << " " << vecfd[j] << " " << vecf[j].first+vecf[j].second << std::endl;
//        }
//    std::cout <<std::endl ;
        for (int j=0;j!=vec.size();++j) // loop on nodes
        {
          int k1=j;
          int k2=(j+1)%vec.size(); // = j+1, except when j+1=nbr nodes: k2=0
//          std::cout <<(vecfd[k1]*vecfd[k2]) << std::endl;
          if ((vecfd[k1]*vecfd[k2])<=0.) //if vecfd changes sign between node k1 and k2
          {

            double u=-(vecfd[k1])/((vecfd[k2]-vecfd[k1]));
            SPoint3 pt1;//Node k1
            SPoint3 pt2;//Node k2
            vec[k1].GetPos(pt1[0],pt1[1],pt1[2]);
            vec[k2].GetPos(pt2[0],pt2[1],pt2[2]);
            SPoint3 ptm=pt1*(1-u) +pt2*(u);//Point interpolated from nodes k1 and k2
            double v1=vecf[k1].first*(1-u)+vecf[k2].first*(u);//Interpol. in field f1 for ptm
            double v2=vecf[k1].second*(1-u)+vecf[k2].second*(u);//Interpol. in field f2 for ptm
//            std::cout << u << " " << v1 << " " << v2 << " "  << v1+v2 << " "  << ((vecfd[k2]-vecfd[k1])) << std::endl;
            if ((first)||(v1+v2)<minv) //if 1st elem or new sum v1+v2 < old one
            {  
              first=false;
              minv=v1+v2;
              val1=v1;
              val2=v2;
              minp=ptm;//Point interpolated is conserved for output
              
            }
          }
        }
            
      }
    }
    pos= minp;
 //   std::cout << std::endl;
//    std::cout << minv << " " << pos[0] << " " << pos[1] << " "<< pos[2] << std::endl;
    
  }
  if (crvmax>crv) // to adjust 
  {
    return 1;
  }
  return 0;
}

int FindMiniPath::getMiniPoint(double valdep,double valarr,double& val1,double& val2,SPoint3 &pos,int f1, int f2,bool singular)
{
  // - find iso-0 field1-field2
  // - along iso-0, find minimal value of field1+field2
  // - return its position, and values of field1 and field
  // -  if there is a singular point, returns 1, and sing point position. Otherwise return 0

  int sing=getSingularPoint(valdep, valarr,val1, val2,pos,f1, f2,0.000015);//if sing=1, there is a singularity at SPoint3 pos
  
  if (sing&&singular) return 1;
  // this part below is quite similar to if(found) of getSingularPoint
  SPoint3 minp(0.,0.,0.);//Mini point
  double minv(0.);//Minimum value of field 1 + field 2 at point minp
  val1=0.;//Value in field f1 at mini point
  val2=0.;//Value in field f2 at mini point
  bool first=true;//true if first iteration
  for ( int i=0;i<FM::FMModel::Ptr->Els.size();++i)
  {
    FM::gmshElement e(FM::FMModel::Ptr->Els[i]);//ith element of FMModel
    std::vector<FM::gmshNode> vec;//Edges of element e
    e.GetNodes(vec);//Read edges of element e into vec
    std::vector<std::pair<double,double> > vecf(vec.size());
    std::vector<double > vecfd(vec.size());
    bool pos=false;//vecfd has at least one positive value
    bool neg=false;//vecfd has at least one negative value
    for (int j=0;j!=vec.size();++j)
    {
      vecf[j].first=fields[f1](vec[j]).first;
      vecf[j].second=fields[f2](vec[j]).first;
      vecfd[j]=(vecf[j].first-vecf[j].second);
      if (vecfd[j]>=0.) pos=true;
      else neg=true;
    }
    
    if (pos&&neg) //if vecfd changes sign
    {
      for (int j=0;j!=vec.size();++j) //loop on nodes
      {
        int k1=j;
        int k2=(j+1)%vec.size();
        if ((vecfd[k1]*vecfd[k2])<=0.) //if sign change between nodes k1 and k2
        {
          // interpolate point and values in fields
          double u=-(vecfd[k1])/((vecfd[k2]-vecfd[k1]));
          SPoint3 pt1;
          SPoint3 pt2;
          vec[k1].GetPos(pt1[0],pt1[1],pt1[2]);
          vec[k2].GetPos(pt2[0],pt2[1],pt2[2]);
          SPoint3 ptm=pt1*(1-u) +pt2*(u);
          double v1=vecf[k1].first*(1-u)+vecf[k2].first*(u);
          double v2=vecf[k1].second*(1-u)+vecf[k2].second*(u);
          if ((first)||(v1+v2)<minv) // if first case or new minimal value, new mini point
          {  
            first=false;
            minv=v1+v2;
            val1=v1;
            val2=v2;
            minp=ptm;
          }
        }
      }
    }
  }
  pos= minp;
  return 0;
}