// Gmsh - Copyright (C) 1997-2016 C. Geuzaine, J.-F. Remacle
//
// See the LICENSE.txt file for license information. Please report all
// bugs and problems to the public mailing list <gmsh@onelab.info>.

#include "meshMetric.h"
#include "meshGFaceOptimize.h"
#include "Context.h"
#include "GEntity.h"
#include "GModel.h"
#include "gmshLevelset.h"
#include "MElementOctree.h"
#include "OS.h"
#include <algorithm>

meshMetric::meshMetric(GModel *gm)
{
  hasAnalyticalMetric = false;
  _dim  = gm->getDim();
  std::map<MElement*, MElement*> newP;
  std::map<MElement*, MElement*> newD;

  if (_dim == 2){
    for (GModel::fiter fit = gm->firstFace(); fit != gm->lastFace(); ++fit){
      for (unsigned int i=0;i<(*fit)->getNumMeshElements();i++){
        MElement *e = (*fit)->getMeshElement(i);
        MElement *copy = e->copy(_vertexMap, newP, newD);
        _elements.push_back(copy);
      }
    }
  }
  else if (_dim == 3){
    for (GModel::riter rit = gm->firstRegion(); rit != gm->lastRegion(); ++rit){
      for (unsigned int i=0;i<(*rit)->getNumMeshElements();i++){
        MElement *e = (*rit)->getMeshElement(i);
        MElement *copy = e->copy(_vertexMap, newP, newD);
        _elements.push_back(copy);
      }
    }
  }
  _octree = new MElementOctree(_elements);
  buildVertexToElement (_elements,_adj);
}

meshMetric::meshMetric(std::vector<MElement*> elements)
{
  hasAnalyticalMetric = false;

  _dim  = elements[0]->getDim();
  std::map<MElement*, MElement*> newP;
  std::map<MElement*, MElement*> newD;

  for (unsigned int i=0;i<elements.size();i++){
    MElement *e = elements[i];
    MElement *copy = e->copy(_vertexMap, newP, newD);
    _elements.push_back(copy);
  }

  _octree = new MElementOctree(_elements);
  buildVertexToElement (_elements,_adj);
}

void meshMetric::addMetric(int technique, simpleFunction<double> *fct,
                           std::vector<double> parameters)
{
  needMetricUpdate = true;

  int metricNumber = setOfMetrics.size();
  setOfFcts[metricNumber] = fct;
  setOfParameters[metricNumber] = parameters;
  setOfTechniques[metricNumber] = technique;

  if (fct->hasDerivatives()) hasAnalyticalMetric = true;

  computeMetric(metricNumber);
}

void meshMetric::updateMetrics()
{
  if (!setOfMetrics.size()){
    Msg::Error("Can't intersect metrics, no metric registered");
    return;
  }

  v2t_cont :: iterator it = _adj.begin();
  for (;it != _adj.end();it++) {
    MVertex *ver = it->first;
    _nodalMetrics[ver] = setOfMetrics[0][ver];
    _nodalSizes[ver] = setOfSizes[0][ver];

    if (setOfMetrics.size() > 1)
      for (unsigned int i=1;i<setOfMetrics.size();i++){
        _nodalMetrics[ver] = (_dim == 3) ?
          intersection_conserve_mostaniso(_nodalMetrics[ver],setOfMetrics[i][ver]) :
          intersection_conserve_mostaniso_2d(_nodalMetrics[ver],setOfMetrics[i][ver]);
        _nodalSizes[ver] = std::min(_nodalSizes[ver],setOfSizes[i][ver]);
      }
  }
  needMetricUpdate=false;
}

void meshMetric::exportInfo(const char * fileendname)
{
  if (needMetricUpdate) updateMetrics();
  std::stringstream sg,sm,sl,sh,shm;
  sg << "meshmetric_gradients_" << fileendname;
  sm << "meshmetric_metric_" << fileendname;
  sl << "meshmetric_levelset_" << fileendname;
  sh << "meshmetric_hessian_" << fileendname;
  shm << "meshmetric_hessianmat_" << fileendname;
  std::ofstream out_grad(sg.str().c_str());
  std::ofstream out_metric(sm.str().c_str());
  std::ofstream out_ls(sl.str().c_str());
  std::ofstream out_hess(sh.str().c_str());
  std::ofstream out_hessmat(shm.str().c_str());
  out_grad << "View \"ls_gradient\"{" << std::endl;
  out_metric << "View \"metric\"{" << std::endl;
  out_ls << "View \"ls\"{" << std::endl;
  out_hess << "View \"hessian\"{" << std::endl;
  out_hessmat << "View \"hessian_mat\"{" << std::endl;
  std::vector<MElement*>::iterator itelem = _elements.begin();
  std::vector<MElement*>::iterator itelemen = _elements.end();
  for (;itelem!=itelemen;itelem++){
    MElement* e = *itelem;
    if (e->getDim() == 2) {
      out_metric << "TT(";
      out_grad << "VT(";
      out_ls << "ST(";
      out_hess << "ST(";
      out_hessmat << "TT(";
    }
    else {
      out_metric << "TS(";
      out_grad << "VS(";
      out_ls << "SS(";
      out_hess << "SS(";
      out_hessmat << "TS(";
    }
    for ( int i = 0; i < e->getNumVertices(); i++) {
      MVertex *ver = e->getVertex(i);
      out_metric << ver->x() << "," << ver->y() << "," << ver->z();
      out_grad << ver->x() << "," << ver->y() << "," << ver->z();
      out_ls << ver->x() << "," << ver->y() << "," << ver->z();
      out_hess << ver->x() << "," << ver->y() << "," << ver->z();
      out_hessmat << ver->x() << "," << ver->y() << "," << ver->z();
      if (i!=e->getNumVertices()-1){
        out_metric << ",";
        out_grad << ",";
        out_ls << ",";
        out_hess << ",";
        out_hessmat << ",";
      }
      else{
        out_metric << "){";
        out_grad << "){";
        out_ls << "){";
        out_hess << "){";
        out_hessmat << "){";
      }
    }
    for ( int i = 0; i < e->getNumVertices(); i++) {
      MVertex *ver = e->getVertex(i);
      out_ls << vals[ver];
      out_hess << (hessians[ver](0,0) + hessians[ver](1,1) + hessians[ver](2,2));
      if (i == (e->getNumVertices() - 1)){
        out_ls << "};" << std::endl;
        out_hess << "};" << std::endl;
      }
      else {
        out_ls << ",";
        out_hess << ",";
      }
      for (int k=0;k<3;k++){
        out_grad << grads[ver](k);
        if ((k == 2) && (i == (e->getNumVertices() - 1))) out_grad << "};" << std::endl;
        else out_grad << ",";
        for (int l=0;l<3;l++){
          out_metric << _nodalMetrics[ver](k,l);
          out_hessmat << hessians[ver](k,l);
          if ((k == 2) && (l == 2) && (i == (e->getNumVertices() - 1))) {
            out_metric << "};" << std::endl;
            out_hessmat << "};" << std::endl;
          }
          else {
            out_metric << ",";
            out_hessmat << ",";
          }
        }
      }
    }
  }
  out_grad << "};" << std::endl;
  out_metric << "};" << std::endl;
  out_ls << "};" << std::endl;
  out_hess << "};" << std::endl;
  out_hessmat << "};" << std::endl;
  out_grad.close();
  out_metric.close();
  out_ls.close();
  out_hess.close();
  out_hessmat.close();
}

meshMetric::~meshMetric()
{
  if (_octree) delete _octree;
  for (unsigned int i=0; i< _elements.size(); i++)
    delete _elements[i];
}

void meshMetric::computeValues()
{

  v2t_cont :: iterator it = _adj.begin();
  while (it != _adj.end()) {
    std::vector<MElement*> lt = it->second;
    MVertex *ver = it->first;
    vals[ver]= (*_fct)(ver->x(),ver->y(),ver->z());
    it++;
  }

}

// Determines set of vertices to use for least squares
std::vector<MVertex*> getLSBlob(unsigned int minNbPt, v2t_cont::iterator it,
                                v2t_cont &adj)
{

  //  static const double RADFACT = 3;
  //
  //  SPoint3 p0 = it->first->point();  // Vertex coordinates (center of circle)
  //
  //  double rad = 0.;
  //  for (std::vector<MElement*>::iterator itEl = it->second.begin(); itEl != it->second.end(); itEl++)
  //    rad = std::max(rad,(*itEl)->getOuterRadius());
  //  rad *= RADFACT;

  std::vector<MVertex*> vv(1,it->first), bvv = vv;                                                      // Vector of vertices in blob and in boundary of blob
  do {
    std::set<MVertex*> nbvv;                                                                            // Set of vertices in new boundary
    for (std::vector<MVertex*>::iterator itBV = bvv.begin(); itBV != bvv.end(); itBV++) {               // For each boundary vertex...
      std::vector<MElement*> &adjBV = adj[*itBV];
      for (std::vector<MElement*>::iterator itBVEl = adjBV.begin(); itBVEl != adjBV.end(); itBVEl++) {
        for (int iV=0; iV<(*itBVEl)->getNumVertices(); iV++){                                           // ... look for adjacent vertices...
          MVertex *v = (*itBVEl)->getVertex(iV);
	  //          if ((find(vv.begin(),vv.end(),v) == vv.end()) && (p0.distance(v->point()) <= rad)) nbvv.insert(v);
          if (find(vv.begin(),vv.end(),v) == vv.end()) nbvv.insert(v);                                  // ... and add them in the new boundary if they are not already in the blob
        }
      }
    }
    if (nbvv.empty()) bvv.clear();
    else {
      bvv.assign(nbvv.begin(),nbvv.end());
      vv.insert(vv.end(),nbvv.begin(),nbvv.end());
    }
    //  } while (!bvv.empty());
  } while (vv.size() < minNbPt);                                                                        // Repeat until min. number of points is reached

  return vv;

}

// Compute derivatives and second order derivatives using least squares
// 2D LS system: a_i0*x^2+a_i1*x*y+a_i2*y^2+a_i3*x+a_i4*y+a_i5=b_i
// 3D LS system: a_i0*x^2+a_i1*x*y+a_i2*x*z+a_i3*y^2+a_i4*y*z+a_i5*z^2+a_i6*x+a_i7*y+a_i8*z+a_i9=b_i
void meshMetric::computeHessian()
{
  unsigned int sysDim = (_dim == 2) ? 6 : 10;
  unsigned int minNbPtBlob = 3*sysDim;

  for (v2t_cont::iterator it = _adj.begin(); it != _adj.end(); it++) {
    MVertex *ver = it->first;
    std::vector<MVertex*> vv = getLSBlob(minNbPtBlob, it, _adj);
    fullMatrix<double> A(vv.size(),sysDim), ATA(sysDim,sysDim);
    fullVector<double> b(vv.size()), ATb(sysDim), coeffs(sysDim);
    for(unsigned int i=0; i<vv.size(); i++) {
      const double &x = vv[i]->x(), &y = vv[i]->y(), &z = vv[i]->z();
      if (_dim == 2) {
        A(i,0) = x*x; A(i,1) = x*y; A(i,2) = y*y;
        A(i,3) = x; A(i,4) = y; A(i,5) = 1.;
      }
      else {
        A(i,0) = x*x; A(i,1) = x*y; A(i,2) = x*z; A(i,3) = y*y; A(i,4) = y*z; A(i,5) = z*z;
        A(i,6) = x; A(i,7) = y; A(i,8) = z; A(i,9) = 1.;
      }
      b(i) = vals[vv[i]];
    }
    ATA.gemm(A,A,1.,0., true, false);
    A.multWithATranspose(b,1.,0.,ATb);
    ATA.luSolve(ATb,coeffs);
    const double &x = ver->x(), &y = ver->y(), &z = ver->z();
    double d2udx2, d2udy2, d2udz2, d2udxy, d2udxz, d2udyz, dudx, dudy, dudz;
    if (_dim == 2) {
      d2udx2 = 2.*coeffs(0); d2udy2 = 2.*coeffs(2); d2udz2 = 0.;
      d2udxy = coeffs(1); d2udxz = 0.; d2udyz = 0.;
      dudx = d2udx2*x+d2udxy*y+coeffs(3);
      dudy = d2udxy*x+d2udy2*y+coeffs(4);
      dudz = 0.;
    }
    else {
      d2udx2 = 2.*coeffs(0); d2udy2 = 2.*coeffs(3); d2udz2 = 2.*coeffs(5);
      d2udxy = coeffs(1); d2udxz = coeffs(2); d2udyz = coeffs(4);
      dudx = d2udx2*x+d2udxy*y+d2udxz*z+coeffs(6);
      dudy = d2udxy*x+d2udy2*y+d2udyz*z+coeffs(7);
      dudz = d2udxz*x+d2udyz*y+d2udz2*z+coeffs(8);
    }
    double duNorm = sqrt(dudx*dudx+dudy*dudy+dudz*dudz);
    if (duNorm == 0. || _technique == meshMetric::HESSIAN ||
        _technique == meshMetric::EIGENDIRECTIONS ||
        _technique == meshMetric::EIGENDIRECTIONS_LINEARINTERP_H) duNorm = 1.;
    grads[ver] = SVector3(dudx/duNorm,dudy/duNorm,dudz/duNorm);
    hessians[ver](0,0) = d2udx2; hessians[ver](0,1) = d2udxy; hessians[ver](0,2) = d2udxz;
    hessians[ver](1,0) = d2udxy; hessians[ver](1,1) = d2udy2; hessians[ver](1,2) = d2udyz;
    hessians[ver](2,0) = d2udxz; hessians[ver](2,1) = d2udyz; hessians[ver](2,2) = d2udz2;
  }
}

void meshMetric::computeMetricLevelSet(MVertex *ver, SMetric3 &hessian,
                                       SMetric3 &metric, double &size,
                                       double x, double y, double z)
{
  double signed_dist;
  SVector3 gr;
  if(ver){
    signed_dist = vals[ver];
    gr = grads[ver];
    hessian = hessians[ver];
  }
  else{
    signed_dist = (*_fct)(x,y,z);
    _fct->gradient(x,y,z,gr(0),gr(1),gr(2));
    _fct->hessian(x,y,z, hessian(0,0),hessian(0,1),hessian(0,2),
		  hessian(1,0),hessian(1,1),hessian(1,2),
		  hessian(2,0),hessian(2,1),hessian(2,2));
  }

  double dist = fabs(signed_dist);

  SMetric3 H;
  double norm = gr(0)*gr(0)+gr(1)*gr(1)+gr(2)*gr(2);
  if (dist < _E && norm != 0.0){
    double h = hmin*(hmax/hmin-1)*dist/_E + hmin;
    double C = 1./(h*h) -1./(hmax*hmax);
    H(0,0) += C*gr(0)*gr(0)/norm ;
    H(1,1) += C*gr(1)*gr(1)/norm ;
    H(2,2) += C*gr(2)*gr(2)/norm ;
    H(1,0) = H(0,1) = C*gr(1)*gr(0)/norm ;
    H(2,0) = H(0,2) = C*gr(2)*gr(0)/norm ;
    H(2,1) = H(1,2) = C*gr(2)*gr(1)/norm ;
  }

  fullMatrix<double> V(3,3);
  fullVector<double> S(3);
  H.eig(V,S);

  double lambda1, lambda2, lambda3;
  lambda1 = S(0);
  lambda2 = S(1);
  lambda3 = (_dim == 3)? S(2) : 1.;

  SVector3 t1 (V(0,0),V(1,0),V(2,0));
  SVector3 t2 (V(0,1),V(1,1),V(2,1));
  SVector3 t3 (V(0,2),V(1,2),V(2,2));

  size = std::min(std::min(1/sqrt(lambda1),1/sqrt(lambda2)),1/sqrt(lambda3));
  metric = SMetric3(lambda1,lambda2,lambda3,t1,t2,t3);
}

void meshMetric::computeMetricHessian(MVertex *ver, SMetric3 &hessian,
                                      SMetric3 &metric, double &size,
                                      double x, double y, double z)
{

  SVector3 gr;
  if(ver != NULL){
    gr = grads[ver];
    hessian = hessians[ver];
  }
  else if (ver == NULL){
    _fct->gradient(x,y,z,gr(0),gr(1),gr(2));
    _fct->hessian(x,y,z, hessian(0,0),hessian(0,1),hessian(0,2),
		  hessian(1,0),hessian(1,1),hessian(1,2),
		  hessian(2,0),hessian(2,1),hessian(2,2));
  }

  double _epsilonP = 1.;
  double hminP = 1.e-12;
  double hmaxP = 1.e+12;

  fullMatrix<double> V(3,3);
  fullVector<double> S(3);
  hessian.eig(V,S);

  double lambda1 = std::min(std::max(fabs(S(0))/_epsilonP,1./(hmaxP*hmaxP)),1./(hminP*hminP));
  double lambda2 = std::min(std::max(fabs(S(1))/_epsilonP,1./(hmaxP*hmaxP)),1./(hminP*hminP));
  double lambda3 = (_dim == 3) ? std::min(std::max(fabs(S(2))/_epsilonP,1./(hmaxP*hmaxP)),1./(hminP*hminP)) : 1.;

  SVector3 t1 (V(0,0),V(1,0),V(2,0));
  SVector3 t2 (V(0,1),V(1,1),V(2,1));
  SVector3 t3  = (_dim == 3) ? SVector3(V(0,2),V(1,2),V(2,2)) : SVector3(0.,0.,1.);

  size =  std::min(std::min(1/sqrt(lambda1),1/sqrt(lambda2)),1/sqrt(lambda3));
  metric = SMetric3(lambda1,lambda2,lambda3,t1,t2,t3);

}

void meshMetric::computeMetricFrey(MVertex *ver, SMetric3 &hessian,
                                   SMetric3 &metric, double &size,
                                   double x, double y, double z)
{
  double signed_dist;
  SVector3 gr;
  if(ver){
    signed_dist = vals[ver];
    gr = grads[ver];
    hessian = hessians[ver];
  }
  else{
    signed_dist = (*_fct)(x,y,z);
    _fct->gradient(x,y,z,gr(0),gr(1),gr(2));
    _fct->hessian(x,y,z, hessian(0,0),hessian(0,1),hessian(0,2),
		  hessian(1,0),hessian(1,1),hessian(1,2),
		  hessian(2,0),hessian(2,1),hessian(2,2));
  }

  double dist = fabs(signed_dist);

  SMetric3 H(1./(hmax*hmax));
  double norm = gr(0)*gr(0)+gr(1)*gr(1)+gr(2)*gr(2);
  if (dist < _E && norm != 0.0){
    double h = hmin*(hmax/hmin-1.0)*dist/_E + hmin;
    double C = 1./(h*h) -1./(hmax*hmax);
    double kappa = hessian(0,0)+hessian(1,1)+hessian(2,2);
    double epsGeom = 4.0*3.14*3.14/(kappa*_Np*_Np);
    H(0,0) += C*gr(0)*gr(0)/(norm) + hessian(0,0)/epsGeom;
    H(1,1) += C*gr(1)*gr(1)/(norm) + hessian(1,1)/epsGeom;
    H(2,2) += C*gr(2)*gr(2)/(norm) + hessian(2,2)/epsGeom;
    H(1,0) = H(0,1) = C*gr(1)*gr(0)/(norm) + hessian(1,0)/epsGeom;
    H(2,0) = H(0,2) = C*gr(2)*gr(0)/(norm) + hessian(2,0)/epsGeom;
    H(2,1) = H(1,2) = C*gr(2)*gr(1)/(norm) + hessian(2,1)/epsGeom;
  }

  fullMatrix<double> V(3,3);
  fullVector<double> S(3);
  H.eig(V,S);

  double lambda1, lambda2, lambda3;
  lambda1 = S(0);
  lambda2 = S(1);
  lambda3 = (_dim == 3)? S(2) : 1.;

  if (dist < _E) {
    lambda1 = std::min(std::max(fabs(S(0))/_epsilon,1./(hmax*hmax)),1./(hmin*hmin));
    lambda2 = std::min(std::max(fabs(S(1))/_epsilon,1./(hmax*hmax)),1./(hmin*hmin));
    lambda3 = (_dim == 3) ? std::min(std::max(fabs(S(2))/_epsilon,1./(hmax*hmax)),1./(hmin*hmin)) : 1.;
  }

  SVector3 t1 (V(0,0),V(1,0),V(2,0));
  SVector3 t2 (V(0,1),V(1,1),V(2,1));
  SVector3 t3 (V(0,2),V(1,2),V(2,2));

  size =  std::min(std::min(1/sqrt(lambda1),1/sqrt(lambda2)),1/sqrt(lambda3));
  metric = SMetric3(lambda1,lambda2,lambda3,t1,t2,t3);

}

void meshMetric::computeMetricEigenDir(MVertex *ver, SMetric3 &hessian,
                                       SMetric3 &metric, double &size,
                                       double x, double y, double z)
{
  double signed_dist;
  SVector3 gVec;
  if(ver){
    signed_dist = vals[ver];
    gVec = grads[ver];
    hessian = hessians[ver];
  }
  else{
    signed_dist = (*_fct)(x,y,z);
    _fct->gradient(x,y,z,gVec(0),gVec(1),gVec(2));
    _fct->hessian(x,y,z, hessian(0,0),hessian(0,1),hessian(0,2),
		  hessian(1,0),hessian(1,1),hessian(1,2),
		  hessian(2,0),hessian(2,1),hessian(2,2));
  }

  double dist = fabs(signed_dist);

  const double metric_value_hmax = 1./(hmax*hmax);
  const double gMag = gVec.norm(), invGMag = 1./gMag;

  if (signed_dist < _E && signed_dist > _E_moins && gMag != 0.0){
    const double metric_value_hmin = 1./(hmin*hmin);
    const SVector3 nVec = invGMag*gVec;                                                         // Unit normal vector
    double lambda_n = 0.;                                                                            // Eigenvalues of metric for normal & tangential directions
    if (_technique==meshMetric::EIGENDIRECTIONS_LINEARINTERP_H){
      //const double h_dist = hmin + ((hmax-hmin)/_E)*dist;                                       // Characteristic element size in the normal direction - linear interp between hmin and hmax
      //lambda_n = 1./(h_dist*h_dist);
      double beta = CTX::instance()->mesh.smoothRatio;
      double h_dista  = std::min( (hmin+(dist*log(beta))), hmax);
      lambda_n = 1./(h_dista*h_dista);
    }
    else if(_technique==meshMetric::EIGENDIRECTIONS){
      const double maximum_distance = (signed_dist>0.) ? _E : fabs(_E_moins);                   // ... or linear interpolation between 1/h_min^2 and 1/h_max^2
      lambda_n = metric_value_hmin + ((metric_value_hmax-metric_value_hmin)/maximum_distance)*dist;
    }
    std::vector<SVector3> tVec;                                                                 // Unit tangential vectors
    std::vector<double> kappa;                                                                  // Curvatures
    if (_dim == 2) {                                                                            // 2D curvature formula: cf. R. Goldman, "Curvature formulas for implicit curves and surfaces", Computer Aided Geometric Design 22 (2005), pp. 632–658
      kappa.resize(2);
      kappa[0] = fabs(-gVec(1)*(-gVec(1)*hessian(0,0)+gVec(0)*hessian(0,1))+
		      gVec(0)*(-gVec(1)*hessian(1,0)+gVec(0)*hessian(1,1)))*pow(invGMag,3);
      kappa[1] = 1.;
      tVec.resize(2);
      tVec[0] = SVector3(-nVec(1),nVec(0),0.);
      tVec[1] = SVector3(0.,0.,1.);
    }
    else {                                                                                      // 3D curvature formula: cf. A.G. Belyaev, A.A. Pasko and T.L. Kunii, "Ridges and Ravines on Implicit Surfaces," CGI, pp.530-535, Computer Graphics International 1998 (CGI'98), 1998
      fullMatrix<double> ImGG(3,3);
      ImGG(0,0) = 1.-gVec(0)*gVec(0); ImGG(0,1) = -gVec(0)*gVec(1); ImGG(0,2) = -gVec(0)*gVec(2);
      ImGG(1,0) = -gVec(1)*gVec(0); ImGG(1,1) = 1.-gVec(1)*gVec(1); ImGG(1,2) = -gVec(1)*gVec(2);
      ImGG(2,0) = -gVec(2)*gVec(0); ImGG(2,1) = -gVec(2)*gVec(1); ImGG(2,2) = 1.-gVec(2)*gVec(2);
      fullMatrix<double> hess(3,3);
      hessian.getMat(hess);
      fullMatrix<double> gN(3,3);                                                               // Gradient of unit normal
      gN.gemm(ImGG,hess,1.,0.);
      gN.scale(invGMag);
      fullMatrix<double> eigVecL(3,3), eigVecR(3,3);
      fullVector<double> eigValRe(3), eigValIm(3);
      gN.eig(eigValRe,eigValIm,eigVecL,eigVecR,false);                                          // Eigendecomp. of gradient of unit normal
      kappa.resize(3);                                                                          // Store abs. val. of eigenvalues (= principal curvatures only in non-normal directions)
      kappa[0] = fabs(eigValRe(0));
      kappa[1] = fabs(eigValRe(1));
      kappa[2] = fabs(eigValRe(2));
      tVec.resize(3);                                                                           // Store normalized eigenvectors (= principal directions only in non-normal directions)
      tVec[0] = SVector3(eigVecR(0,0),eigVecR(1,0),eigVecR(2,0));
      tVec[0].normalize();
      tVec[1] = SVector3(eigVecR(0,1),eigVecR(1,1),eigVecR(2,1));
      tVec[1].normalize();
      tVec[2] = SVector3(eigVecR(0,2),eigVecR(1,2),eigVecR(2,2));
      tVec[2].normalize();
      std::vector<double> tVecDotNVec(3);                                                       // Store dot products with normal vector to look for normal direction
      tVecDotNVec[0] = fabs(dot(tVec[0],nVec));
      tVecDotNVec[1] = fabs(dot(tVec[1],nVec));
      tVecDotNVec[2] = fabs(dot(tVec[2],nVec));
      const int i_N = max_element(tVecDotNVec.begin(),tVecDotNVec.end())-tVecDotNVec.begin();   // Index of normal dir. = max. dot products (close to 0. in tangential dir.)
      kappa.erase(kappa.begin()+i_N);                                                           // Remove normal dir.
      tVec.erase(tVec.begin()+i_N);
    }
    const double invh_t0 = (_Np*kappa[0])/6.283185307, invh_t1 = (_Np*kappa[1])/6.283185307;    // Inverse of tangential size 0
    const double lambda_t0 = invh_t0*invh_t0, lambda_t1 = invh_t1*invh_t1;
    const double lambdaP_n = std::min(std::max(lambda_n,metric_value_hmax),metric_value_hmin);  // Truncate eigenvalues
    const double lambdaP_t0 = std::min(std::max(lambda_t0,metric_value_hmax),metric_value_hmin);
    const double lambdaP_t1 = (_dim == 2) ? 1. : std::min(std::max(lambda_t1,metric_value_hmax),metric_value_hmin);
    metric = SMetric3(lambdaP_n,lambdaP_t0,lambdaP_t1,nVec,tVec[0],tVec[1]);
    const double h_n = 1./sqrt(lambdaP_n), h_t0 = 1./sqrt(lambdaP_t0), h_t1 = 1./sqrt(lambdaP_t1);
    size = std::min(std::min(h_n,h_t0),h_t1);
  }
  else{// isotropic metric !
    SMetric3 mymetric(metric_value_hmax);
    metric = mymetric;
    size = hmax;
  }
}

void meshMetric::computeMetricIsoLinInterp(MVertex *ver, SMetric3 &hessian,
                                           SMetric3 &metric, double &size,
                                           double x, double y, double z)
{
  double signed_dist;
  SVector3 gr;
  if(ver){
    signed_dist = vals[ver];
    gr = grads[ver];
    hessian = hessians[ver];
  }
  else{
    signed_dist = (*_fct)(x,y,z);
    _fct->gradient(x,y,z,gr(0),gr(1),gr(2));
    _fct->hessian(x,y,z, hessian(0,0),hessian(0,1),hessian(0,2),
		  hessian(1,0),hessian(1,1),hessian(1,2),
		  hessian(2,0),hessian(2,1),hessian(2,2));
  }

  double dist = fabs(signed_dist);
  double norm = gr.normalize();
  size = hmax;                      // the characteristic element size in all directions - linear interp between hmin and hmax
  if (norm != 0.) {
    if ((signed_dist >= 0) && (signed_dist < _E)) size = hmin + ((hmax-hmin)/_E)*dist;
    else if ((signed_dist < 0) && (signed_dist > _E_moins)) size = hmin - ((hmax-hmin)/_E_moins)*dist;
  }

  double lambda = 1./size/size;
  metric(0,0) = lambda; metric(0,1) = 0.; metric(0,2) = 0.;
  metric(1,0) = 0.; metric(1,1) = lambda; metric(1,2) = 0.;
  metric(2,0) = 0.; metric(2,1) = 0.; metric(2,2) = (_dim == 3)? lambda : 1.;
}

// this function scales the mesh metric in order
// to reach a target number of elements
// We know that the number of elements in the final
// mesh will be (assuming M_e the metric at centroid of element e)
//   N = \sum_e \sqrt {\det (M_e)} V_e
// where V_e is the volume of e
// assuming that N_{target} = K N, we have
//   K N = K \sum_e \sqrt {\det (M_e)} V_e
//       =   \sum_e \sqrt {K^2 \det (M_e)} V_e
//       =   \sum_e \sqrt {\det (K^{2/d} M_e)} V_e
//  where d is the dimension of the problem.
// This means that the metric should be scaled by K^{2/d} where
// K is N_target / N
void meshMetric::scaleMetric(int nbElementsTarget,
                             nodalMetricTensor &nmt)
{
  // compute N
  double N = 0;
  for (unsigned int i=0;i<_elements.size();i++){
    MElement *e = _elements[i];
    SMetric3 m1 = nmt[e->getVertex(0)];
    SMetric3 m2 = nmt[e->getVertex(1)];
    SMetric3 m3 = nmt[e->getVertex(2)];
    if (_dim == 2){
      SMetric3 m =  interpolation(m1,m2,m3,0.3333,0.3333);
      N += sqrt(m.determinant()) * e->getVolume()  * 4./sqrt(3.0); //3.0
    }
    else{
      SMetric3 m4 = nmt[e->getVertex(3)];
      SMetric3 m =  interpolation(m1,m2,m3,m4,0.25,0.25,0.25);
      N += sqrt(m.determinant()) * e->getVolume() * 12./sqrt(2.0); //4.0;
    }
  }
  double scale = pow ((double)nbElementsTarget/N,2.0/_dim);
  for (nodalMetricTensor::iterator it = nmt.begin(); it != nmt.end() ; ++it){
    if (_dim == 3){
      it->second *= scale;
    }
    else {
      it->second(0,0) *= scale;
      it->second(1,0) *= scale;
      it->second(1,1) *= scale;
    }
    SMetric3 &m = it->second;
    fullMatrix<double> V(3,3);
    fullVector<double> S(3);
    m.eig(V,S);
    S(0) = std::min(std::max(S(0),1/(hmax*hmax)),1/(hmin*hmin));
    S(1) = std::min(std::max(S(1),1/(hmax*hmax)),1/(hmin*hmin));
    if (_dim == 3)S(2) = std::min(std::max(S(2),1/(hmax*hmax)),1/(hmin*hmin));
    SVector3 t1 (V(0,0),V(1,0),V(2,0));
    SVector3 t2 (V(0,1),V(1,1),V(2,1));
    SVector3 t3 (V(0,2),V(1,2),V(2,2));
    m = SMetric3(S(0),S(1),S(2),t1,t2,t3);
  }
}

void meshMetric::computeMetric(int metricNumber)
{
  _fct = setOfFcts[metricNumber];
  std::vector<double>  parameters = setOfParameters[metricNumber];
  int technique = setOfTechniques[metricNumber];

  hmin = parameters.size() >=3 ? parameters[1] : CTX::instance()->mesh.lcMin;
  hmax = parameters.size() >=3 ? parameters[2] : CTX::instance()->mesh.lcMax;
  _E = parameters[0];
  _E_moins = (parameters.size() >= 5) ? parameters[4] : -parameters[0];
  if (_E_moins>0.) _E_moins *= -1.;
  _epsilon = parameters[0];
  _technique =  (MetricComputationTechnique)technique;
  _Np = (parameters.size() >= 4) ? parameters[3] : 15.;

  computeValues();
  computeHessian();

  for (v2t_cont::iterator it=_adj.begin(); it!=_adj.end(); it++) {

    MVertex *ver = it->first;
    SMetric3 hessian, metric;
    double size;
    switch(_technique) {
    case (LEVELSET) : computeMetricLevelSet(ver, hessian, metric, size); break;
    case (HESSIAN) : computeMetricHessian(ver, hessian, metric, size); break;
    case (FREY) : computeMetricFrey(ver, hessian, metric, size); break;
    case (EIGENDIRECTIONS) : computeMetricEigenDir(ver, hessian, metric, size); break;
    case (EIGENDIRECTIONS_LINEARINTERP_H) : computeMetricEigenDir(ver, hessian, metric,size); break;
    case (ISOTROPIC_LINEARINTERP_H) : computeMetricIsoLinInterp(ver, hessian, metric, size); break;
    }

    setOfSizes[metricNumber].insert(std::make_pair(ver,size));
    setOfMetrics[metricNumber].insert(std::make_pair(ver,metric));

  }

  if( _technique ==  HESSIAN)  scaleMetric(_epsilon, setOfMetrics[metricNumber]);
}

double meshMetric::operator() (double x, double y, double z, GEntity *ge)
{
  if (needMetricUpdate) updateMetrics();
  if (!setOfMetrics.size()){
    std::cout  << "meshMetric::operator() : No metric defined ! " << std::endl;
    throw;
  }
  SPoint3 xyz(x,y,z), uvw;
  double initialTol = MElement::getTolerance();
  MElement::setTolerance(1.e-4);
  MElement *e = _octree->find(x, y, z, _dim);
  MElement::setTolerance(initialTol);
  double value = 0.;
  if (e) {
    e->xyz2uvw(xyz,uvw);
    double *val = new double [e->getNumVertices()];
    for (int i=0;i<e->getNumVertices();i++){
      val[i] = _nodalSizes[e->getVertex(i)];
    }
    value = e->interpolate(val,uvw[0],uvw[1],uvw[2]);
    delete [] val;
  }
  else {
    Msg::Warning("point %g %g %g not found, looking for nearest node",x,y,z);
    double minDist = 1.e100;
    for (nodalField::iterator it = _nodalSizes.begin(); it != _nodalSizes.end(); it++) {
      const double dist = xyz.distance(it->first->point());
      if (dist <= minDist) {
        minDist = dist;
        value = it->second;
      }
    }
  }
  return value;
}

void meshMetric::operator() (double x, double y, double z, SMetric3 &metr, GEntity *ge)
{
  if (needMetricUpdate) {
    updateMetrics();
  }
  if (!setOfMetrics.size()){
    std::cout  << "meshMetric::operator() : No metric defined ! " << std::endl;
    throw;
  }
  metr = SMetric3(1.e-22);

  //RECOMPUTE MESH METRIC AT XYZ
  if (hasAnalyticalMetric){
    int nbMetrics = setOfMetrics.size();
    std::vector<SMetric3> newSetOfMetrics(nbMetrics);
    for (int iMetric=0;iMetric<nbMetrics;iMetric++){
      _fct = setOfFcts[iMetric];
      _technique  = (MetricComputationTechnique)setOfTechniques[iMetric];
      if (_fct->hasDerivatives()){
	SMetric3 hessian, metric;
	double size;
	switch(_technique) {
	case (LEVELSET) : computeMetricLevelSet(NULL,hessian,metric,size,x,y,z); break;
	case (HESSIAN) : computeMetricHessian(NULL, hessian,metric,size,x,y,z); break;
	case (FREY) : computeMetricFrey(NULL, hessian,metric,size,x,y,z); break;
	case (EIGENDIRECTIONS) : computeMetricEigenDir(NULL, hessian,metric,size,x,y,z); break;
	case (EIGENDIRECTIONS_LINEARINTERP_H) : computeMetricEigenDir(NULL, hessian,metric,size,x,y,z); break;
	case (ISOTROPIC_LINEARINTERP_H) : computeMetricIsoLinInterp(NULL, hessian,metric,size,x,y,z); break;
	}
	newSetOfMetrics[iMetric] = metric;
      }
      else{
	//find other metrics here
	SMetric3 metric;
	SPoint3 xyz(x,y,z), uvw;
	double initialTol = MElement::getTolerance();
	MElement::setTolerance(1.e-4);
	MElement *e = _octree->find(x, y, z, _dim);
	MElement::setTolerance(initialTol);
	if (e){
	  e->xyz2uvw(xyz,uvw);
	  SMetric3 m1 = setOfMetrics[iMetric][e->getVertex(0)];
	  SMetric3 m2 = setOfMetrics[iMetric][e->getVertex(1)];
	  SMetric3 m3 = setOfMetrics[iMetric][e->getVertex(2)];
	  if (_dim == 2)
	    metric =  interpolation(m1,m2,m3,uvw[0],uvw[1]);
	  else {
	    SMetric3 m4 = setOfMetrics[iMetric][e->getVertex(3)];
	    metric =  interpolation(m1,m2,m3,m4,uvw[0],uvw[1],uvw[2]);
	  }
	  newSetOfMetrics[iMetric] = metric;
	}
	else{
	  Msg::Warning("point %g %g %g not found, looking for nearest node",x,y,z);
	}
      }
    }
    //intersect metrics here
    metr = newSetOfMetrics[0];
    for (int i=1;i<nbMetrics;i++)
      metr = intersection_conserve_mostaniso(metr,newSetOfMetrics[i]);
  }
  //INTERPOLATE DISCRETE MESH METRIC
  else{
    SPoint3 xyz(x,y,z), uvw;
    double initialTol = MElement::getTolerance();
    MElement::setTolerance(1.e-4);
    MElement *e = _octree->find(x, y, z, _dim);
    MElement::setTolerance(initialTol);

    if (e){
      e->xyz2uvw(xyz,uvw);
      SMetric3 m1 = _nodalMetrics[e->getVertex(0)];
      SMetric3 m2 = _nodalMetrics[e->getVertex(1)];
      SMetric3 m3 = _nodalMetrics[e->getVertex(2)];
      if (_dim == 2)
	metr =  interpolation(m1,m2,m3,uvw[0],uvw[1]);
      else {
	SMetric3 m4 = _nodalMetrics[e->getVertex(3)];
	metr =  interpolation(m1,m2,m3,m4,uvw[0],uvw[1],uvw[2]);
      }

    }
    else{
      Msg::Warning("point %g %g %g not found, looking for nearest node",x,y,z);
      double minDist = 1.e100;
      for (nodalMetricTensor::iterator it = _nodalMetrics.begin(); it != _nodalMetrics.end(); it++) {
	const double dist = xyz.distance(it->first->point());
	if (dist <= minDist) {
	  minDist = dist;
	  metr = it->second;
	}
      }
    }
  }

}

double meshMetric::getLaplacian (MVertex *v)
{
  MVertex *vNew = _vertexMap[v->getNum()];
  std::map<MVertex*, SMetric3 >::const_iterator it = hessians.find(vNew);
  SMetric3 h = it->second;
  return h(0,0)+h(1,1)+h(2,2);
}

SVector3 meshMetric::getGradient (MVertex *v)
{
  MVertex *vNew = _vertexMap[v->getNum()];
  std::map<MVertex*,SVector3>::const_iterator it = grads.find(vNew);
  SVector3 gr = it->second;
  return gr;
}

/*void meshMetric::curvatureContributionToMetric (){
  std::map<MVertex*,SMetric3>::iterator it = _nodalMetrics.begin();
  std::map<MVertex*,double>::iterator its = _nodalSizes.begin();
  for (; it != _nodalMetrics.end(); ++it,++its){
  MVertex *v = it->first;
  if (v->onWhat()->dim() == 0){
  double l;
  SMetric3 curvMetric = max_edge_curvature_metric((GVertex*)v->onWhat(),l);
  it->second = intersection(it->second,curvMetric);
  its->second = std::min(l,its->second);
  }
  else if (v->onWhat()->dim() == 1){
  double u,l;
  v->getParameter(0,u);
  SMetric3 curvMetric = max_edge_curvature_metric((GEdge*)v->onWhat(),u,l);
  it->second = intersection(it->second,curvMetric);
  its->second = std::min(l,its->second);
  }
  }
  }*/

/*
  void meshMetric::smoothMetric (dgDofContainer *sol){
  return;
  dgGroupCollection *groups = sol->getGroups();
  std::set<MEdge,Less_Edge> edges;
  for (int i=0;i<groups->getNbElementGroups();i++){
  dgGroupOfElements *group = groups->getElementGroup(i);
  for (int j=0;j<group->getNbElements();j++){
  MElement *e = group->getElement(j);
  for (int k=0;k<e->getNumEdges();k++){
  edges.insert(e->getEdge(k));
  }
  }
  }

  std::set<MEdge,Less_Edge>::iterator it = edges.begin();
  double logRatio = log (CTX::instance()->mesh.smoothRatio);
  for ( ; it !=edges.end(); ++it){
  MEdge e = *it;
  std::map<MVertex*,SMetric3>::iterator it1 = _nodalMetrics.find(e.getVertex(0));
  std::map<MVertex*,SMetric3>::iterator it2 = _nodalMetrics.find(e.getVertex(1));
  SVector3 aij (e.getVertex(1)->x()-e.getVertex(0)->x(),
  e.getVertex(1)->y()-e.getVertex(0)->y(),
  e.getVertex(1)->z()-e.getVertex(0)->z());
  SMetric3 m1 = it1->second;
  double al1 = sqrt(dot(aij,m1,aij));
  SMetric3 m2 = it2->second;
  double al2 = sqrt(dot(aij,m2,aij));
  //    it1->second.print("m1");
  //    it2->second.print("m2");
  m1 *= 1./((1+logRatio*al1)*(1+logRatio*al1));
  m2 *= 1./((1+logRatio*al2)*(1+logRatio*al2));
  it1->second = intersection (it1->second,m2);
  it2->second = intersection (it2->second,m1);
  //    it1->second.print("m1 after");
  //    it2->second.print("m2 after");
  //    getchar();
  }
  }
*/

