// Copyright (C) 2017 ULg-UCL
//
// Permission is hereby granted, free of charge, to any person
// obtaining a copy of this software and associated documentation
// files (the "Software"), to deal in the Software without
// restriction, including without limitation the rights to use, copy,
// modify, merge, publish, distribute, and/or sell copies of the
// Software, and to permit persons to whom the Software is furnished
// to do so, provided that the above copyright notice(s) and this
// permission notice appear in all copies of the Software and that
// both the above copyright notice(s) and this permission notice
// appear in supporting documentation.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
// EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
// MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
// NONINFRINGEMENT OF THIRD PARTY RIGHTS. IN NO EVENT SHALL THE
// COPYRIGHT HOLDER OR HOLDERS INCLUDED IN THIS NOTICE BE LIABLE FOR
// ANY CLAIM, OR ANY SPECIAL INDIRECT OR CONSEQUENTIAL DAMAGES, OR ANY
// DAMAGES WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS,
// WHETHER IN AN ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS
// ACTION, ARISING OUT OF OR IN CONNECTION WITH THE USE OR PERFORMANCE
// OF THIS SOFTWARE.
//
// Contributors: Amaury Johnen

#include "BoundaryLayerCurver.h"
#include "MQuadrangle.h"
#include "MTriangle.h"
#include "BasisFactory.h"
#include "GFace.h"
#include "orthogonalBasis.h"
#include "bezierBasis.h"

#include "gmshVertex.h"
#include "Geo.h"
#include "MLine.h"
#include "GModel.h"
#if defined(HAVE_POST)
#include "PView.h"
#endif
#if defined(HAVE_FLTK)
#include "FlGui.h"
#endif
#include "Options.h"
#include "AnalyseCurvedMesh.h"
#include "InnerVertexPlacement.h"
#include "pointsGenerators.h"
#include "qualityMeasuresJacobian.h"

namespace
{
  void drawEquidistantPoints(GEdge *gedge, int N)
  {
    return;
    const unsigned int numLine = gedge->getNumMeshElements();
    for (unsigned int i = 0; i < numLine; ++i) {
      gedge->getMeshElement(i)->setVisibility(0);
    }

    const double umin = gedge->getLowerBound();
    const double umax = gedge->getUpperBound();
    const double step = (umax-umin) / (N-1);

    for (int i = 0; i < N; ++i) {
      const double u = umin + i * step;
      const GPoint p = gedge->point(u);
      MVertex *v = new MVertex(p.x(), p.y(), p.z());
      gedge->addMeshVertex(v);
      gedge->addLine(new MLine(v, v));
    }
  }

  void drawBezierControlPolygon(const fullMatrix<double> &controlPoints,
                                GEdge *gedge)
  {
    const int nVert = controlPoints.size1();
    std::vector<int> idx(nVert);
    idx[0] = 0;
    for (int i = 1; i < nVert-1; ++i) idx[i] = i+1;
    idx[nVert-1] = 1;

    MVertex *previous = NULL;
    for (int i = 0; i < nVert; ++i) {
      MVertex *v = new MVertex(controlPoints(idx[i], 0), controlPoints(idx[i], 1),
                               controlPoints(idx[i], 2), gedge);
      if (previous) {
        MLine *line = new MLine(v, previous);
        gedge->addLine(line);
      }
      gedge->addMeshVertex(v);
      previous = v;
    }
  }

  void drawBezierControlPolygon(const std::vector<MVertex *> &vertices,
                                GEdge *gedge = NULL)
  {
    if (!gedge) {
      gedge = *GModel::current()->firstEdge();
    }

    const int nVert = (int)vertices.size();
    const bezierBasis *fs = BasisFactory::getBezierBasis(TYPE_LIN, nVert - 1);

    fullMatrix<double> xyz(nVert, 3);
    for (int i = 0; i < nVert; ++i) {
      xyz(i, 0) = vertices[i]->x();
      xyz(i, 1) = vertices[i]->y();
      xyz(i, 2) = vertices[i]->z();
    }
    fullMatrix<double> controlPoints(nVert, 3);
    fs->lag2Bez(xyz, controlPoints);

    bool subdivide = false;
    bool subdivide2 = false;
    if (subdivide) {
      fullMatrix<double> allSubs(2*nVert, 3);
      fs->subdivideBezCoeff(controlPoints, allSubs);
      fullMatrix<double> sub(nVert, 3);
      sub.copy(allSubs, 0, nVert, 0, 3, 0, 0);
      if (subdivide2) {
        fullMatrix<double> allSubs2(2*nVert, 3);
        fs->subdivideBezCoeff(sub, allSubs2);
        fullMatrix<double> sub2(nVert, 3);
        sub2.copy(allSubs2, 0, nVert, 0, 3, 0, 0);
        drawBezierControlPolygon(sub2, gedge);
        sub2.copy(allSubs2, nVert, nVert, 0, 3, 0, 0);
        drawBezierControlPolygon(sub2, gedge);
      }
      else
        drawBezierControlPolygon(sub, gedge);
      sub.copy(allSubs, nVert, nVert, 0, 3, 0, 0);
      if (subdivide2) {
        fullMatrix<double> allSubs2(2*nVert, 3);
        fs->subdivideBezCoeff(sub, allSubs2);
        fullMatrix<double> sub2(nVert, 3);
        sub2.copy(allSubs2, 0, nVert, 0, 3, 0, 0);
        drawBezierControlPolygon(sub2, gedge);
        sub2.copy(allSubs2, nVert, nVert, 0, 3, 0, 0);
        drawBezierControlPolygon(sub2, gedge);
      }
      else
        drawBezierControlPolygon(sub, gedge);
    }
    else {
      drawBezierControlPolygon(controlPoints, gedge);
    }
  }

  void draw3DFrame(SPoint3 &p, SVector3 &t, SVector3 &n, SVector3 &w,
                   double unitDimension, GFace *gFace = NULL)
  {
    return;
    if (!gFace)
      gFace = *GModel::current()->firstFace();

    MVertex *v = new MVertex(p.x(), p.y(), p.z(), gFace);

    SPoint3 pnt = p + n * unitDimension * .75;
    MVertex *vn = new MVertex(pnt.x(), pnt.y(), pnt.z(), gFace);

    pnt = p + w * unitDimension * 2;
    MVertex *vw = new MVertex(pnt.x(), pnt.y(), pnt.z(), gFace);

    pnt = p + t * unitDimension;
    MVertex *vt = new MVertex(pnt.x(), pnt.y(), pnt.z(), gFace);

    gFace->addMeshVertex(v);
    gFace->addMeshVertex(vn);
    gFace->addMeshVertex(vw);

    MLine *line = new MLine(v, vn);
    gFace->edges().front()->addLine(line);

    line = new MLine(v, vw);
    gFace->edges().front()->addLine(line);

    line = new MLine(v, vt);
    gFace->edges().front()->addLine(line);
  }
}

namespace BoundaryLayerCurver
{
  void projectVertexIntoGFace(MVertex *v, const GFace *gface)
  {
    SPoint3 p = v->point();
    SPoint2 param = gface->parFromPoint(p);
    GPoint projected = gface->point(param);
    v->x() = projected.x();
    v->y() = projected.y();
    v->z() = projected.z();
  }

  void projectVerticesIntoGFace(const MEdgeN *edge, const GFace *gface,
                                bool alsoExtremity = true)
  {
    int i = alsoExtremity ? 0 : 2;

    for (; i < edge->getNumVertices(); ++i)
      projectVertexIntoGFace(edge->getVertex(i), gface);
  }

  void projectVerticesIntoGFace(const MFaceN *face, const GFace *gface,
                                bool alsoBoundary = true)
  {
    int i = alsoBoundary ? 0 : face->getNumVerticesOnBoundary();

    for (; i < face->getNumVertices(); ++i)
      projectVertexIntoGFace(face->getVertex(i), gface);
  }

  namespace EdgeCurver2D
  {
    // TODO: smooth normals if CAD not available
    // TODO: check quality of elements

    _Frame::_Frame(const MEdgeN *edge, const GFace *gface, const GEdge *gedge,
                   const SVector3 &normal)
        : _normalToTheMesh(normal), _gface(gface), _gedge(gedge),
          _edgeOnBoundary(edge)
    {
      const int nVert = (int)edge->getNumVertices();

      if (gface) {
        for (int i = 0; i < nVert; ++i) {
          SPoint2 param;
          bool success =
              reparamMeshVertexOnFace(edge->getVertex(i), gface, param, true);
          _paramVerticesOnGFace[2*i+0] = param[0];
          _paramVerticesOnGFace[2*i+1] = param[1];
          if (!success) {
            Msg::Warning("Could not compute param of vertex %d on surface %d",
                         edge->getVertex(i)->getNum(), gface->tag());
          }
          // TODO: Check if periodic face
        }
      }

      if (gedge) {
        for (int i = nVert - 1; i >= 0; --i) {
          bool success = reparamMeshVertexOnEdge(edge->getVertex(i), gedge,
                                                 _paramVerticesOnGEdge[i]);
          if (!success) {
            Msg::Warning("Could not compute param of vertex %d on edge %d",
                         edge->getVertex(i)->getNum(), gedge->tag());
          }
          else if (gedge->periodic(0) &&
                   edge->getVertex(i) == gedge->getBeginVertex()->mesh_vertices[0]) {
            double u0 = gedge->getLowerBound();
            double un = gedge->getUpperBound();
            int k = (nVert == 2 ? 1-i : (i == 0 ? 2 : nVert-1)) ;
            double uk = _paramVerticesOnGEdge[k];
            _paramVerticesOnGEdge[i] = uk - u0 < un - uk ? u0 : un;
          }
        }
      }
    }

    void _Frame::computeFrame(double paramEdge, SVector3 &t, SVector3 &n,
                              SVector3 &w, bool atExtremity) const
    {
      if (_gedge) {
        double paramGeoEdge;
        if (atExtremity) {
          if (paramEdge < 0) paramGeoEdge = _paramVerticesOnGEdge[0];
          else               paramGeoEdge = _paramVerticesOnGEdge[1];
        }
        else paramGeoEdge =
            _edgeOnBoundary->interpolate(_paramVerticesOnGEdge, paramEdge);

        t = _gedge->firstDer(paramGeoEdge);
        t.normalize();
      }
      if (!_gedge || t.norm() == 0) {
        t = _edgeOnBoundary->tangent(paramEdge);
      }

      if (_gface) {
        SPoint2 paramGFace;
        if (atExtremity) {
          if (paramEdge < 0) paramGFace = SPoint2(_paramVerticesOnGFace[0],
                                                  _paramVerticesOnGFace[1]);
          else               paramGFace = SPoint2(_paramVerticesOnGFace[2],
                                                  _paramVerticesOnGFace[3]);
        }
        else {
          paramGFace = SPoint2(
              _edgeOnBoundary->interpolate(_paramVerticesOnGFace, paramEdge, 2),
              _edgeOnBoundary->interpolate(_paramVerticesOnGFace+1, paramEdge, 2)
          );
        }
        w = _gface->normal(paramGFace);
      }
      else {
        w = _normalToTheMesh;
      }
      if (w.norm() == 0) {
        Msg::Error("normal to the CAD or 2Dmesh is nul. BL curving will fail.");
      }
      n = crossprod(w, t);
    }

    SPoint3 _Frame::pnt(double u) const
    {
      if (!_gedge)
        return SPoint3();

      double paramGeoEdge =
          _edgeOnBoundary->interpolate(_paramVerticesOnGEdge, u);
      GPoint p = _gedge->point(paramGeoEdge);
      return SPoint3(p.x(), p.y(), p.z());
    }

    void _computeParameters(const MEdgeN *baseEdge, const MEdgeN *otherEdge,
                            const _Frame &frame, double coeffs[2][3])
    {
      SVector3 t, n, w, h;
      MVertex *vb, *vt;

      frame.computeFrame(-1, t, n, w, true);
      vb = baseEdge->getVertex(0);
      vt = otherEdge->getVertex(0);
      h = SVector3(vt->x() - vb->x(),
                   vt->y() - vb->y(),
                   vt->z() - vb->z());
      coeffs[0][0] = dot(h, n);
      coeffs[0][1] = dot(h, t);
      coeffs[0][2] = dot(h, w);

      SPoint3 p1 = frame.pnt(-1);
//      SPoint3 p1(vb->x(), vb->y(), vb->z());
      draw3DFrame(p1, t, n, w, .0004);

      frame.computeFrame(1, t, n, w, true);
      vb = baseEdge->getVertex(1);
      vt = otherEdge->getVertex(1);
      h = SVector3(vt->x() - vb->x(),
                   vt->y() - vb->y(),
                   vt->z() - vb->z());
      coeffs[1][0] = dot(h, n);
      coeffs[1][1] = dot(h, t);
      coeffs[1][2] = dot(h, w);
//
      SPoint3 p2 = frame.pnt(1);
//      SPoint3 p2(vb->x(), vb->y(), vb->z());
      draw3DFrame(p2, t, n, w, .0004);
    }

    void _idealPositionEdge(const MEdgeN *baseEdge, const _Frame &frame,
                            double coeffs[2][3], int nbPoints,
                            const IntPt *points, fullMatrix<double> &xyz)
    {
      for (int i = 0; i < nbPoints; ++i) {
        double u = points[i].pt[0];
        SPoint3 p = baseEdge->pnt(u);
        SVector3 t, n, w;
        frame.computeFrame(u, t, n, w);

//        draw3DFrame(p, t, n, w, .0002);
        SPoint3 pp = frame.pnt(u);
        draw3DFrame(pp, t, n, w, .0002);

        double interpolatedCoeffs[3];
        for (int j = 0; j < 3; ++j) {
          interpolatedCoeffs[j] =   coeffs[0][j] * (1-u)/2
                                  + coeffs[1][j] * (1+u)/2;
        }
        SVector3 h;
        h =   interpolatedCoeffs[0] * n
            + interpolatedCoeffs[1] * t
            + interpolatedCoeffs[2] * w;
        xyz(i, 0) = p.x() + h.x();
        xyz(i, 1) = p.y() + h.y();
        xyz(i, 2) = p.z() + h.z();
      }
    }

    void _drawIdealPositionEdge(const MEdgeN *baseEdge, const _Frame &frame,
                                double coeffs[2][3], GEdge *gedge)
    {
      int N = 100;
      MVertex *previous = NULL;

      for (int i = 0; i < N+1; ++i) {
        const double u = (double)i / N * 2 - 1;
        SPoint3 p = baseEdge->pnt(u);
        SVector3 t, n, w;
        frame.computeFrame(u, t, n, w);

        double interpolatedCoeffs[3];
        for (int j = 0; j < 3; ++j) {
          interpolatedCoeffs[j] =   coeffs[0][j] * (1-u)/2
                                  + coeffs[1][j] * (1+u)/2;
        }
        SVector3 h;
        h =   interpolatedCoeffs[0] * n
            + interpolatedCoeffs[1] * t
            + interpolatedCoeffs[2] * w;
        double x = p.x() + h.x();
        double y = p.y() + h.y();
        double z = p.z() + h.z();

        MVertex *current = new MVertex(x, y, z, gedge);
        gedge->addMeshVertex(current);
        if (previous) {
          MLine *line = new MLine(previous, current);
          gedge->addLine(line);
        }
//        MVertex *base = new MVertex(p.x(), p.y(), p.z(), gedge);
//        MLine *line = new MLine(base, current);
//        gedge->addLine(line);
        previous = current;
      }
    }

    void curveEdge(const MEdgeN *baseEdge, MEdgeN *edge, const GFace *gface,
                   const GEdge *gedge, const SVector3 &normal)
    {
      _Frame frame(baseEdge, gface, gedge, normal);

      double coeffs[2][3];
      _computeParameters(baseEdge, edge, frame, coeffs);

      const int orderCurve = baseEdge->getPolynomialOrder();
      const int orderGauss = orderCurve * 2;
      const int sizeSystem = getNGQLPts(orderGauss);
      const IntPt *gaussPnts = getGQLPts(orderGauss);

      // Least square projection
      fullMatrix<double> xyz(sizeSystem + 2, 3);
      _idealPositionEdge(baseEdge, frame, coeffs, sizeSystem, gaussPnts, xyz);
//      _drawIdealPositionEdge(baseEdge, frame, coeffs, (GEdge*)gedge);
      for (int i = 0; i < 2; ++i) {
        xyz(sizeSystem + i, 0) = edge->getVertex(i)->x();
        xyz(sizeSystem + i, 1) = edge->getVertex(i)->y();
        xyz(sizeSystem + i, 2) = edge->getVertex(i)->z();
      }

      LeastSquareData *data = getLeastSquareData(TYPE_LIN, orderCurve,
                                                 orderGauss);
      fullMatrix<double> newxyz(orderCurve + 1, 3);
      data->invA.mult(xyz, newxyz);

      for (int i = 2; i < edge->getNumVertices(); ++i) {
        edge->getVertex(i)->x() = newxyz(i, 0);
        edge->getVertex(i)->y() = newxyz(i, 1);
        edge->getVertex(i)->z() = newxyz(i, 2);
      }

      if (gface) projectVerticesIntoGFace(edge, gface, false);
    }

    void _reduceCurving(MEdgeN *edge, double factor, const GFace *gface)
    {
      int order = edge->getPolynomialOrder();

      MVertex *v0 = edge->getVertex(0);
      MVertex *v1 = edge->getVertex(1);

      for (int i = 2; i < order + 1; ++i) {
        double f = (double)(i-1) / order;
        MVertex *v = edge->getVertex(i);
        v->x() = (1-factor) * v->x() + factor * ((1-f) * v0->x() + f * v1->x());
        v->y() = (1-factor) * v->y() + factor * ((1-f) * v0->y() + f * v1->y());
        v->z() = (1-factor) * v->z() + factor * ((1-f) * v0->z() + f * v1->z());
      }
      if (gface) projectVerticesIntoGFace(edge, gface, false);
    }

    void _reduceOrderCurve(MEdgeN *edge, int order, const GFace *gface)
    {
      const int orderCurve = edge->getPolynomialOrder();
      const int orderGauss = order * 2;
      const int sizeSystem = getNGQLPts(orderGauss);
      const IntPt *gaussPnts = getGQLPts(orderGauss);

      // Least square projection
      fullMatrix<double> xyz(sizeSystem + 2, 3);
      for (int i = 0; i < sizeSystem; ++i) {
        SPoint3 p = edge->pnt(gaussPnts[i].pt[0]);
        xyz(i, 0) = p.x();
        xyz(i, 1) = p.y();
        xyz(i, 2) = p.z();
      }
      for (int i = 0; i < 2; ++i) {
        xyz(sizeSystem + i, 0) = edge->getVertex(i)->x();
        xyz(sizeSystem + i, 1) = edge->getVertex(i)->y();
        xyz(sizeSystem + i, 2) = edge->getVertex(i)->z();
      }

      LeastSquareData *data = getLeastSquareData(TYPE_LIN, order, orderGauss);
      fullMatrix<double> newxyzLow(order + 1, 3);
      data->invA.mult(xyz, newxyzLow);

      std::vector<MVertex *> vertices = edge->getVertices();
      vertices.resize((unsigned int) order + 1);
      MEdgeN lowOrderEdge(vertices);

      for (unsigned int i = 2; i < vertices.size(); ++i) {
        vertices[i]->x() = newxyzLow(i, 0);
        vertices[i]->y() = newxyzLow(i, 1);
        vertices[i]->z() = newxyzLow(i, 2);
      }

      const int tagLine = ElementType::getType(TYPE_LIN, orderCurve);
      const nodalBasis *nb = BasisFactory::getNodalBasis(tagLine);
      const fullMatrix<double> &refpnts = nb->getReferenceNodes();

      fullMatrix<double> newxyz(edge->getNumVertices(), 3);
      for (unsigned int i = 2; i < edge->getNumVertices(); ++i) {
        SPoint3 p = lowOrderEdge.pnt(refpnts(i, 0));
        newxyz(i, 0) = p.x();
        newxyz(i, 1) = p.y();
        newxyz(i, 2) = p.z();
      }

      for (int i = 2; i < edge->getNumVertices(); ++i) {
        edge->getVertex(i)->x() = newxyz(i, 0);
        edge->getVertex(i)->y() = newxyz(i, 1);
        edge->getVertex(i)->z() = newxyz(i, 2);
      }

      if (gface) projectVerticesIntoGFace(edge, gface, false);
    }

    void recoverQualityElements(std::vector<MEdgeN> &stackEdges,
                                std::vector<MFaceN> &stackFaces,
                                std::vector<MElement*> &stackElements,
                                int iFirst, int iLast, const GFace *gface)
    {
      std::vector<MEdgeN> subsetEdges(4);
      subsetEdges[0] = stackEdges[0];
      subsetEdges[1] = stackEdges[iFirst];
      subsetEdges[2] = stackEdges[iLast-1];
      subsetEdges[3] = stackEdges[iLast];
      MEdgeN *lastEdge = &stackEdges[iLast];
      std::vector<MFaceN> subsetFaces;
      subsetFaces.push_back(stackFaces[iLast-1]);
      subsetFaces.push_back(stackFaces[iLast]);
      // Warning: subsetFaces should contain 2 faces since
      // repositionInnerVertices() need a column of BL faces + the exterior face

      // First get sure that last element of the BL is of good quality
      MElement *lastElementBL = stackElements[iLast-1];
      MElement *linear = createPrimaryElement(lastElementBL);
      double qualLinear = jacobianBasedQuality::minIGEMeasure(linear);
      delete linear;

      InteriorEdgeCurver::curveEdges(subsetEdges, 1, 3, gface);
      repositionInnerVertices(subsetFaces, gface);
      double qual = jacobianBasedQuality::minIGEMeasure(lastElementBL);
      int currentOrder = lastEdge->getPolynomialOrder();
      while (qual < .75 && qual < .8*qualLinear && currentOrder > 2) {
        _reduceOrderCurve(lastEdge, --currentOrder, gface);
        InteriorEdgeCurver::curveEdges(subsetEdges, 1, 3, gface);
        repositionInnerVertices(subsetFaces, gface);
        qual = jacobianBasedQuality::minIGEMeasure(lastElementBL);
      }
      int iter = 0;
      const int maxIter = 15;
      while (qual < .75 && qual < .8*qualLinear && ++iter < maxIter) {
        _reduceCurving(lastEdge, .25, gface);
        InteriorEdgeCurver::curveEdges(subsetEdges, 1, 3, gface);
        repositionInnerVertices(subsetFaces, gface);
        qual = jacobianBasedQuality::minIGEMeasure(lastElementBL);
      }

      // Now, get sure the exterior element is of good quality
      MElement *lastElement = stackElements[iLast];
      linear = createPrimaryElement(lastElement);
      qualLinear = jacobianBasedQuality::minIGEMeasure(linear);
      delete linear;

      qual = jacobianBasedQuality::minIGEMeasure(lastElement);
      while (qual < .75 && qual < .8*qualLinear && ++iter < maxIter) {
        _reduceCurving(lastEdge, .25, gface);
        repositionInnerVertices(subsetFaces, gface);
        qual = jacobianBasedQuality::minIGEMeasure(lastElement);
      }
      if (iter == maxIter)
        _reduceCurving(lastEdge, 1, gface);
    }
  }

  namespace InteriorEdgeCurver
  {
    static std::map<std::pair<int, int>, TFIData*> tfiData;

    TFIData* _constructTFIData(int typeElement, int order)
    {
      TFIData *data = new TFIData;
      int nbDof = order+1;

      fullMatrix<double> Mh; // lag coeff p n -> lag coeff p (n+1)
      fullMatrix<double> M0; // lag coeff p (n+1) c -> (1-xi) * c
      fullMatrix<double> M1; // lag coeff p (n+1) c ->    xi  * c
      fullMatrix<double> Ml; // lag coeff p (n+1) -> leg coeff p n
      fullMatrix<double> Me; // leg coeff p n -> lag coeff p n

      if (typeElement == TYPE_LIN) {
        int tagLine = ElementType::getType(TYPE_LIN, order);
        const nodalBasis *fs = BasisFactory::getNodalBasis(tagLine);
        const fullMatrix<double> &refNodes = fs->getReferenceNodes();
        const fullMatrix<double> refNodesh = gmshGeneratePointsLine(order + 1);

        int nbDofh = refNodesh.size1();

//      refNodesh.print("refNodesh");

        Mh.resize(nbDofh, nbDof);
        for (int i = 0; i < nbDofh; ++i) {
          double sf[100];
          fs->f(refNodesh(i, 0), refNodesh(i, 1), refNodesh(i, 2), sf);
          for (int j = 0; j < nbDof; ++j) {
            Mh(i, j) = sf[j];
          }
        }
//      Mh.print("Mh");

        M0.resize(nbDofh, nbDofh, true);
        M1.resize(nbDofh, nbDofh, true);
        for (int i = 0; i < nbDofh; ++i) {
          M0(i, i) = .5 - refNodesh(i, 0) / 2;
          M1(i, i) = .5 + refNodesh(i, 0) / 2;
        }
//      M0.print("M0");
//      M1.print("M1");

        Ml.resize(nbDof, nbDofh);
        {
          fullMatrix<double> vandermonde(nbDofh, nbDofh);

          double *val = new double[nbDofh];
          for (int i = 0; i < nbDofh; ++i) {
            LegendrePolynomials::fc(order+1, refNodesh(i,0), val);
            for (int j = 0; j < nbDofh; ++j) {
              vandermonde(i, j) = val[j];
            }
          }
          delete val;

          fullMatrix<double> tmp;
          vandermonde.invert(tmp);
//        vandermonde.print("vandermonde");
//        tmp.print("tmp");
          Ml.copy(tmp, 0, nbDof, 0, nbDofh, 0, 0);
        }
//      Ml.print("Ml");

        Me.resize(nbDof, nbDof);
        {
          double *val = new double[nbDof];
          for (int i = 0; i < nbDof; ++i) {
            LegendrePolynomials::fc(order, refNodes(i, 0), val);
            for (int j = 0; j < nbDof; ++j) {
              Me(i, j) = val[j];
            }
          }
          delete val;
        }
//      Me.print("Me");

        fullMatrix<double> tmp0(nbDofh, nbDof);
        fullMatrix<double> tmp1(nbDofh, nbDof);
        M0.mult(Mh, tmp0);
        M1.mult(Mh, tmp1);
        fullMatrix<double> tmp(nbDof, nbDofh);
        Me.mult(Ml, tmp);
//      tmp.print("tmp");
        data->T0.resize(nbDof, nbDof);
        data->T1.resize(nbDof, nbDof);
        tmp.mult(tmp0, data->T0);
        tmp.mult(tmp1, data->T1);

//      data->T0.print("data->T0");
//      data->T1.print("data->T1");
      }

//    fullVector<double> x(nbDof);
//    x.setAll(1);
//    fullVector<double> b1(nbDof);
//    fullVector<double> b2(nbDof);
//    data->T0.mult(x, b1);
//    b1.print("b");
//    data->T1.mult(x, b1);
//    b1.print("b");
//
//    x(0) = 0;
//    x(2) = 1/6.;
//    x(3) = 2/6.;
//    x(4) = 3/6.;
//    x(5) = 4/6.;
//    x(6) = 5/6.;
//    x(1) = 1;
//    data->T0.mult(x, b1);
//    b1.print("b1");
//    data->T1.mult(x, b2);
//    b2.print("b2");
//    b1.axpy(b2);
//    b1.print("b");
//
//    x(0) = 0;
//    x(2) = 0.000021433470508;
//    x(3) = 0.001371742112483;
//    x(4) = 0.015625000000000;
//    x(5) = 0.087791495198903;
//    x(6) = 0.334897976680384;
//    x(1) = 1.000000000000000;
//    data->T0.mult(x, b1);
//    b1.print("b1");
//    data->T1.mult(x, b2);
//    b2.print("b2");
//    b1.axpy(b2);
//    b1.print("b");

      return data;
    }

    TFIData* _getTFIData(int typeElement, int order)
    {
      std::pair<int, int> typeOrder(typeElement, order);
      std::map<std::pair<int, int>, TFIData*>::iterator it;
      it = tfiData.find(typeOrder);

      if (it != tfiData.end()) return it->second;

      TFIData *data = _constructTFIData(typeElement, order);

      tfiData[typeOrder] = data;
      return data;
    }

    void _linearize(const fullMatrix<double> &x, fullMatrix<double> &lin)
    {
      int n = x.size1();
      lin.copy(x, 0, 2, 0, 3, 0, 0);
      for (int i = 2; i < n; ++i) {
        double fact = ((double)i-1)/(n-1);
        for (int j = 0; j < 3; ++j)
          lin(i, j) = (1-fact) * x(0, j) + fact * x(1, j);
      }
    }

    void _computeEtas(const std::vector<MEdgeN> &stack,
                      std::vector<std::pair<double, double> > &eta)
    {
      eta.resize(stack.size());
      eta[0] = std::make_pair(0, 0);
      MVertex *vb0 = stack[0].getVertex(0);
      MVertex *vb1 = stack[0].getVertex(1);

      for (unsigned int i = 1; i < stack.size(); ++i) {
        MVertex *v0 = stack[i].getVertex(0);
        MVertex *v1 = stack[i].getVertex(1);
        eta[i].first = vb0->distance(v0);
        eta[i].second = vb1->distance(v1);
      }

      for (int i = 1; i < eta.size(); ++i) {
        eta[i].first /= eta.back().first;
        eta[i].second /= eta.back().second;
      }
    }

    void _computeDeltaForTFI(const std::vector<MEdgeN> &stack, int iFirst, int iLast,
                             fullMatrix<double> &delta0,
                             fullMatrix<double> &delta1,
                             fullMatrix<double> &deltaN)
    {
      const int numVertices = stack[0].getNumVertices();

      fullMatrix<double> x0(numVertices, 3);
      fullMatrix<double> x1(numVertices, 3);
      fullMatrix<double> xN(numVertices, 3);
      for (int k = 0; k < numVertices; ++k) {
        x0(k, 0) = stack[0].getVertex(k)->x();
        x0(k, 1) = stack[0].getVertex(k)->y();
        x0(k, 2) = stack[0].getVertex(k)->z();
        x1(k, 0) = stack[iFirst].getVertex(k)->x();
        x1(k, 1) = stack[iFirst].getVertex(k)->y();
        x1(k, 2) = stack[iFirst].getVertex(k)->z();
        xN(k, 0) = stack[iLast].getVertex(k)->x();
        xN(k, 1) = stack[iLast].getVertex(k)->y();
        xN(k, 2) = stack[iLast].getVertex(k)->z();
      }

      fullMatrix<double> x0linear(numVertices, 3);
      fullMatrix<double> x1linear(numVertices, 3);
      fullMatrix<double> xNlinear(numVertices, 3);
      _linearize(x0, x0linear);
      _linearize(x1, x1linear);
      _linearize(xN, xNlinear);
      delta0 = x0; delta0.add(x0linear, -1);
      delta1 = x1; delta1.add(x1linear, -1);
      deltaN = xN; deltaN.add(xNlinear, -1);
    }

    void _computeTerms(const fullMatrix<double> &delta0,
                       const fullMatrix<double> &delta1,
                       const fullMatrix<double> &deltaN, double eta1,
                       fullMatrix<double> terms[8])
    {
      fullMatrix<double> &term0 = terms[0];
      fullMatrix<double> &term1d10 = terms[1];
      fullMatrix<double> &term1d11 = terms[2];
      fullMatrix<double> &term1dN0 = terms[3];
      fullMatrix<double> &term1dN1 = terms[4];
      fullMatrix<double> &term20 = terms[5];
      fullMatrix<double> &term21 = terms[6];
      fullMatrix<double> &term22 = terms[7];

      const int numVertices = delta0.size1();

      fullMatrix<double> delta10 = delta1;
      delta10.add(delta0, -1);
      delta10.scale(1 / eta1);
      fullMatrix<double> deltaN0 = deltaN;
      deltaN0.add(delta0, -1);

      term0.resize(numVertices, 3);
      term1d10.resize(numVertices, 3);
      term1d11.resize(numVertices, 3);
      term1dN0.resize(numVertices, 3);
      term1dN1.resize(numVertices, 3);
      term20.resize(numVertices, 3);
      term21.resize(numVertices, 3);
      term22.resize(numVertices, 3);

      TFIData *tfiData = _getTFIData(TYPE_LIN, numVertices-1);

      term0.copy(delta0);
      tfiData->T0.mult(delta10, term1d10);
      tfiData->T1.mult(delta10, term1d11);
      tfiData->T0.mult(deltaN0, term1dN0);
      tfiData->T1.mult(deltaN0, term1dN1);
      fullMatrix<double> diff(numVertices, 3);
      fullMatrix<double> dum0(numVertices, 3);
      fullMatrix<double> dum1(numVertices, 3);
      diff.copy(deltaN0);
      diff.add(delta10, -1);
      tfiData->T0.mult(diff, dum0);
      tfiData->T1.mult(diff, dum1);
      tfiData->T0.mult(dum0, term20);
      tfiData->T1.mult(dum0, term21);
      tfiData->T1.mult(dum1, term22);
    }

    void _generalTFI(std::vector<MEdgeN> &stack, int iLast,
                     const std::vector<std::pair<double, double> > &eta,
                     const fullMatrix<double> terms[8],
                     double coeffHermite, const GFace *gface)
    {
      // Let L() be the linear TFI transformation
      // Let H() be the semi-Hermite TFI transformation
      // This function return (1-coeffHermite) * L() + coeffHermite * H()

      const fullMatrix<double> &term0 = terms[0];
      const fullMatrix<double> &term1d10 = terms[1];
      const fullMatrix<double> &term1d11 = terms[2];
      const fullMatrix<double> &term1dN0 = terms[3];
      const fullMatrix<double> &term1dN1 = terms[4];
      const fullMatrix<double> &term20 = terms[5];
      const fullMatrix<double> &term21 = terms[6];
      const fullMatrix<double> &term22 = terms[7];

      int numVertices = stack[0].getNumVertices();

      for (unsigned int i = 1; i < stack.size(); ++i) {
        if (i == iLast) continue;
        // we want to change stack[iFirst] but not stack[iLast]

        fullMatrix<double> x(numVertices, 3);
        for (int j = 0; j < numVertices; ++j) {
          MVertex *v = stack[i].getVertex(j);
          x(j, 0) = v->x();
          x(j, 1) = v->y();
          x(j, 2) = v->z();
        }
        _linearize(x, x);

        double &c = coeffHermite;
        x.axpy(term0);
        x.axpy(term1d10, c * eta[i].first);
        x.axpy(term1d11, c * eta[i].second);
        x.axpy(term1dN0, (1-c) * eta[i].first);
        x.axpy(term1dN1, (1-c) * eta[i].second);
        x.axpy(term20, c * eta[i].first * eta[i].first);
        x.axpy(term21, c * 2*eta[i].first * eta[i].second);
        x.axpy(term22, c * eta[i].second * eta[i].second);

        for (int j = 2; j < numVertices; ++j) {
          MVertex *v = stack[i].getVertex(j);
          v->x() = x(j, 0);
          v->y() = x(j, 1);
          v->z() = x(j, 2);
        }
        if (gface) projectVerticesIntoGFace(&stack[i], gface, false);
      }
    }

    void _computeEtaAndTerms(std::vector<MEdgeN> &stack, int iFirst, int iLast,
                             std::vector<std::pair<double, double> > &eta,
                             fullMatrix<double> terms[8])
    {
      // Compute eta_i^k, k=0,1
      _computeEtas(stack, eta);

      // Precompute Delta(x_i), i=0,1,n
      fullMatrix<double> delta0, delta1, deltaN;
      _computeDeltaForTFI(stack, iFirst, iLast, delta0, delta1, deltaN);

      // Compute terms
      double eta1 = .5 * (eta[iFirst].first + eta[iFirst].second);
      _computeTerms(delta0, delta1, deltaN, eta1, terms);
    }

    void curveEdges(std::vector<MEdgeN> &stack, int iFirst, int iLast,
                    const GFace *gface)
    {
      std::vector<std::pair<double, double> > eta;
      fullMatrix<double> terms[8];
      _computeEtaAndTerms(stack, iFirst, iLast, eta, terms);

      _generalTFI(stack, iLast, eta, terms, 1, gface);
    }

    void curveEdgesAndPreserveQuality(std::vector<MEdgeN> &stackEdges,
                                      std::vector<MFaceN> &stackFaces,
                                      std::vector<MElement*> &stackElements,
                                      int iFirst, int iLast, const GFace *gface)
    {
      std::vector<std::pair<double, double> > eta;
      fullMatrix<double> terms[8];
      _computeEtaAndTerms(stackEdges, iFirst, iLast, eta, terms);

      // Compute quality of primary elements
      unsigned long numElements = stackElements.size() - 1;
      std::vector<double> qualitiesLinear(numElements);
      for (unsigned int i = 0; i < numElements; ++i) {
        MElement *linear = createPrimaryElement(stackElements[i]);
        qualitiesLinear[i] = jacobianBasedQuality::minIGEMeasure(linear);
        delete linear;
      }

      static double coeffHermite[11] = {1, .9, .8, .7, .6, .5, .4, .3, .2, .1, 0};
      for (int i = 0; i < 11; ++i) {
        _generalTFI(stackEdges, iLast, eta, terms, coeffHermite[i], gface);
        repositionInnerVertices(stackFaces, gface);

        bool allOk = true;
        if (coeffHermite[i]) {
          for (unsigned int i = 0; i < numElements; ++i) {
            double qual = jacobianBasedQuality::minIGEMeasure(stackElements[i]);
            if (qual < .5 && qual < .8 * qualitiesLinear[i]) {
              allOk = false;
              break;
            }
          }
        }

        if (allOk) return;
      }
    }
  }

  MElement* createPrimaryElement(MElement *el)
  {
    int tagLinear = ElementType::getType(el->getType(), 1);
    std::vector<MVertex *> vertices;
    el->getVertices(vertices);
    MElementFactory f;
    return f.create(tagLinear, vertices, -1);
  }

  LeastSquareData* constructLeastSquareData(int typeElement, int order,
                                            int orderGauss)
  {
    // invM1 gives
    //     value of coefficients in Legendre basis
    //   + value of Lagrange multipliers
    // from
    //     Ij + value of function f at extremities
    // M2 gives
    //     Ij = integral of product function f with Legendre polynomial j
    //   + value of function f at extremities
    // from
    //     the values of function f at Gauss points
    //   + value of function f at extremities

    orthogonalBasis basis(typeElement, order);
    LeastSquareData *data = new LeastSquareData;

    if (typeElement == TYPE_LIN) {
      data->nbPoints = getNGQLPts(orderGauss);
      data->intPoints = getGQLPts(orderGauss);

      const int szSpace = order + 1;
      const int nGP = data->nbPoints;

      double *val = new double[szSpace];

      fullMatrix<double> M2(szSpace+2, nGP+2, true);
      {
        for (int j = 0; j < nGP; ++j) {
          basis.f(data->intPoints[j].pt[0], 0, 0, val);
          for (int i = 0; i < szSpace; ++i) {
            M2(i, j) = val[i] * data->intPoints[j].weight;
          }
        }
        M2(szSpace, nGP) = M2(szSpace+1, nGP+1) = 1;
      }

      fullMatrix<double> M1(szSpace+2, szSpace+2, true);
      {
        basis.integralfSquared(val);
        for (int k = 0; k < szSpace; ++k)
          M1(k, k) = val[k];

        basis.f(-1, 0, 0, val);
        for (int k = 0; k < szSpace; ++k)
          M1(szSpace, k) = M1(k, szSpace) = val[k];

        basis.f( 1, 0, 0, val);
        for (int k = 0; k < szSpace; ++k)
          M1(szSpace+1, k) = M1(k, szSpace+1) = val[k];
      }
      fullMatrix<double> invM1;
      M1.invert(invM1);

      fullMatrix<double> Leg2Lag(szSpace, szSpace, true);
      {
        int tagLine = ElementType::getType(TYPE_LIN, order);
        const nodalBasis *fs = BasisFactory::getNodalBasis(tagLine);
        const fullMatrix<double> &refNodes = fs->getReferenceNodes();
        for (int i = 0; i < szSpace; ++i) {
          basis.f(refNodes(i, 0), 0, 0, val);
          for (int j = 0; j < szSpace; ++j) {
            Leg2Lag(i, j) = val[j];
          }
        }
      }

      delete val;

      fullMatrix<double> tmp(szSpace+2, nGP+2, false);
      invM1.mult(M2, tmp);
      fullMatrix<double> tmp2(szSpace, nGP+2, false);
      tmp2.copy(tmp, 0, szSpace, 0, nGP+2, 0, 0);

      data->invA.resize(szSpace, nGP+2, false);
      Leg2Lag.mult(tmp2, data->invA);
      return data;
    }

    else if (typeElement == TYPE_QUA) {
      data->nbPoints = getNGQQPts(orderGauss);
      data->intPoints = getGQQPts(orderGauss);

      fullMatrix<double> refNodes = gmshGeneratePointsQuadrangle(order);

      const int szSpace = (order + 1) * (order + 1);
      const int nGP = data->nbPoints;
      const int nConstraint = 4 * order;

      double *val = new double[szSpace];

      fullMatrix<double> M2(szSpace+nConstraint, nGP+nConstraint, true);
      {
        for (int j = 0; j < nGP; ++j) {
          basis.f(data->intPoints[j].pt[0], data->intPoints[j].pt[1], 0, val);
          for (int i = 0; i < szSpace; ++i) {
            M2(i, j) = val[i] * data->intPoints[j].weight;
          }
        }
        for (int i = 0; i < nConstraint; ++i) {
          M2(szSpace+i, nGP+i) = 1;
        }
      }

      fullMatrix<double> M1(szSpace+nConstraint, szSpace+nConstraint, true);
      {
        basis.integralfSquared(val);
        for (int k = 0; k < szSpace; ++k)
          M1(k, k) = val[k];

        for (int i = 0; i < nConstraint; ++i) {
          basis.f(refNodes(i, 0), refNodes(i, 1), 0, val);
          for (int k = 0; k < szSpace; ++k) {
            M1(szSpace+i, k) = M1(k, szSpace+i) = val[k];
          }
        }
      }
      fullMatrix<double> invM1;
      M1.invert(invM1);

      fullMatrix<double> Leg2Lag(szSpace, szSpace, true);
      {
        for (int i = 0; i < szSpace; ++i) {
          basis.f(refNodes(i, 0), refNodes(i, 1), 0, val);
          for (int j = 0; j < szSpace; ++j) {
            Leg2Lag(i, j) = val[j];
          }
        }
      }

      delete val;

      fullMatrix<double> tmp(szSpace+nConstraint, nGP+nConstraint, false);
      invM1.mult(M2, tmp);
      fullMatrix<double> tmp2(szSpace, nGP+nConstraint, false);
      tmp2.copy(tmp, 0, szSpace, 0, nGP+nConstraint, 0, 0);

      data->invA.resize(szSpace, nGP+nConstraint, false);
      Leg2Lag.mult(tmp2, data->invA);
      return data;
    }

    else if (typeElement == TYPE_TRI) {
      data->nbPoints = getNGQTPts(orderGauss);
      data->intPoints = getGQTPts(orderGauss);

      fullMatrix<double> refNodes = gmshGeneratePointsTriangle(order);

      const int szSpace = (order + 1) * (order + 2) / 2;
      const int nGP = data->nbPoints;
      const int nConstraint = 3 * order;

      double *val = new double[szSpace];

      fullMatrix<double> M2(szSpace+nConstraint, nGP+nConstraint, true);
      {
        for (int j = 0; j < nGP; ++j) {
          basis.f(data->intPoints[j].pt[0], data->intPoints[j].pt[1], 0, val);
          for (int i = 0; i < szSpace; ++i) {
            M2(i, j) = val[i] * data->intPoints[j].weight;
          }
        }
        for (int i = 0; i < nConstraint; ++i) {
          M2(szSpace+i, nGP+i) = 1;
        }
      }

      fullMatrix<double> M1(szSpace+nConstraint, szSpace+nConstraint, true);
      {
        basis.integralfSquared(val);
        for (int k = 0; k < szSpace; ++k)
          M1(k, k) = val[k];

        for (int i = 0; i < nConstraint; ++i) {
          basis.f(refNodes(i, 0), refNodes(i, 1), 0, val);
          for (int k = 0; k < szSpace; ++k) {
            M1(szSpace+i, k) = M1(k, szSpace+i) = val[k];
          }
        }
      }
      fullMatrix<double> invM1;
      M1.invert(invM1);

      fullMatrix<double> Leg2Lag(szSpace, szSpace, true);
      {
        for (int i = 0; i < szSpace; ++i) {
          basis.f(refNodes(i, 0), refNodes(i, 1), 0, val);
          for (int j = 0; j < szSpace; ++j) {
            Leg2Lag(i, j) = val[j];
          }
        }
      }

      delete val;

      fullMatrix<double> tmp(szSpace+nConstraint, nGP+nConstraint, false);
      invM1.mult(M2, tmp);
      fullMatrix<double> tmp2(szSpace, nGP+nConstraint, false);
      tmp2.copy(tmp, 0, szSpace, 0, nGP+nConstraint, 0, 0);

      data->invA.resize(szSpace, nGP+nConstraint, false);
      Leg2Lag.mult(tmp2, data->invA);
      return data;
    }
  }

  LeastSquareData* getLeastSquareData(int typeElement, int order,
                                      int orderGauss)
  {
    TupleLeastSquareData typeOrder(typeElement, std::make_pair(order, orderGauss));
    std::map<TupleLeastSquareData, LeastSquareData*>::iterator it;
    it = leastSquareData.find(typeOrder);

    if (it != leastSquareData.end()) return it->second;

    LeastSquareData *data = constructLeastSquareData(typeElement, order,
                                                     orderGauss);
    leastSquareData[typeOrder] = data;
    return data;
  }

  bool computeCommonEdge(MElement *el1, MElement *el2, MEdge &e)
  {
    for (int i = 0; i < el1->getNumEdges(); ++i) {
      e = el1->getEdge(i);
      for (int j = 0; j < el2->getNumEdges(); ++j) {
        MEdge e2 = el2->getEdge(j);
        if (e == e2) return true;
      }
    }
    e = MEdge();
    return false;
  }

  void compute2DstackPrimaryVertices(const PairMElemVecMElem &column,
                                     std::vector<MVertex*> &stack)
  {
    MElement *bottomElement = column.first;
    const std::vector<MElement *> &stackElements = column.second;
    int numVertexPerLayer = 2;
    unsigned long numLayers = stackElements.size();
    stack.assign(numVertexPerLayer * numLayers, NULL);

    int k = 0;
    for (int i = 0; i < numVertexPerLayer; ++i) {
      stack[k++] = bottomElement->getVertex(i);
    }
    MEdge bottomEdge = bottomElement->getEdge(0);
    for (unsigned int i = 0; i < numLayers - 1; ++i) {
      MElement *currentElement = stackElements[i];
      MEdge topEdge;
      if (!computeCommonEdge(currentElement, stackElements[i+1], topEdge)) {
        Msg::Error("Did not find common edge");
      }

      // Eeach edge that is not the bottom edge nor the top edge links a bottom
      // node with the corresponding top node
      for (int j = 0; j < currentElement->getNumEdges(); ++j) {
        MEdge edge = currentElement->getEdge(j);
        if (edge == bottomEdge || edge == topEdge) continue;

        MVertex *vbot, *vtop;
        if (edge.getVertex(0) == bottomEdge.getVertex(0) ||
            edge.getVertex(0) == bottomEdge.getVertex(1)) {
          vbot = edge.getVertex(0);
          vtop = edge.getVertex(1);
        }
        else {
          vbot = edge.getVertex(1);
          vtop = edge.getVertex(0);
        }

        for (int l = k-numVertexPerLayer; l < k; ++l) {
          if (stack[l] == vbot) {
            stack[l+numVertexPerLayer] = vtop;
          }
        }
      }

      // If there remains NULL values, it is because the vertex is the same
      // on bottom face and top face.
      for (int l = k; l < k+numVertexPerLayer; ++l) {
        if (stack[l] == NULL) {
          stack[l] = stack[l-numVertexPerLayer];
        }
      }

      k += numVertexPerLayer;
      bottomEdge = topEdge;
    }
  }

  void computeStackHOEdgesFaces(const PairMElemVecMElem &column,
                                std::vector<MEdgeN> &stackEdges,
                                std::vector<MFaceN> &stackFaces)
  {
    const std::vector<MElement *> &stackElements = column.second;
    const int numElements = (int)stackElements.size();
    stackEdges.resize(numElements);
    stackFaces.resize(numElements);

    std::vector<MVertex*> primVert;
    compute2DstackPrimaryVertices(column, primVert);

    for (unsigned int i = 0; i < numElements; ++i) {
      MEdge e(primVert[2*i+0], primVert[2*i+1]);
      stackEdges[i] = stackElements[i]->getHighOrderEdge(e);
    }
    for (unsigned int i = 0; i < numElements - 1; ++i) {
      MFace face;
      if (primVert[2*i+0] == primVert[2*i+2])
        face = MFace(primVert[2*i+1], primVert[2*i+0], primVert[2*i+3]);
      else if (primVert[2*i+1] == primVert[2*i+3])
        face = MFace(primVert[2*i+0], primVert[2*i+1], primVert[2*i+2]);
      else
        face = MFace(primVert[2*i+0], primVert[2*i+1], primVert[2*i+3],
                   primVert[2*i+2]);
      stackFaces[i] = stackElements[i]->getHighOrderFace(face);
    }
    // We don't care about the orientation of the last element
    stackFaces.back() = stackElements.back()->getHighOrderFace(0, 0, 0);
  }

  bool edgesShareVertex(MEdgeN *e0, MEdgeN *e1)
  {
    MVertex *v = e0->getVertex(0);
    MVertex *v0 = e1->getVertex(0);
    if (v == v0) return true;
    MVertex *v1 = e1->getVertex(1);
    if (v == v1) return true;
    v = e0->getVertex(1);
    if (v == v0) return true;
    if (v == v1) return true;
    return false;
  }

  void repositionInnerVertices(const std::vector<MFaceN> &stackFaces,
                               const GFace *gface)
  {
    if (stackFaces.empty()) return;

    int order = stackFaces[0].getPolynomialOrder();
    const fullMatrix<double> *placementTri, *placementQua, *placement;

    placementTri = InnerVertPlacementMatrices::triangle(order, true);
    placementQua = InnerVertPlacementMatrices::quadrangle(order, true);

    for (unsigned int i = 0; i < stackFaces.size() - 1; ++i) {
      const MFaceN &face = stackFaces[i];
      if (face.getType() == TYPE_QUA)
        face.repositionInnerVertices(placementQua);
      else
        face.repositionInnerVertices(placementTri);
      if (gface) projectVerticesIntoGFace(&face, gface, false);
    }

    if (stackFaces.back().getType() == TYPE_QUA) {
      placement = InnerVertPlacementMatrices::quadrangle(order, false);
    }
    else {
      placement = InnerVertPlacementMatrices::triangle(order, false);
    }
    stackFaces.back().repositionInnerVertices(placement);
    if (gface) projectVerticesIntoGFace(&stackFaces.back(), gface, false);
  }

  bool curve2Dcolumn(PairMElemVecMElem &column, const GFace *gface,
                     const GEdge *gedge, const SVector3 &normal)
  {
    // Here, either gface is defined and not normal, or the normal
    // is defined and not gface!

    if (column.second.size() < 2) return true;

    // Compute stack high order edges and faces
    std::vector<MEdgeN> stackEdges;
    std::vector<MFaceN> stackFaces;
    computeStackHOEdgesFaces(column, stackEdges, stackFaces);

    // Curve topEdge of first element and last edge
    int iFirst = 1, iLast = (int)stackEdges.size() - 1;
    MEdgeN *baseEdge = &stackEdges[0];
    MEdgeN *firstEdge = &stackEdges[iFirst];
    if (edgesShareVertex(baseEdge, firstEdge)) {
      iFirst = 2;
      firstEdge = &stackEdges[iFirst];
    }
    MEdgeN *topEdge = &stackEdges[iLast];

    EdgeCurver2D::curveEdge(baseEdge, firstEdge, gface, gedge, normal);
    EdgeCurver2D::curveEdge(baseEdge, topEdge, gface, gedge, normal);
    EdgeCurver2D::recoverQualityElements(stackEdges, stackFaces, column.second,
                                         iFirst, iLast, gface);

    // Curve interior edges and inner vertices
    InteriorEdgeCurver::curveEdgesAndPreserveQuality(stackEdges, stackFaces,
                                                     column.second, iFirst,
                                                     iLast, gface);
    return true;
  }

  void computeThicknessQuality(std::vector<MVertex *> &bottomVertices,
                               std::vector<MVertex *> &topVertices,
                               std::vector<double> &thickness,
                               SVector3 &w)
  {
    int nVertices = (int)bottomVertices.size();
    int tagLine = ElementType::getType(TYPE_LIN, nVertices-1);
    const nodalBasis *fs = BasisFactory::getNodalBasis(tagLine);

    for (int i = 0; i < nVertices; ++i) {
      const MVertex *v0 = bottomVertices[i];
      const MVertex *v1 = topVertices[i];
      SVector3 t, n, h;
      h = SVector3(v1->x()-v0->x(), v1->y()-v0->y(), v1->z()-v0->z());

      double xi = fs->points(i, 0);
      double xc = 0, yc = 0, zc = 0;
      double dx = 0, dy = 0, dz = 0;
      {
        double f[100];
        double sf[100][3];
        fs->f(xi, 0, 0, f);
        fs->df(xi, 0, 0, sf);
        for (int j = 0; j < fs->getNumShapeFunctions(); j++) {
          const MVertex *v = bottomVertices[j];
          xc += f[j] * v->x();
          yc += f[j] * v->y();
          zc += f[j] * v->z();
          dx += sf[j][0] * v->x();
          dy += sf[j][0] * v->y();
          dz += sf[j][0] * v->z();
        }
      }
      t = SVector3(dx, dy, dz).unit();
      n = crossprod(w, t);
      thickness.push_back(dot(h, n));
    }

    double t0 = thickness[0];
    double t1 = thickness[1];
    thickness[0] = 1;
    thickness[1] = 1;
    for (int j = 2; j < nVertices; ++j) {
      double fact = ((double)j-1)/(nVertices-1);
      double idealThickness = (1-fact) * t0 + fact * t1;
      int sign = gmsh_sign(idealThickness);
      if (sign * thickness[j] < 0)
        thickness[j] = 0;
      else if (sign*thickness[j] < sign*idealThickness)
        thickness[j] = thickness[j] / idealThickness;
      else
        thickness[j] = idealThickness / thickness[j];
    }
  }

  void computeThicknessPView(MElement *el, SVector3 &w,
                             std::map<int, std::vector<double> > &data)
  {
//    if (el->getType() == TYPE_QUA) {
//      std::vector<MVertex *> bottomVertices, topVertices;
//
//      el->getEdgeVertices(0, bottomVertices);
//      el->getEdgeVertices(2, topVertices);
//      std::reverse(topVertices.begin(), topVertices.begin() + 2);
//      std::reverse(topVertices.begin() + 2, topVertices.end());
//
//      std::vector<double> thickness[2];
//      computeThicknessQuality(bottomVertices, topVertices, thickness[0], w);
//      computeThicknessQuality(topVertices, bottomVertices, thickness[1], w);
//
//      std::map<int, double> v2q;
//      v2q[0] = thickness[0][0];
//      v2q[1] = thickness[0][1];
//      v2q[2] = thickness[1][1];
//      v2q[3] = thickness[1][0];
//      int nEdgeVertex = (int)topVertices.size()-2;
//      for (int i = 2; i < (int)thickness[0].size(); ++i) {
//        v2q[4+i-2] = thickness[0][i];
//        v2q[3+3*nEdgeVertex-i+2] = thickness[1][i];
//      }
//
//      double q00 = v2q[0];
//      double q10 = v2q[1];
//      double q11 = v2q[2];
//      double q01 = v2q[3];
//      for (int i = 0; i < nEdgeVertex; ++i) {
//        double fact = ((double)i+1)/(nEdgeVertex+1);
//        v2q[4+nEdgeVertex+i]   = (1-fact) * q10 + fact * q11;
//        v2q[4+3*nEdgeVertex+i] = (1-fact) * q01 + fact * q00;
//      }
//
//      int tag = el->getTypeForMSH();
//      InteriorPlacementData *pData;
//      std::map<int, InteriorPlacementData*>::iterator it;
//      it = interiorPlacementData.find(tag);
//      if (it != interiorPlacementData.end()) pData = it->second;
//      else {
//        pData = constructInteriorPlacementData(tag);
//        interiorPlacementData[tag] = pData;
//      }
//      for (int i = 0; i < pData->iToMove.size(); ++i) {
//        int idx = pData->iToMove[i];
//        double q0 = v2q[pData->i0[i]];
//        double q1 = v2q[pData->i1[i]];
//        v2q[idx] = pData->factor[i] * q0 + (1-pData->factor[i]) * q1;
//      }
//
//      std::vector<double> &vData = data[el->getNum()];
//      std::map<int, double>::iterator it2;
//      for (it2 = v2q.begin(); it2 != v2q.end(); ++it2) {
//        vData.push_back(it2->second);
//      }
//    }
  }
}


void curve2DBoundaryLayer(VecPairMElemVecMElem &bndEl2column, SVector3 normal,
                          const GEdge *gedge)
{
  double length = normal.normalize();
  if (length == 0) {
    Msg::Error("normal must be non-zero for boundary layer curving");
    return;
  }

//  for (int i = 0; i < bndEl2column.size(); ++i) {
//    bndEl2column[i].first->setVisibility(1);
//    for (unsigned int j = 0; j < bndEl2column[i].second.size(); ++j) {
//      bndEl2column[i].second[j]->setVisibility(1);
//    }
//  }

  for (int i = 0; i < bndEl2column.size(); ++i) {
//    if (bndEl2column[i].first->getNum() != 205) continue; // t161
//    if (bndEl2column[i].first->getNum() != 316) continue; // t161
//    if (bndEl2column[i].first->getNum() != 1156) continue; // trimesh
//    if (   bndEl2column[i].first->getNum() != 1156
//        && bndEl2column[i].first->getNum() != 1079
//        && bndEl2column[i].first->getNum() != 1102
//        && bndEl2column[i].first->getNum() != 1119) continue;
//    std::cout << std::endl;
//    std::cout << "column " << bndEl2column[i].first->getNum() << std::endl;
//    if (bndEl2column[i].first->getNum() != 1079) continue; // Good
//    if (bndEl2column[i].first->getNum() != 1078) continue; // Next to good
//    if (bndEl2column[i].first->getNum() != 1099) continue; // Long on corner
//    if (bndEl2column[i].first->getNum() != 1102) continue; // Bad HO
//    if (bndEl2column[i].first->getNum() != 1136) continue; // Bad linear
//    if (bndEl2column[i].first->getNum() != 1149) continue; // shorter
//    if (bndEl2column[i].first->getNum() != 1150) continue; // concave
//    if (bndEl2column[i].first->getNum() != 1151) continue; // symetric of concave
//    if (bndEl2column[i].first->getNum() != 1156) continue; // Strange
//    if (bndEl2column[i].first->getNum() != 1157) continue; // next to Strange
    BoundaryLayerCurver::curve2Dcolumn(bndEl2column[i], NULL, gedge, normal);
  }
}


void curve2DBoundaryLayer(VecPairMElemVecMElem &bndEl2column,
                          const GFace *gface, const GEdge *gedge)
{
  if (!gface || !gedge) {
    Msg::Error("both gface and gedge are needed for boundary layer curving "
               "(%d, %d)", gface, gedge);
    return;
  }

//  for (int i = 0; i < bndEl2column.size(); ++i) {
//    bndEl2column[i].first->setVisibility(1);
//    for (unsigned int j = 0; j < bndEl2column[i].second.size(); ++j) {
//      bndEl2column[i].second[j]->setVisibility(1);
//    }
//  }

  for (int i = 0; i < bndEl2column.size(); ++i)
    BoundaryLayerCurver::curve2Dcolumn(bndEl2column[i], gface, gedge,
                                       SVector3());
}
