// Gmsh - Copyright (C) 1997-2013 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@geuz.org>.
//
// Contributor(s):
//   Koen Hillewaert
//

#include "HighOrder.h"
#include "MLine.h"
#include "MTriangle.h"
#include "MQuadrangle.h"
#include "MTetrahedron.h"
#include "MHexahedron.h"
#include "MPrism.h"
#include "MPyramid.h"
#include "GmshMessage.h"
#include "OS.h"
#include "Numeric.h"
#include "Context.h"
#include "fullMatrix.h"
#include "BasisFactory.h"

// The aim here is to build a polynomial representation that consist
// in polynomial segments of equal length

static double mylength(GEdge *ge, int i, double *u)
{
  return ge->length(u[i], u[i+1], 10);
}

static void myresid(int N, GEdge *ge, double *u, fullVector<double> &r)
{
  double L[100];
  for (int i = 0; i < N - 1; i++) L[i] = mylength(ge, i, u);
  for (int i = 0; i < N - 2; i++) r(i) = L[i + 1] - L[i];
}

static bool computeEquidistantParameters(GEdge *ge, double u0, double uN, int N,
                                         double *u, double underRelax)
{
  const double PRECISION = 1.e-6;
  const int MAX_ITER = 50;
  const double eps = (uN - u0) * 1.e-5;

  // newton algorithm
  // N is the total number of points (3 for quadratic, 4 for cubic ...)
  // u[0] = u0;
  // u[N-1] = uN;
  // initialize as equidistant in parameter space
  u[0] = u0;
  double du = (uN - u0) / (N - 1);
  for (int i = 1; i < N; i++){
    u[i] = u[i - 1] + du;
  }

  // create the tangent matrix
  const int M = N - 2;
  fullMatrix<double> J(M, M);
  fullVector<double> DU(M);
  fullVector<double> R(M);
  fullVector<double> Rp(M);

  int iter = 1;

  while (iter < MAX_ITER){
    iter++;
    myresid(N, ge, u, R);

    for (int i = 0; i < M; i++){
      u[i + 1] += eps;
      myresid(N, ge, u, Rp);
      for (int j = 0; j < M; j++){
        J(i, j) = (Rp(j) - R(j)) / eps;
      }
      u[i+1] -= eps;
    }

    if (M == 1)
      DU(0) = R(0) / J(0, 0);
    else
      J.luSolve(R, DU);

    for (int i = 0; i < M; i++){
      u[i+1] -= underRelax*DU(i);
    }

    if (u[1] < u0) break;
    if (u[N - 2] > uN) break;

    double newt_norm = DU.norm();
    if (newt_norm < PRECISION) {
      return true;
    }
  }
  return false;
}

static bool computeEquidistantParameters(GFace *gf, double u0, double uN,
                                         double v0, double vN, int N,
                                         double *u, double *v)
{
  double t[100];
  // initialize the points by equal subdivision of geodesics
  u[0] = u0;
  v[0] = v0;
  t[0] = 0;
  for (int i = 1; i < N; i++){
    t[i] = (double)i / (N - 1);
    SPoint2 p = gf->geodesic(SPoint2(u0, v0), SPoint2(uN, vN), t[i]);
    u[i] = p.x();
    v[i] = p.y();
  }
  u[N] = uN;
  v[N] = vN;
  t[N] = 1.0;

  return true;
}

static void getEdgeVertices(GEdge *ge, MElement *ele, std::vector<MVertex*> &ve,
                            edgeContainer &edgeVertices, bool linear,
                            int nPts = 1)
{
  if(ge->geomType() == GEntity::DiscreteCurve ||
     ge->geomType() == GEntity::BoundaryLayerCurve)
    linear = true;

  for(int i = 0; i < ele->getNumEdges(); i++){
    MEdge edge = ele->getEdge(i);
    std::pair<MVertex*, MVertex*> p(edge.getMinVertex(), edge.getMaxVertex());
    if(edgeVertices.count(p)){
      if(edge.getVertex(0) == edge.getMinVertex())
        ve.insert(ve.end(), edgeVertices[p].begin(), edgeVertices[p].end());
      else
        ve.insert(ve.end(), edgeVertices[p].rbegin(), edgeVertices[p].rend());
    }
    else{
      MVertex *v0 = edge.getVertex(0), *v1 = edge.getVertex(1);
      std::vector<MVertex*> temp;
      double u0 = 0., u1 = 0., US[100];
      bool reparamOK = true;
      if(!linear) {
        reparamOK &= reparamMeshVertexOnEdge(v0, ge, u0);
        if(ge->periodic(0) && ge->getEndVertex()->getNumMeshVertices() > 0 &&
           v1 == ge->getEndVertex()->mesh_vertices[0])
          u1 = ge->parBounds(0).high();
        else
          reparamOK &= reparamMeshVertexOnEdge(v1, ge, u1);
        if(reparamOK){
          double relax = 1.;
          while (1){
            if(computeEquidistantParameters(ge, std::min(u0,u1), std::max(u0,u1),
                                            nPts + 2, US, relax))
              break;

            relax /= 2.0;
            if(relax < 1.e-2)
              break;
          }
          if(relax < 1.e-2)
            Msg::Warning
              ("Failed to compute equidistant parameters (relax = %g, value = %g) "
               "for edge %d-%d parametrized with %g %g on GEdge %d linear %d",
               relax, US[1], v0->getNum(), v1->getNum(),u0,u1,ge->tag(), linear);
        }
        else{
          Msg::Error("Cannot reparam a mesh Vertex in high order meshing");
        }
      }
      for(int j = 0; j < nPts; j++){
        const double t = (double)(j + 1)/(nPts + 1);
        double uc = (1. - t) * u0 + t * u1; // can be wrong, that's ok
        MVertex *v;
        if(linear || !reparamOK || uc < std::min(u0,u1) || uc > std::max(u0,u1)){
          if (!linear)
            Msg::Warning("We don't have a valid parameter on curve %d-%d",
                         v0->getNum(), v1->getNum());
          // we don't have a (valid) parameter on the curve
          SPoint3 pc = edge.interpolate(t);
          v = new MVertex(pc.x(), pc.y(), pc.z(), ge);
        }
        else {
          int count = u0<u1? j + 1 : nPts + 1  - (j + 1);
          GPoint pc = ge->point(US[count]);
          v = new MEdgeVertex(pc.x(), pc.y(), pc.z(), ge,US[count]);
        }
        temp.push_back(v);
        // this destroys the ordering of the mesh vertices on the edge
        ge->mesh_vertices.push_back(v);
        ve.push_back(v);
      }

      if(edge.getVertex(0) == edge.getMinVertex())
        edgeVertices[p].insert(edgeVertices[p].end(), temp.begin(), temp.end());
      else
        edgeVertices[p].insert(edgeVertices[p].end(), temp.rbegin(), temp.rend());
    }
  }
}

static void getEdgeVertices(GFace *gf, MElement *ele, std::vector<MVertex*> &ve,
                            edgeContainer &edgeVertices, bool linear,
                            int nPts = 1)
{
  if(gf->geomType() == GEntity::DiscreteSurface ||
     gf->geomType() == GEntity::BoundaryLayerSurface)
    linear = true;

  for(int i = 0; i < ele->getNumEdges(); i++){
    MEdge edge = ele->getEdge(i);
    std::pair<MVertex*, MVertex*> p(edge.getMinVertex(), edge.getMaxVertex());
    if(edgeVertices.count(p)){
      if(edge.getVertex(0) == edge.getMinVertex())
        ve.insert(ve.end(), edgeVertices[p].begin(), edgeVertices[p].end());
      else
        ve.insert(ve.end(), edgeVertices[p].rbegin(), edgeVertices[p].rend());
    }
    else{
      MVertex *v0 = edge.getVertex(0), *v1 = edge.getVertex(1);
      SPoint2 p0, p1;
      double US[100], VS[100];
      bool reparamOK = true;
      if(!linear){
        reparamOK = reparamMeshEdgeOnFace(v0, v1, gf, p0, p1);
        if(reparamOK)
          computeEquidistantParameters(gf, p0[0], p1[0], p0[1], p1[1], nPts + 2,
                                       US, VS);
      }
      std::vector<MVertex*> temp;
      for(int j = 0; j < nPts; j++){
        const double t = (double)(j + 1) / (nPts + 1);
        MVertex *v;
        if(linear || !reparamOK){
          // we don't have (valid) parameters on the surface
          SPoint3 pc = edge.interpolate(t);
          v = new MVertex(pc.x(), pc.y(), pc.z(), gf);
        }
        else{
          GPoint pc = gf->point(US[j + 1], VS[j + 1]);
          v = new MFaceVertex(pc.x(), pc.y(), pc.z(), gf, US[j + 1], VS[j + 1]);
        }
        temp.push_back(v);
        gf->mesh_vertices.push_back(v);
        ve.push_back(v);
      }
      if(edge.getVertex(0) == edge.getMinVertex())
        edgeVertices[p].insert(edgeVertices[p].end(), temp.begin(), temp.end());
      else
        edgeVertices[p].insert(edgeVertices[p].end(), temp.rbegin(), temp.rend());
    }
  }
}

static void getEdgeVertices(GRegion *gr, MElement *ele, std::vector<MVertex*> &ve,
                            edgeContainer &edgeVertices, bool linear,
                            int nPts = 1)
{
  for(int i = 0; i < ele->getNumEdges(); i++){
    MEdge edge = ele->getEdge(i);
    std::pair<MVertex*, MVertex*> p(edge.getMinVertex(), edge.getMaxVertex());
    if(edgeVertices.count(p)){
      if(edge.getVertex(0) == edge.getMinVertex())
        ve.insert(ve.end(), edgeVertices[p].begin(), edgeVertices[p].end());
      else
        ve.insert(ve.end(), edgeVertices[p].rbegin(), edgeVertices[p].rend());
    }
    else{
      std::vector<MVertex*> temp;
      for(int j = 0; j < nPts; j++){
        double t = (double)(j + 1) / (nPts + 1);
        SPoint3 pc = edge.interpolate(t);
        MVertex *v = new MVertex(pc.x(), pc.y(), pc.z(), gr);
        temp.push_back(v);
        gr->mesh_vertices.push_back(v);
        ve.push_back(v);
      }
      if(edge.getVertex(0) == edge.getMinVertex())
        edgeVertices[p].insert(edgeVertices[p].end(), temp.begin(), temp.end());
      else
        edgeVertices[p].insert(edgeVertices[p].end(), temp.rbegin(), temp.rend());
    }
  }
}

static void getFaceVertices(GFace *gf, MElement *incomplete, MElement *ele,
                            std::vector<MVertex*> &vf, faceContainer &faceVertices,
                            bool linear, int nPts = 1)
{
  if(gf->geomType() == GEntity::DiscreteSurface ||
     gf->geomType() == GEntity::BoundaryLayerSurface)
    linear = true;

  for(int i = 0; i < ele->getNumFaces(); i++){
    MFace face = ele->getFace(i);
    faceContainer::iterator fIter = faceVertices.find(face);
    if(fIter != faceVertices.end()){
      vf.insert(vf.end(), fIter->second.begin(), fIter->second.end());
    }
    else{
      std::vector<MVertex*> &vtcs = faceVertices[face];
      SPoint2 pts[1000];
      bool reparamOK = true;
      if(!linear){
        for(int k = 0; k < incomplete->getNumVertices(); k++)
          reparamOK &= reparamMeshVertexOnFace(incomplete->getVertex(k), gf, pts[k]);
      }
      int start = face.getNumVertices() * (nPts + 1);
      const fullMatrix<double> &points = ele->getFunctionSpace(nPts + 1)->points;
      for(int k = start; k < points.size1(); k++){
        MVertex *v;
        const double t1 = points(k, 0);
        const double t2 = points(k, 1);
        if(linear){
          SPoint3 pc = face.interpolate(t1, t2);
          v = new MVertex(pc.x(), pc.y(), pc.z(), gf);
        }
        else{
          double X(0), Y(0), Z(0), GUESS[2] = {0, 0};
          double sf[1256];
          incomplete->getShapeFunctions(t1, t2, 0, sf);
          for (int j = 0; j < incomplete->getNumShapeFunctions(); j++){
            MVertex *vt = incomplete->getShapeFunctionNode(j);
            X += sf[j] * vt->x();
            Y += sf[j] * vt->y();
            Z += sf[j] * vt->z();
            if (reparamOK){
              GUESS[0] += sf[j] * pts[j][0];
              GUESS[1] += sf[j] * pts[j][1];
            }
          }
          if(reparamOK){
            GPoint gp = gf->point(SPoint2(GUESS[0], GUESS[1]));
            // closest point is not necessary (slow and for high quality HO
            // meshes it should be optimized anyway afterwards + closest point
            // is still buggy (e.g. BFGS for a plane Ruled Surface)
            // GPoint gp = gf->closestPoint(SPoint3(X, Y, Z), GUESS);
            if (gp.g()){
              v = new MFaceVertex(gp.x(), gp.y(), gp.z(), gf, gp.u(), gp.v());
            }
            else{
              v = new MVertex(X, Y, Z, gf);
            }
          }
          else{
            v = new MVertex(X, Y, Z, gf);
          }
        }
        // should be expensive -> induces a new search each time
        vtcs.push_back(v);
        gf->mesh_vertices.push_back(v);
        vf.push_back(v);
      }
    }
  }
}

static void reorientTrianglePoints(std::vector<MVertex*> &vtcs, int orientation,
                                   bool swap)
{
  int nbPts = vtcs.size();
  if (nbPts <= 1) return;
  std::vector<MVertex*> tmp(nbPts);
  int interiorOrder = (int)((sqrt(1. + 8. * nbPts) - 3) / 2);
  int pos = 0;
  for (int o = interiorOrder; o>0; o-=3) {
    if (swap) {
      tmp[pos] = vtcs[pos];
      tmp[pos+1] = vtcs[pos+2];
      tmp[pos+2] = vtcs[pos+1];
      for (int i = 0; i < 3*(o-1); i++)
        tmp[pos+3+i] = vtcs[pos+3*o-i-1];
    }
    else {
      for (int i = 0; i < 3*o; i++)
        tmp[pos+i] = vtcs[pos+i];
    }
    for (int i = 0; i < 3; i++) {
      int ri = (i+orientation)%3;
      vtcs[pos+ri] = tmp[pos+i];
      for (int j = 0; j < o-1; j++)
        vtcs[pos+3+(o-1)*ri+j] = tmp[pos+3+(o-1)*i+j];
    }
    pos += 3*o;
  }
}

static void reorientQuadPoints(std::vector<MVertex*> &vtcs, int orientation,
                               bool swap, int order)
{
  int nbPts = vtcs.size();
  if (nbPts <= 1) return;
  std::vector<MVertex*> tmp(nbPts);

  int start = 0;
  while (1){
    // CORNERS
    int index = 0;
    if (order == 0){
      start++;
    }
    else{
      int i1(0),i2(0),i3(0),i4(0);
      if (!swap){
        if      (orientation == 0){ i1 = 0; i2 = 1; i3 = 2; i4 = 3; }
        else if (orientation == 1){ i1 = 3; i2 = 0; i3 = 1; i4 = 2; }
        else if (orientation == 2){ i1 = 2; i2 = 3; i3 = 0; i4 = 1; }
        else if (orientation == 3){ i1 = 1; i2 = 2; i3 = 3; i4 = 0; }
      }
      else{
        if      (orientation == 0){ i1 = 0; i2 = 3; i3 = 2; i4 = 1; }
        else if (orientation == 3){ i1 = 3; i2 = 2; i3 = 1; i4 = 0; }
        else if (orientation == 2){ i1 = 2; i2 = 1; i3 = 0; i4 = 3; }
        else if (orientation == 1){ i1 = 1; i2 = 0; i3 = 3; i4 = 2; }
      }

      int indices[4] = {i1, i2, i3, i4};
      for (int i = 0; i < 4; i++) tmp[i] = vtcs[start + indices[i]];
      for (int i = 0; i < 4; i++) vtcs[start + i] = tmp[i];

      // EDGES
      for (int iEdge=0;iEdge<4;iEdge++){
        int p1 = indices[iEdge];
        int p2 = indices[(iEdge+1)%4];
        int nbP = order-1;
        if      (p1 == 0 && p2 == 1){
          for (int i = 4+0*nbP; i < 4+1*nbP; i++) tmp[index++] = vtcs[i];
        }
        else if (p1 == 1 && p2 == 2){
          for (int i = 4+1*nbP; i< 4+2*nbP; i++) tmp[index++] = vtcs[i];
        }
        else if (p1 == 2 && p2 == 3){
          for (int i = 4+2*nbP; i< 4+3*nbP; i++) tmp[index++] = vtcs[i];
        }
        else if (p1 == 3 && p2 == 0){
          for (int i = 4+3*nbP; i< 4+4*nbP; i++) tmp[index++] = vtcs[i];
        }
        else if (p1 == 1 && p2 == 0){
          for (int i = 4+1*nbP-1; i >= 4+0*nbP; i--) tmp[index++] = vtcs[i];
        }
        else if (p1 == 2 && p2 == 1){
          for (int i = 4+2*nbP-1; i >= 4+1*nbP; i--) tmp[index++] = vtcs[i];
        }
        else if (p1 == 3 && p2 == 2){
          for (int i = 4+3*nbP-1; i >= 4+2*nbP; i--) tmp[index++] = vtcs[i];
        }
        else if (p1 == 0 && p2 == 3){
          for (int i = 4+4*nbP-1; i >= 4+3*nbP; i--) tmp[index++] = vtcs[i];
        }
        else Msg::Error("Something wrong in reorientQuadPoints");
      }
      for (int i = 0; i < index; i++)vtcs[start+4+i] = tmp[i];
      start += (4 + index);
    }

    order -= 2;
    if (start >= (int) vtcs.size()) break;
  }
}

static int getNewFacePoints(int numPrimaryFacePoints, int nPts, fullMatrix<double> &points)
{
  int start = 0;

  int tag = 0;
  switch(numPrimaryFacePoints) {
    case 3:
      if (nPts < 2) break;

      tag = ElementType::getTag(TYPE_TRI, nPts+1);
      if (!tag) {
        Msg::Error("getFaceVertices not implemented for order %i", nPts +1);
        break;
      }
      points = BasisFactory::getNodalBasis(tag)->points;
      start = 3 * (1 + nPts);
      break;

    case 4:
      if (nPts < 1) break;

      tag = ElementType::getTag(TYPE_QUA, nPts+1);
      if (!tag) {
        Msg::Error("getFaceVertices not implemented for order %i", nPts +1);
        break;
      }
      points = BasisFactory::getNodalBasis(tag)->points;
      start = 4 * (1 + nPts);
      break;

    default:
      Msg::Error("getFaceVertices not implemented for %d-node faces", numPrimaryFacePoints);
      break;
  }
  return start;
}

static void getFaceVertices(GRegion *gr, MElement *ele, std::vector<MVertex*> &vf,
                            faceContainer &faceVertices, edgeContainer &edgeVertices,
                            bool linear, int nPts = 1)
{
  for(int i = 0; i < ele->getNumFaces(); i++){
    MFace face = ele->getFace(i);
    faceContainer::iterator fIter = faceVertices.find(face);
    if(fIter != faceVertices.end()) {
      std::vector<MVertex*> vtcs = fIter->second;
      if(face.getNumVertices() == 3 && nPts > 1){ // tri face
        int orientation;
        bool swap;
        if (fIter->first.computeCorrespondence(face, orientation, swap))
          reorientTrianglePoints(vtcs, orientation, swap);
        else
          Msg::Error("Error in face lookup for recuperation of high order face nodes");
      }
      else if(face.getNumVertices() == 4){ // quad face
        int orientation;
        bool swap;
        if (fIter->first.computeCorrespondence(face, orientation, swap)){
          reorientQuadPoints(vtcs, orientation, swap, nPts-1);
        }
        else
          Msg::Error("Error in face lookup for recuperation of high order face nodes");
      }
      vf.insert(vf.end(), vtcs.begin(), vtcs.end());
    }
    else{
      std::vector<MVertex*> &vtcs = faceVertices[face];
      fullMatrix<double> points;
      int start = getNewFacePoints(face.getNumVertices(), nPts, points);
      if(face.getNumVertices() == 3 && nPts > 1){ // tri face
        // construct incomplete element to take into account curved
        // edges on surface boundaries
        std::vector<MVertex*> hoEdgeNodes;
        for (int i = 0; i < 3; i++) {
          MVertex* v0 = face.getVertex(i);
          MVertex* v1 = face.getVertex((i + 1) % 3);
          edgeContainer::iterator eIter = edgeVertices.find
            (std::pair<MVertex*,MVertex*>(std::min(v0, v1), std::max(v0, v1)));
          if (eIter == edgeVertices.end())
            Msg::Error("Could not find ho nodes for an edge");
          if (v0 == eIter->first.first)
            hoEdgeNodes.insert(hoEdgeNodes.end(), eIter->second.begin(),
                               eIter->second.end());
          else
            hoEdgeNodes.insert(hoEdgeNodes.end(), eIter->second.rbegin(),
                               eIter->second.rend());
        }
        MTriangleN incomplete(face.getVertex(0), face.getVertex(1),
                              face.getVertex(2), hoEdgeNodes, nPts + 1);
        for (int k = start; k < points.size1(); k++) {
          double t1 = points(k, 0);
          double t2 = points(k, 1);
          SPoint3 pos;
          incomplete.pnt(t1, t2, 0, pos);
          MVertex* v = new MVertex(pos.x(), pos.y(), pos.z(), gr);
          vtcs.push_back(v);
          gr->mesh_vertices.push_back(v);
          vf.push_back(v);
        }
      }
      else if(face.getNumVertices() == 4){ // quad face
        for (int k = start; k < points.size1(); k++) {
          double t1 = points(k, 0);
          double t2 = points(k, 1);
          SPoint3 pc = face.interpolate(t1, t2);
          MVertex *v = new MVertex(pc.x(), pc.y(), pc.z(), gr);
          vtcs.push_back(v);
          gr->mesh_vertices.push_back(v);
          vf.push_back(v);
        }
      }
    }
  }
}

static void getRegionVertices(GRegion *gr, MElement *incomplete, MElement *ele,
                              std::vector<MVertex*> &vr, bool linear, int nPts = 1)
{
  fullMatrix<double> points;
  int start = 0;

  switch (incomplete->getType()){
  case TYPE_TET :
    switch (nPts){
    case 0: return;
    case 1: return;
    case 2: points = BasisFactory::getNodalBasis(MSH_TET_20)->points; break;
    case 3: points = BasisFactory::getNodalBasis(MSH_TET_35)->points; break;
    case 4: points = BasisFactory::getNodalBasis(MSH_TET_56)->points; break;
    case 5: points = BasisFactory::getNodalBasis(MSH_TET_84)->points; break;
    case 6: points = BasisFactory::getNodalBasis(MSH_TET_120)->points; break;
    case 7: points = BasisFactory::getNodalBasis(MSH_TET_165)->points; break;
    case 8: points = BasisFactory::getNodalBasis(MSH_TET_220)->points; break;
    case 9: points = BasisFactory::getNodalBasis(MSH_TET_286)->points; break;
    default:
      Msg::Error("getRegionVertices not implemented for order %i", nPts+1);
      break;
    }
    start = ((nPts+2)*(nPts+3)*(nPts+4)-(nPts-2)*(nPts-1)*(nPts))/6;  // = 4+6*(p-1)+4*(p-2)*(p-1)/2 = 4*(p+1)*(p+2)/2-6*(p-1)-2*4 = 2*p*p+2
    break;
  case TYPE_HEX :
    switch (nPts){
    case 0: return;
    case 1: points = BasisFactory::getNodalBasis(MSH_HEX_27)->points; break;
    case 2: points = BasisFactory::getNodalBasis(MSH_HEX_64)->points; break;
    case 3: points = BasisFactory::getNodalBasis(MSH_HEX_125)->points; break;
    case 4: points = BasisFactory::getNodalBasis(MSH_HEX_216)->points; break;
    case 5: points = BasisFactory::getNodalBasis(MSH_HEX_343)->points; break;
    case 6: points = BasisFactory::getNodalBasis(MSH_HEX_512)->points; break;
    case 7: points = BasisFactory::getNodalBasis(MSH_HEX_729)->points; break;
    case 8: points = BasisFactory::getNodalBasis(MSH_HEX_1000)->points; break;
    default :
      Msg::Error("getRegionVertices not implemented for order %i", nPts+1);
      break;
    }
    start = (nPts+2)*(nPts+2)*(nPts+2) - (nPts)*(nPts)*(nPts) ; // = 6*(p-1)*(p-1)+12*(p-1)+8 = 6*(p+1)*(p+1)-12*(p-1)-2*8 = 6*p*p+2
    break;
  case TYPE_PRI :
    switch (nPts){
    case 0: return;
    case 1: return;
    case 2: points = BasisFactory::getNodalBasis(MSH_PRI_40)->points; break;
    case 3: points = BasisFactory::getNodalBasis(MSH_PRI_75)->points; break;
    case 4: points = BasisFactory::getNodalBasis(MSH_PRI_126)->points; break;
    case 5: points = BasisFactory::getNodalBasis(MSH_PRI_196)->points; break;
    case 6: points = BasisFactory::getNodalBasis(MSH_PRI_288)->points; break;
    case 7: points = BasisFactory::getNodalBasis(MSH_PRI_405)->points; break;
    case 8: points = BasisFactory::getNodalBasis(MSH_PRI_550)->points; break;
    default:
      Msg::Error("getRegionVertices not implemented for order %i", nPts+1);
      break;
    }
    start = 4*(nPts+1)*(nPts+1)+2;  // = 4*p*p+2 = 6+9*(p-1)+2*(p-2)*(p-1)/2+3*(p-1)*(p-1) = 2*(p+1)*(p+2)/2+3*(p+1)*(p+1)-9*(p-1)-2*6
    break;
  case TYPE_PYR:
    switch (nPts){
    case 0:
    case 1: return;
    case 2: points = BasisFactory::getNodalBasis(MSH_PYR_30)->points; break;
    case 3: points = BasisFactory::getNodalBasis(MSH_PYR_55)->points; break;
    case 4: points = BasisFactory::getNodalBasis(MSH_PYR_91)->points; break;
    case 5: points = BasisFactory::getNodalBasis(MSH_PYR_140)->points; break;
    case 6: points = BasisFactory::getNodalBasis(MSH_PYR_204)->points; break;
    case 7: points = BasisFactory::getNodalBasis(MSH_PYR_285)->points; break;
    case 8: points = BasisFactory::getNodalBasis(MSH_PYR_385)->points; break;
    default :
      Msg::Error("getRegionVertices not implemented for order %i", nPts+1);
      break;
    }
    start = ( nPts+2) * ( (nPts+2) + 1) * (2*(nPts+2) + 1) / 6  -
      (nPts-1) * ( (nPts-1) + 1) * (2*(nPts-1) + 1) / 6;
    break;

  }

  for(int k = start; k < points.size1(); k++){
    MVertex *v;
    const double t1 = points(k, 0);
    const double t2 = points(k, 1);
    const double t3 = points(k, 2);
    SPoint3 pos;
    incomplete->pnt(t1, t2, t3, pos);
    v = new MVertex(pos.x(), pos.y(), pos.z(), gr);
    gr->mesh_vertices.push_back(v);
    vr.push_back(v);
  }
}

static void setHighOrder(GEdge *ge, edgeContainer &edgeVertices, bool linear,
                         int nbPts = 1)
{
  std::vector<MLine*> lines2;
  for(unsigned int i = 0; i < ge->lines.size(); i++){
    MLine *l = ge->lines[i];
    std::vector<MVertex*> ve;
    getEdgeVertices(ge, l, ve, edgeVertices, linear, nbPts);
    if(nbPts == 1)
      lines2.push_back(new MLine3(l->getVertex(0), l->getVertex(1), ve[0], l->getPartition()));
    else
      lines2.push_back(new MLineN(l->getVertex(0), l->getVertex(1), ve, l->getPartition()));
    delete l;
  }
  ge->lines = lines2;
  ge->deleteVertexArrays();
}

static MTriangle *setHighOrder(MTriangle *t, GFace *gf,
                               edgeContainer &edgeVertices,
                               faceContainer &faceVertices,
                               bool linear, bool incomplete, int nPts)
{
  std::vector<MVertex*> ve, vf;
  getEdgeVertices(gf, t, ve, edgeVertices, linear, nPts);
  if(nPts == 1){
    return new MTriangle6(t->getVertex(0), t->getVertex(1), t->getVertex(2),
                          ve[0], ve[1], ve[2], 0, t->getPartition());
  }
  else{
    if(!incomplete){
      MTriangleN incpl(t->getVertex(0), t->getVertex(1), t->getVertex(2),
                       ve, nPts + 1, 0, t->getPartition());
      getFaceVertices(gf, &incpl, t, vf, faceVertices, linear, nPts);
      ve.insert(ve.end(), vf.begin(), vf.end());
    }
    return new MTriangleN(t->getVertex(0), t->getVertex(1), t->getVertex(2),
                          ve, nPts + 1, 0, t->getPartition());
  }
}

static MQuadrangle *setHighOrder(MQuadrangle *q, GFace *gf,
                                 edgeContainer &edgeVertices,
                                 faceContainer &faceVertices,
                                 bool linear, bool incomplete, int nPts)
{
  std::vector<MVertex*> ve, vf;
  getEdgeVertices(gf, q, ve, edgeVertices, linear, nPts);
  if(incomplete){
    if(nPts == 1){
      return new MQuadrangle8(q->getVertex(0), q->getVertex(1), q->getVertex(2),
                              q->getVertex(3), ve[0], ve[1], ve[2], ve[3],
                              0, q->getPartition());
    }
    else{
      return new MQuadrangleN(q->getVertex(0), q->getVertex(1), q->getVertex(2),
                              q->getVertex(3), ve, nPts + 1,
                              0, q->getPartition());
    }
  }
  else {
    MQuadrangleN incpl(q->getVertex(0), q->getVertex(1), q->getVertex(2),
                       q->getVertex(3), ve, nPts + 1, 0, q->getPartition());
    getFaceVertices(gf, &incpl, q, vf, faceVertices, linear, nPts);
    ve.insert(ve.end(), vf.begin(), vf.end());
    if(nPts == 1){
      return new MQuadrangle9(q->getVertex(0), q->getVertex(1), q->getVertex(2),
                              q->getVertex(3), ve[0], ve[1], ve[2], ve[3], vf[0],
                              0, q->getPartition());
    }
    else{
      return new MQuadrangleN(q->getVertex(0), q->getVertex(1), q->getVertex(2),
                              q->getVertex(3), ve, nPts + 1,
                              0, q->getPartition());
    }
  }
}

static void setHighOrder(GFace *gf, edgeContainer &edgeVertices,
                         faceContainer &faceVertices, bool linear, bool incomplete,
                         int nPts = 1)
{
  std::vector<MTriangle*> triangles2;
  for(unsigned int i = 0; i < gf->triangles.size(); i++){
    MTriangle *t = gf->triangles[i];
    MTriangle *tNew = setHighOrder(t, gf, edgeVertices, faceVertices, linear,
                                   incomplete, nPts);
    triangles2.push_back(tNew);
    delete t;
  }
  gf->triangles = triangles2;
  std::vector<MQuadrangle*> quadrangles2;
  for(unsigned int i = 0; i < gf->quadrangles.size(); i++){
    MQuadrangle *q = gf->quadrangles[i];
    MQuadrangle *qNew = setHighOrder(q, gf, edgeVertices, faceVertices, linear,
                                     incomplete, nPts);
    quadrangles2.push_back(qNew);
    delete q;
  }
  gf->quadrangles = quadrangles2;
  gf->deleteVertexArrays();
}

static MTetrahedron *setHighOrder(MTetrahedron *t, GRegion *gr,
                                  edgeContainer &edgeVertices,
                                  faceContainer &faceVertices,
                                  bool linear, bool incomplete, int nPts)
{
  std::vector<MVertex*> ve, vf, vr;
  getEdgeVertices(gr, t, ve, edgeVertices, linear, nPts);
  if(nPts == 1){
    return new MTetrahedron10(t->getVertex(0), t->getVertex(1), t->getVertex(2),
                              t->getVertex(3), ve[0], ve[1], ve[2], ve[3], ve[4], ve[5],
                              0, t->getPartition());
  }
  else{
    if(!incomplete){
      // create serendipity element to place internal vertices (we used to
      // saturate face vertices also, but the corresponding function spaces do
      // not exist anymore, and there is no theoretical justification for doing
      // it either way)
      MTetrahedronN incpl(t->getVertex(0), t->getVertex(1), t->getVertex(2),
                          t->getVertex(3), ve, nPts + 1, 0, t->getPartition());
      getFaceVertices(gr, t, vf, faceVertices, edgeVertices, linear, nPts);
      ve.insert(ve.end(), vf.begin(), vf.end());
      getRegionVertices(gr, &incpl, t, vr, linear, nPts);
      ve.insert(ve.end(), vr.begin(), vr.end());
    }
    return new MTetrahedronN(t->getVertex(0), t->getVertex(1),
                             t->getVertex(2), t->getVertex(3), ve, nPts + 1,
                             0, t->getPartition());
  }
}

static MHexahedron *setHighOrder(MHexahedron *h, GRegion *gr,
                                 edgeContainer &edgeVertices,
                                 faceContainer &faceVertices,
                                 bool linear, bool incomplete, int nPts)
{
  std::vector<MVertex*> ve, vf, vr;
  getEdgeVertices(gr, h, ve, edgeVertices, linear, nPts);
  if(incomplete){
    if(nPts == 1){
      return new MHexahedron20(h->getVertex(0), h->getVertex(1), h->getVertex(2),
                               h->getVertex(3), h->getVertex(4), h->getVertex(5),
                               h->getVertex(6), h->getVertex(7), ve[0], ve[1], ve[2],
                               ve[3], ve[4], ve[5], ve[6], ve[7], ve[8], ve[9], ve[10],
                               ve[11], 0, h->getPartition());
    }
    else{
      return new MHexahedronN(h->getVertex(0), h->getVertex(1), h->getVertex(2),
                              h->getVertex(3), h->getVertex(4), h->getVertex(5),
                              h->getVertex(6), h->getVertex(7), ve, nPts + 1, 0,
                              h->getPartition());
    }
  }
  else{
    if(nPts == 1){
      getFaceVertices(gr, h, vf, faceVertices, edgeVertices, linear, nPts);
      SPoint3 pc = h->barycenter();
      MVertex *v = new MVertex(pc.x(), pc.y(), pc.z(), gr);
      gr->mesh_vertices.push_back(v);
      return new MHexahedron27(h->getVertex(0), h->getVertex(1), h->getVertex(2),
                               h->getVertex(3), h->getVertex(4), h->getVertex(5),
                               h->getVertex(6), h->getVertex(7), ve[0], ve[1], ve[2],
                               ve[3], ve[4], ve[5], ve[6], ve[7], ve[8], ve[9], ve[10],
                               ve[11], vf[0], vf[1], vf[2], vf[3], vf[4], vf[5], v,
                               0, h->getPartition());
    }
    else {
      // create serendipity element to place internal vertices (we used to
      // saturate face vertices also, but the corresponding function spaces do
      // not exist anymore, and there is no theoretical justification for doing
      // it either way)
      MHexahedronN incpl(h->getVertex(0), h->getVertex(1), h->getVertex(2),
                         h->getVertex(3), h->getVertex(4), h->getVertex(5),
                         h->getVertex(6), h->getVertex(7), ve, nPts + 1, 0,
                         h->getPartition());
      getFaceVertices(gr, h, vf, faceVertices, edgeVertices, linear, nPts);
      ve.insert(ve.end(), vf.begin(), vf.end());
      getRegionVertices(gr, &incpl, h, vr, linear, nPts);
      ve.insert(ve.end(), vr.begin(), vr.end());
      return new MHexahedronN(h->getVertex(0), h->getVertex(1), h->getVertex(2),
                              h->getVertex(3), h->getVertex(4), h->getVertex(5),
                              h->getVertex(6), h->getVertex(7), ve, nPts + 1,
                              0, h->getPartition());
    }
  }
}

static MPrism *setHighOrder(MPrism *p, GRegion *gr,
                            edgeContainer &edgeVertices,
                            faceContainer &faceVertices,
                            bool linear, bool incomplete, int nPts)
{
  std::vector<MVertex*> ve, vf, vr;
  getEdgeVertices(gr, p, ve, edgeVertices, linear, nPts);
  if(incomplete){
    if(nPts == 1){
      return new MPrism15(p->getVertex(0), p->getVertex(1), p->getVertex(2),
                          p->getVertex(3), p->getVertex(4), p->getVertex(5),
                          ve[0], ve[1], ve[2], ve[3], ve[4], ve[5], ve[6], ve[7], ve[8],
                          0, p->getPartition());
    }
    else{
      return new MPrismN(p->getVertex(0), p->getVertex(1), p->getVertex(2),
                         p->getVertex(3), p->getVertex(4), p->getVertex(5),
                         ve, nPts + 1, 0, p->getPartition());
    }
  }
  else{
    if (nPts == 1) {
      getFaceVertices(gr, p, vf, faceVertices, edgeVertices, linear, nPts);
      return new MPrism18(p->getVertex(0), p->getVertex(1), p->getVertex(2),
                          p->getVertex(3), p->getVertex(4), p->getVertex(5),
                          ve[0], ve[1], ve[2], ve[3], ve[4], ve[5], ve[6], ve[7], ve[8],
                          vf[0], vf[1], vf[2],
                          0, p->getPartition());
    }
    else {
      // create serendipity element to place internal vertices (we used to
      // saturate face vertices also, but the corresponding function spaces do
      // not exist anymore, and there is no theoretical justification for doing
      // it either way)
      MPrismN incpl(p->getVertex(0), p->getVertex(1), p->getVertex(2),
                    p->getVertex(3), p->getVertex(4), p->getVertex(5),
                    ve, nPts + 1, 0, p->getPartition());
      getFaceVertices(gr, p, vf, faceVertices, edgeVertices, linear, nPts);
      ve.insert(ve.end(), vf.begin(), vf.end());
      getRegionVertices(gr, &incpl, p, vr, linear, nPts);
      ve.insert(ve.end(), vr.begin(), vr.end());
      return new MPrismN(p->getVertex(0), p->getVertex(1), p->getVertex(2),
                         p->getVertex(3), p->getVertex(4), p->getVertex(5),
                         ve, nPts + 1, 0, p->getPartition());
    }
  }
}

static MPyramid *setHighOrder(MPyramid *p, GRegion *gr,
                              edgeContainer &edgeVertices,
                              faceContainer &faceVertices,
                              bool linear, bool incomplete, int nPts)
{
  std::vector<MVertex*> ve, vf, vr;
  getEdgeVertices(gr, p, ve, edgeVertices, linear, nPts);
  getFaceVertices(gr, p, vf, faceVertices, edgeVertices, linear, nPts);
  ve.insert(ve.end(), vf.begin(), vf.end());
  vr.reserve((nPts-1)*(nPts)*(2*(nPts-1)+1)/6);
  int verts_lvl3[12] = {37,40,38,43,46,44,49,52,50,55,58,56};
  int verts_lvl2[8];
  if (nPts == 4) {
    verts_lvl2[0] = 42; verts_lvl2[1] = 41;
    verts_lvl2[2] = 48; verts_lvl2[3] = 47;
    verts_lvl2[4] = 54; verts_lvl2[5] = 53;
    verts_lvl2[6] = 60; verts_lvl2[7] = 59;
  }
  else {
    verts_lvl2[0] = 29; verts_lvl2[1] = 30;
    verts_lvl2[2] = 35; verts_lvl2[3] = 36;
    verts_lvl2[4] = 38; verts_lvl2[5] = 39;
    verts_lvl2[6] = 32; verts_lvl2[7] = 33;
  }
  int verts_lvl1[4];
  switch(nPts) {
  case(4):
    verts_lvl1[0] = 39;
    verts_lvl1[1] = 45;
    verts_lvl1[2] = 51;
    verts_lvl1[3] = 57;
    break;
  case(3):
    verts_lvl1[0] = 31;
    verts_lvl1[1] = 37;
    verts_lvl1[2] = 40;
    verts_lvl1[3] = 34;
    break;
  case(2):
    verts_lvl1[0] = 21;
    verts_lvl1[1] = 23;
    verts_lvl1[2] = 24;
    verts_lvl1[3] = 22;
    break;
  }
  for (int q = 0; q < nPts - 1; q++) {
    std::vector<MVertex*> vq, veq;
    vq.push_back(ve[2*nPts + q]);
    vq.push_back(ve[4*nPts + q]);
    vq.push_back(ve[6*nPts + q]);
    vq.push_back(ve[7*nPts + q]);

    if (nPts-q == 4)
      for (int f = 0; f < 12; f++)
        veq.push_back(ve[verts_lvl3[f]-5]);
    else if (nPts-q == 3)
      for (int f = 0; f < 8; f++)
        veq.push_back(ve[verts_lvl2[f]-5]);
    else if (nPts-q == 2)
      for (int f = 0; f < 4; f++)
        veq.push_back(ve[verts_lvl1[f]-5]);

    if (nPts-q == 2) {
      MQuadrangle8 incpl2(vq[0], vq[1], vq[2], vq[3],
                          veq[0], veq[1], veq[2], veq[3]);
      SPoint3 pointz;
      incpl2.pnt(0,0,0,pointz);
      MVertex *v = new MVertex(pointz.x(), pointz.y(), pointz.z(), gr);

      gr->mesh_vertices.push_back(v);
      std::vector<MVertex*>::iterator cursor = vr.begin();
      cursor += nPts == 2 ? 0 : 4;
      vr.insert(cursor, v);
    }
    else if (nPts-q == 3) {
      MQuadrangleN incpl2(vq[0], vq[1], vq[2], vq[3], veq, 3);
      int offsets[4] = {nPts == 4 ? 7 : 0,
                        nPts == 4 ? 9 : 1,
                        nPts == 4 ? 11 : 2,
                        nPts == 4 ? 12 : 3};
      double quad_v [4][2] = {{-1.0/3.0, -1.0/3.0},
                              { 1.0/3.0, -1.0/3.0},
                              { 1.0/3.0,  1.0/3.0},
                              {-1.0/3.0,  1.0/3.0}};
      SPoint3 pointz;
      for (int k = 0; k<4; k++) {
        incpl2.pnt(quad_v[k][0], quad_v[k][1], 0, pointz);
        MVertex *v = new MVertex(pointz.x(), pointz.y(), pointz.z(), gr);
        gr->mesh_vertices.push_back(v);
        std::vector<MVertex*>::iterator cursor = vr.begin();
        cursor += offsets[k];
        vr.insert(cursor, v);
      }
    }
    else if (nPts-q == 4) {
      MQuadrangleN incpl2(vq[0], vq[1], vq[2], vq[3], veq, 4);
      int offsets[9] = {0, 1, 2, 3, 5, 8, 10, 6, 13};
      double quad_v [9][2] = {
        { -0.5, -0.5},
        {  0.5, -0.5},
        {  0.5,  0.5},
        { -0.5,  0.5},
        {  0.0, -0.5},
        {  0.5,  0.0},
        {  0.0,  0.5},
        { -0.5,  0.0},
        {  0.0,  0.0}
      };
      SPoint3 pointz;
      for (int k = 0; k<9; k++) {
        incpl2.pnt(quad_v[k][0], quad_v[k][1], 0, pointz);
        MVertex *v = new MVertex(pointz.x(), pointz.y(), pointz.z(), gr);
        gr->mesh_vertices.push_back(v);
        std::vector<MVertex*>::iterator cursor = vr.begin();
        cursor += offsets[k];
        vr.insert(cursor, v);
      }
    }
  }
  ve.insert(ve.end(), vr.begin(), vr.end());
  return new MPyramidN(p->getVertex(0), p->getVertex(1),
                       p->getVertex(2), p->getVertex(3),
                       p->getVertex(4), ve, nPts + 1,
                       0, p->getPartition());
}

static void setHighOrder(GRegion *gr, edgeContainer &edgeVertices,
                         faceContainer &faceVertices, bool linear, bool incomplete,
                         int nPts = 1)
{
  std::vector<MTetrahedron*> tetrahedra2;
  for(unsigned int i = 0; i < gr->tetrahedra.size(); i++){
    MTetrahedron *t = gr->tetrahedra[i];
    MTetrahedron *tNew = setHighOrder(t, gr, edgeVertices, faceVertices, linear,
                                      incomplete, nPts);
    tetrahedra2.push_back(tNew);
    delete t;
  }
  gr->tetrahedra = tetrahedra2;

  std::vector<MHexahedron*> hexahedra2;
  for(unsigned int i = 0; i < gr->hexahedra.size(); i++){
    MHexahedron *h = gr->hexahedra[i];
    MHexahedron *hNew = setHighOrder(h, gr, edgeVertices, faceVertices, linear,
                                     incomplete, nPts);
    hexahedra2.push_back(hNew);
    delete h;
  }
  gr->hexahedra = hexahedra2;

  std::vector<MPrism*> prisms2;
  for(unsigned int i = 0; i < gr->prisms.size(); i++){
    MPrism *p = gr->prisms[i];
    MPrism *pNew = setHighOrder(p, gr, edgeVertices, faceVertices, linear,
                                incomplete, nPts);
    prisms2.push_back(pNew);
    delete p;
  }
  gr->prisms = prisms2;

  std::vector<MPyramid*> pyramids2;
  for(unsigned int i = 0; i < gr->pyramids.size(); i++) {
    MPyramid *p = gr->pyramids[i];
    MPyramid *pNew = setHighOrder(p, gr, edgeVertices, faceVertices, linear,
                                  incomplete, nPts);
    pyramids2.push_back(pNew);
    delete p;
  }
  gr->pyramids = pyramids2;

  gr->deleteVertexArrays();
}

template<class T>
static void setFirstOrder(GEntity *e, std::vector<T*> &elements, bool onlyVisible)
{
  if (onlyVisible && !e->getVisibility())return;
  std::vector<T*> elements1;
  for(unsigned int i = 0; i < elements.size(); i++){
    T *ele = elements[i];
    int n = ele->getNumPrimaryVertices();
    std::vector<MVertex*> v1;
    for(int j = 0; j < n; j++)
      v1.push_back(ele->getVertex(j));
    elements1.push_back(new T(v1, 0, ele->getPartition()));
    delete ele;
  }
  elements = elements1;
  e->deleteVertexArrays();
}

static void removeHighOrderVertices(GEntity *e, bool onlyVisible)
{
  if (onlyVisible && !e->getVisibility())return;
  std::vector<MVertex*> v1;
  for(unsigned int i = 0; i < e->mesh_vertices.size(); i++){
    if(e->mesh_vertices[i]->getPolynomialOrder() > 1)
      delete e->mesh_vertices[i];
    else
      v1.push_back(e->mesh_vertices[i]);
  }
  e->mesh_vertices = v1;
}

void SetOrder1(GModel *m,  bool onlyVisible)
{
  m->destroyMeshCaches();

  // replace all elements with first order elements
  for(GModel::eiter it = m->firstEdge(); it != m->lastEdge(); ++it){
    setFirstOrder(*it, (*it)->lines, onlyVisible);
  }
  for(GModel::fiter it = m->firstFace(); it != m->lastFace(); ++it){
    setFirstOrder(*it, (*it)->triangles, onlyVisible);
    setFirstOrder(*it, (*it)->quadrangles, onlyVisible);
  }
  for(GModel::riter it = m->firstRegion(); it != m->lastRegion(); ++it){
    setFirstOrder(*it, (*it)->tetrahedra, onlyVisible);
    setFirstOrder(*it, (*it)->hexahedra, onlyVisible);
    setFirstOrder(*it, (*it)->prisms, onlyVisible);
    setFirstOrder(*it, (*it)->pyramids, onlyVisible);
  }

  // remove all high order vertices
  for(GModel::eiter it = m->firstEdge(); it != m->lastEdge(); ++it)
    removeHighOrderVertices(*it, onlyVisible);
  for(GModel::fiter it = m->firstFace(); it != m->lastFace(); ++it)
    removeHighOrderVertices(*it, onlyVisible);
  for(GModel::riter it = m->firstRegion(); it != m->lastRegion(); ++it)
    removeHighOrderVertices(*it, onlyVisible);
}

void checkHighOrderTriangles(const char* cc, GModel *m,
                             std::vector<MElement*> &bad, double &minJGlob)
{
  bad.clear();
  minJGlob = 1.0;
  double minGGlob = 1.0;
  double avg = 0.0;
  int count = 0, nbfair=0;
  for(GModel::fiter it = m->firstFace(); it != m->lastFace(); ++it){
    for(unsigned int i = 0; i < (*it)->triangles.size(); i++){
      MTriangle *t = (*it)->triangles[i];
      double disto_ = t->distoShapeMeasure();
      double gamma_ = t->gammaShapeMeasure();
      double disto = disto_;
      minJGlob = std::min(minJGlob, disto);
      minGGlob = std::min(minGGlob, gamma_);
      avg += disto; count++;
      if (disto < 0) bad.push_back(t);
      else if (disto < 0.2) nbfair++;
    }
    /* FIXME THIS IS WRONG (crashes in distoShapeMesure -- see comment in
       bez2LagPoints in JacobianBasis.cpp)

    for(unsigned int i = 0; i < (*it)->quadrangles.size(); i++){
      MQuadrangle *t = (*it)->quadrangles[i];
      double disto_ = t->distoShapeMeasure();
      double gamma_ = t->gammaShapeMeasure();
      double disto = disto_;
      minJGlob = std::min(minJGlob, disto);
      minGGlob = std::min(minGGlob, gamma_);
      avg += disto; count++;
      if (disto < 0) bad.push_back(t);
      else if (disto < 0.2) nbfair++;
    }
    */
  }
  if(!count) return;
  if (minJGlob > 0)
    Msg::Info("%s : Worst Face Distorsion Mapping %g Gamma %g Nb elem. (0<d<0.2) = %d",
              cc, minJGlob, minGGlob,nbfair );
  else
    Msg::Warning("%s : Worst Face Distorsion Mapping %g (%d negative jacobians) "
                 "Worst Gamma %g Avg Smoothness %g", cc, minJGlob, bad.size(),
                 minGGlob, avg / (count ? count : 1));
}

void checkHighOrderTetrahedron(const char* cc, GModel *m,
                               std::vector<MElement*> &bad, double &minJGlob)
{
  bad.clear();
  minJGlob = 1.0;
  double avg = 0.0;
  int count = 0, nbfair=0;
  for(GModel::riter it = m->firstRegion(); it != m->lastRegion(); ++it){
    for(unsigned int i = 0; i < (*it)->tetrahedra.size(); i++){
      MTetrahedron *t = (*it)->tetrahedra[i];
      double disto_ = t->distoShapeMeasure();
      minJGlob = std::min(minJGlob, disto_);
      avg += disto_; count++;
      if (disto_ < 0) bad.push_back(t);
      else if (disto_ < 0.2) nbfair++;
    }
  }
  if(!count) return;
  if (minJGlob < 0)
    Msg::Warning("%s : Worst Tetrahedron Smoothness %g (%d negative jacobians) "
                 "Avg Smoothness %g", cc, minJGlob, bad.size(),
                 avg / (count ? count : 1));
  else
    Msg::Info("%s : Worst Tetrahedron Smoothness %g (%d negative jacobians) "
                 "Avg Smoothness %g", cc, minJGlob, bad.size(),
                 avg / (count ? count : 1));
}

void getMeshInfoForHighOrder(GModel *gm, int &meshOrder, bool &complete,
                             bool &CAD)
{
  meshOrder = -1;
  CAD = true;
  complete = 1;
  for (GModel::riter itr = gm->firstRegion(); itr != gm->lastRegion(); ++itr) {
    if ((*itr)->getNumMeshElements()){
      meshOrder = (*itr)->getMeshElement(0)->getPolynomialOrder();
      complete = (meshOrder <= 2) ? 1 :  (*itr)->getMeshElement(0)->getNumVolumeVertices();
      break;
    }
  }
  for (GModel::fiter itf = gm->firstFace(); itf != gm->lastFace(); ++itf) {
    if ((*itf)->getNumMeshElements()){
      if (meshOrder == -1) {
        meshOrder = (*itf)->getMeshElement(0)->getPolynomialOrder();
        complete = (meshOrder <= 2) ? 1 :  (*itf)->getMeshElement(0)->getNumFaceVertices();
        if ((*itf)->geomType() == GEntity::DiscreteSurface)CAD = false;
        break;
      }
    }
  }
}

void SetOrderN(GModel *m, int order, bool linear, bool incomplete, bool onlyVisible)
{
  // replace all the elements in the mesh with second order elements
  // by creating unique vertices on the edges/faces of the mesh:
  //
  // - if linear is set to true, new vertices are created by linear
  //   interpolation between existing ones. If not, new vertices are
  //   created on the exact geometry, provided that the geometrical
  //   edges/faces are discretized with 1D/2D elements. (I.e., if
  //   there are only 3D elements in the mesh then any new vertices on
  //   the boundary will always be created by linear interpolation,
  //   whether linear is set or not.)
  //
  // - if incomplete is set to true, we only create new vertices on
  //   edges (creating 8-node quads, 20-node hexas, etc., instead of
  //   9-node quads, 27-node hexas, etc.)

  // - if onlyVisible is true, then only the visible entities will be curved.

  int nPts = order - 1;

  char msg[256];
  sprintf(msg, "Meshing order %d (curvilinear %s)...", order, linear ? "off" : "on");

  Msg::StatusBar(true, msg);

  double t1 = Cpu();

  // first, make sure to remove any existsing second order vertices/elements
  SetOrder1(m, onlyVisible);

  // then create new second order vertices/elements
  edgeContainer edgeVertices;
  faceContainer faceVertices;

  Msg::ResetProgressMeter();

  int counter = 0, nTot = m->getNumEdges() + m->getNumFaces() + m->getNumRegions();

  for(GModel::eiter it = m->firstEdge(); it != m->lastEdge(); ++it) {
    Msg::Info("Meshing curve %d order %d", (*it)->tag(), order);
    Msg::ProgressMeter(++counter, nTot, false, msg);
    if (onlyVisible && !(*it)->getVisibility()) continue;
    setHighOrder(*it, edgeVertices, linear, nPts);
  }

  for(GModel::fiter it = m->firstFace(); it != m->lastFace(); ++it) {
    Msg::Info("Meshing surface %d order %d", (*it)->tag(), order);
    Msg::ProgressMeter(++counter, nTot, false, msg);
    if (onlyVisible && !(*it)->getVisibility()) continue;
    setHighOrder(*it, edgeVertices, faceVertices, linear, incomplete, nPts);
  }

  for(GModel::riter it = m->firstRegion(); it != m->lastRegion(); ++it) {
    Msg::Info("Meshing volume %d order %d", (*it)->tag(), order);
    Msg::ProgressMeter(++counter, nTot, false, msg);
    if (onlyVisible && !(*it)->getVisibility())continue;
    setHighOrder(*it, edgeVertices, faceVertices, linear, incomplete, nPts);
  }

  double t2 = Cpu();

  std::vector<MElement*> bad;
  double worst;
  checkHighOrderTriangles("Surface mesh", m, bad, worst);
  checkHighOrderTetrahedron("Volume Mesh", m, bad, worst);

  Msg::StatusBar(true, "Done meshing order %d (%g s)", order, t2 - t1);
}

void computeDistanceFromMeshToGeometry (GModel *m, distanceFromMeshToGeometry_t &dist)
{
  for (GModel::eiter itEdge = m->firstEdge(); itEdge != m->lastEdge(); ++itEdge) {
    double d2,dmax;
    (*itEdge)->computeDistanceFromMeshToGeometry(d2, dmax);
    dist.d2[*itEdge] = d2;
    dist.d_max[*itEdge] = dmax;
  }

  for (GModel::fiter itFace = m->firstFace(); itFace != m->lastFace(); ++itFace) {

  }
}

void SetHighOrderComplete(GModel *m, bool onlyVisible)
{
  faceContainer faceVertices;
  for(GModel::fiter it = m->firstFace(); it != m->lastFace(); ++it) {
    if (onlyVisible && !(*it)->getVisibility()) continue;
    std::vector<MTriangle*> newT;
    std::vector<MQuadrangle*> newQ;
    for (unsigned int i = 0; i < (*it)->triangles.size(); i++){
      MTriangle *t = (*it)->triangles[i];
      std::vector<MVertex*> vf, vt;
      int nPts = t->getPolynomialOrder() - 1;
      MTriangle TEMP (t->getVertex(0), t->getVertex(1), t->getVertex(2), 0, t->getPartition());
      getFaceVertices (*it, t, t, vf, faceVertices, false, nPts);
      for (int j=3;j<t->getNumVertices();j++)vt.push_back(t->getVertex(j));
      vt.insert(vt.end(), vf.begin(), vf.end());
      MTriangleN *newTr = new MTriangleN(t->getVertex(0), t->getVertex(1), t->getVertex(2),
                                         vt, nPts + 1, 0, t->getPartition());
      newT.push_back(newTr);

      delete t;
    }
    (*it)->triangles = newT;

    for (unsigned int i = 0; i < (*it)->quadrangles.size(); i++){
      MQuadrangle *t = (*it)->quadrangles[i];
      std::vector<MVertex*> vf, vt;
      int nPts = t->getPolynomialOrder() - 1;
      MQuadrangle TEMP (t->getVertex(0), t->getVertex(1), t->getVertex(2),
                        t->getVertex(3), 0, t->getPartition());
      getFaceVertices (*it, t, &TEMP, vf, faceVertices, false, nPts);
      for (int j=4;j<t->getNumVertices();j++)vt.push_back(t->getVertex(j));
      vt.insert(vt.end(), vf.begin(), vf.end());
      newQ.push_back(new MQuadrangleN(t->getVertex(0), t->getVertex(1),
                                      t->getVertex(2), t->getVertex(3),
                                      vt, nPts + 1, 0, t->getPartition()));
      delete t;
    }
    (*it)->quadrangles = newQ;

    std::set<MVertex*> newV;
    for (unsigned int i = 0; i < (*it)->getNumMeshElements(); ++i){
      MElement *e = (*it)->getMeshElement(i);
      for (int j=0;j<e->getNumVertices();j++)newV.insert(e->getVertex(j));
    }
    (*it)->mesh_vertices.clear();
    (*it)->mesh_vertices.insert((*it)->mesh_vertices.begin(), newV.begin(), newV.end());
  }
}

void SetHighOrderInComplete (GModel *m, bool onlyVisible)
{
  std::set<MVertex*> toDelete;
  for(GModel::fiter it = m->firstFace(); it != m->lastFace(); ++it) {
    if (onlyVisible && !(*it)->getVisibility()) continue;
    std::vector<MTriangle*> newT;

    for (unsigned int i = 0; i < (*it)->triangles.size(); i++){
      MTriangle *t = (*it)->triangles[i];
      std::vector<MVertex*> vt;
      int order = t->getPolynomialOrder();
      for (int j=3;j<t->getNumVertices()-t->getNumFaceVertices();j++)
        vt.push_back(t->getVertex(j));
      for (int j = t->getNumVertices()-t->getNumFaceVertices();
           j < t->getNumVertices(); j++)
        toDelete.insert(t->getVertex(j));
      newT.push_back(new MTriangleN(t->getVertex(0), t->getVertex(1), t->getVertex(2),
                                    vt, order, 0, t->getPartition()));
      delete t;
    }
    (*it)->triangles = newT;

    std::vector<MQuadrangle*> newQ;
    for (unsigned int i = 0; i < (*it)->quadrangles.size(); i++){
      MQuadrangle *t = (*it)->quadrangles[i];
      std::vector<MVertex*> vt;
      int nPts = t->getPolynomialOrder() - 1;
      for (int j = 4; j < t->getNumVertices()-t->getNumFaceVertices(); j++)
        vt.push_back(t->getVertex(j));
      newQ.push_back(new MQuadrangleN(t->getVertex(0), t->getVertex(1),
                                      t->getVertex(2), t->getVertex(3),
                                      vt, nPts + 1, 0, t->getPartition()));
      delete t;
    }
    (*it)->quadrangles = newQ;

    std::vector<MVertex*> newV;
    int numd = 0;
    for (unsigned int i = 0; i < (*it)->mesh_vertices.size(); ++i){
      if (toDelete.find((*it)->mesh_vertices[i]) == toDelete.end())
        newV.push_back((*it)->mesh_vertices[i]);
      else{
        delete (*it)->mesh_vertices[i];
        numd++;
      }
    }
    (*it)->mesh_vertices = newV;
  }
}
