bool fullMatrix<double>::invert(fullMatrix<double> &result) const
{
  int M = size1(), N = size2(), lda = size1(), info;
  int *ipiv = new int[std::min(M, N)];
  if (result.size2() != M || result.size1() != N) {
    if (result._own_data || !result._data)
      result.resize(M,N,false);
    else
      Msg::Fatal("FullMatrix: Bad dimension, I cannot write in proxy");
  }
  result.setAll(*this);
  F77NAME(dgetrf)(&M, &N, result._data, &lda, ipiv, &info);
  if(info == 0){
    int lwork = M * 4;
    double *work = new double[lwork];
    F77NAME(dgetri)(&M, result._data, &lda, ipiv, work, &lwork, &info);
    delete [] work;
  }
  delete [] ipiv;
  if(info == 0) return true;
  else if(info > 0)
    Msg::Error("U(%d,%d)=0 in matrix inversion", info, info);
  else
    Msg::Error("Wrong %d-th argument in matrix inversion", -info);
  return false;
}

void LagMultTerm::get(MElement *ele, int npts, IntPt *GP,
                      fullMatrix<double> &m) const
{
  int nbFF1 = BilinearTerm<SVector3, SVector3>::space1.getNumKeys(
    ele); // nbVertices*nbcomp of parent
  int nbFF2 = BilinearTerm<SVector3, SVector3>::space2.getNumKeys(
    ele); // nbVertices of boundary
  double jac[3][3];
  m.resize(nbFF1, nbFF2);
  m.setAll(0.);
  for(int i = 0; i < npts; i++) {
    double u = GP[i].pt[0];
    double v = GP[i].pt[1];
    double w = GP[i].pt[2];
    const double weight = GP[i].weight;
    const double detJ = ele->getJacobian(u, v, w, jac);
    std::vector<TensorialTraits<SVector3>::ValType> Vals;
    std::vector<TensorialTraits<SVector3>::ValType> ValsT;
    BilinearTerm<SVector3, SVector3>::space1.f(ele, u, v, w, Vals);
    BilinearTerm<SVector3, SVector3>::space2.f(ele, u, v, w, ValsT);
    for(int j = 0; j < nbFF1; j++) {
      for(int k = 0; k < nbFF2; k++) {
        m(j, k) += _eqfac * dot(Vals[j], ValsT[k]) * weight * detJ;
      }
    }
  }
}

void fullMatrix<double>::multOnBlock(const fullMatrix<double> &b, const int ncol, const int fcol, const int alpha_, const int beta_, fullVector<double> &c) const
{
  int M = 1, N = ncol, K = b.size1() ;
  int LDA = _r, LDB = b.size1(), LDC = 1;
  double alpha = alpha_, beta = beta_;
  F77NAME(dgemm)("N", "N", &M, &N, &K, &alpha, _data, &LDA, &(b._data[fcol*K]), &LDB,
                 &beta, &(c._data[fcol]), &LDC);
}

void PViewFactory::setEntry (int id, const fullMatrix<double> &val)
{
  std::vector<double> &vv = _values[id];
  vv.resize(val.size1()*val.size2());
  int k=0;
  for (int i=0;i<val.size1(); i++) {
    for (int j=0;j<val.size2(); j++) {
      vv[k++] = val(i,j);
    }
  }
}

template<class T2> void BilinearTermContract<T2>::get(MElement *ele, int npts, IntPt *GP, fullMatrix<double> &m) const
{
  fullVector<T2> va;
  fullVector<T2> vb;
  a->get(ele,npts,GP,va);
  b->get(ele,npts,GP,vb);
  m.resize(va.size(), vb.size());
  m.setAll(0.);
  for (int i=0;i<va.size();++i)
    for (int j=0;j<vb.size();++j)
      m(i,j)=dot(va(i),vb(j));
}

bool fullMatrix<double>::svd(fullMatrix<double> &V, fullVector<double> &S)
{
  fullMatrix<double> VT(V.size2(), V.size1());
  int M = size1(), N = size2(), LDA = size1(), LDVT = VT.size1(), info;
  int lwork = std::max(3 * std::min(M, N) + std::max(M, N), 5 * std::min(M, N));
  fullVector<double> WORK(lwork);
  F77NAME(dgesvd)("O", "A", &M, &N, _data, &LDA, S._data, _data, &LDA,
                  VT._data, &LDVT, WORK._data, &lwork, &info);
  V = VT.transpose();
  if(info == 0) return true;
  if(info > 0)
    Msg::Error("SVD did not converge");
  else
    Msg::Error("Wrong %d-th argument in SVD decomposition", -info);
  return false;
}

void polynomialBasis::df(const fullMatrix<double> &coord,
                         fullMatrix<double> &dfm) const
{
  double dfv[1256][3];
  dfm.resize(coord.size1() * 3, coefficients.size1(), false);
  int dimCoord = coord.size2();
  for(int iPoint = 0; iPoint < coord.size1(); iPoint++) {
    df(coord(iPoint, 0), dimCoord > 1 ? coord(iPoint, 1) : 0.,
       dimCoord > 2 ? coord(iPoint, 2) : 0., dfv);
    for(int i = 0; i < coefficients.size1(); i++) {
      dfm(iPoint * 3 + 0, i) = dfv[i][0];
      dfm(iPoint * 3 + 1, i) = dfv[i][1];
      dfm(iPoint * 3 + 2, i) = dfv[i][2];
    }
  }
}

void polynomialBasis::f(const fullMatrix<double> &coord,
                        fullMatrix<double> &sf) const
{
  double p[1256];
  sf.resize(coord.size1(), coefficients.size1());
  for(int iPoint = 0; iPoint < coord.size1(); iPoint++) {
    evaluateMonomials(coord(iPoint, 0),
                      coord.size2() > 1 ? coord(iPoint, 1) : 0,
                      coord.size2() > 2 ? coord(iPoint, 2) : 0, p);
    for(int i = 0; i < coefficients.size1(); i++) {
      sf(iPoint, i) = 0.;
      for(int j = 0; j < coefficients.size2(); j++)
        sf(iPoint, i) += coefficients(i, j) * p[j];
    }
  }
}

void epsRRod(fullVector<double>& xyz, fullMatrix<Complex>& epsR){
  epsR.scale(0);

  epsR(0, 0) = Complex(epsRRodRe, epsRRodIm);
  epsR(1, 1) = Complex(epsRRodRe, epsRRodIm);
  epsR(2, 2) = Complex(epsRRodRe, epsRRodIm);
}

void nuRRod(fullVector<double>& xyz, fullMatrix<Complex>& nuR){
  nuR.scale(0);

  nuR(0, 0) = 1;
  nuR(1, 1) = 1;
  nuR(2, 2) = 1;
}

void BilinearTermBase::get(MElement *ele, int npts, IntPt *GP, fullMatrix<double> &m) const
{
  std::vector<fullMatrix<double> > mv(npts);
  get(ele,npts,GP,mv);
  m.resize(mv[0].size1(), mv[0].size2());
  m.setAll(0.);
  double jac[3][3];
  for (int k=0;k<npts;k++)
  {
    const double u = GP[k].pt[0]; const double v = GP[k].pt[1]; const double w = GP[k].pt[2];
    const double weight = GP[k].weight; const double detJ = ele->getJacobian(u, v, w, jac);
    const double coeff=weight*detJ;
    for (int i=0;i<mv[k].size1();++i)
      for (int j=0;j<mv[k].size2();++j)
        m(i,j)+=mv[k](i,j)*coeff;
  }
}

void ana(double (*f)(fullVector<double>& xyz),
         const fullMatrix<double>& point,
         fullMatrix<double>& eval){
  // Alloc eval for Scalar Values //
  const size_t nPoint = point.size1();
  eval.resize(nPoint, 1);

  // Loop on point and evaluate f //
  fullVector<double> xyz(3);
  for(size_t i = 0; i < nPoint; i++){
    xyz(0) = point(i, 0);
    xyz(1) = point(i, 1);
    xyz(2) = point(i, 2);

    eval(i, 0) = f(xyz);
  }
}

double l2Norm(const fullMatrix<double>& valA, const fullMatrix<double>& valB){
  const size_t nPoint = valA.size1();
  const size_t    dim = valA.size2();

  double norm = 0;
  double modSquare = 0;

  for(size_t i = 0; i < nPoint; i++){
    modSquare = 0;

    for(size_t j = 0; j < dim; j++)
      modSquare += (valA(i, j) - valB(i, j)) * (valA(i, j) - valB(i, j));

    norm += modSquare;
  }

  return norm = sqrt(norm);
}

void pyramidalBasis::f(const fullMatrix<double> &coord, fullMatrix<double> &sf) const
{

  const int N = bergot->size(), NPts = coord.size1();

  sf.resize(NPts, N);
  double *fval = new double[N];

  for (int iPt=0; iPt<NPts; iPt++) {
    bergot->f(coord(iPt,0), coord(iPt,1), coord(iPt,2), fval);
    for (int i = 0; i < N; i++) {
      sf(iPt,i) = 0.;
      for (int j = 0; j < N; j++) sf(iPt,i) += coefficients(i,j)*fval[j];
    }
  }

  delete[] fval;

}

void Quadrature::point(fullMatrix<double>& gC,
                       fullVector<double>& gW){
  gW.resize(1);
  gC.resize(1, 1);

  gW(0)    = 1;
  gC(0, 0) = 0;
  gC(0, 1) = 0;
  gC(0, 2) = 0;
}

void solve( const fullMatrix & A, 
              fullMatrix& X, 
              const fullMatrix& Y )
{ // find X s.t. A X = Y using Gausian Elimination with
  // row pivoting -- no scaling this version

  if( A.row != A.col ){
      std::cerr << "solve only works for square matrices\n";
      return;
  }

  int i, j, k, n = A.col, m=Y.col;
  double s, pa;

  // initialize X to Y and copy A
  X = Y;
  fullMatrix  B(A);
  // could scale here

  // next do forward elimination and forward solution
  for( i=0; i<n; i++ ){ // clean up the i-th column

    // find the pivot
    k = i;  // row with the pivot
    pa = std::abs(B(i,i));
    for( j=i+1; j<n; j++ )
      if( std::abs(B(j,i)) > pa ){
        pa = std::abs(B(j,i));
        k = j;
      }
    if( pa == 0.0 )
      error( "solve: singular matrix");
    B.swapRows(i,k);
    X.swapRows(i,k); // the pivot is now in B(i,i)

    for( j=i+1; j<n; j++ ){ // for each row below row i
      s = B(j,i)/B(i,i);
      for( k=i+1; k<n; k++ ) // modifications that make B(j,i) zero
        B(j,k) -= s*B(i,k);
      for( k=0; k<m; k++ )   // change the rhs too
        X(j,k) -= s*X(i,k);
    }
  }
  // now B is upper triangular

  for( i=n-1; i>=0; i-- ){ // do back solution
    for(j = i+1; j<n; j++ ){
      for( k=0; k<m; k++ )
        X(i,k) -= B(i,j)*X(j,k);
    }
    for( k=0; k<m; k++ )
      X(i,k) /= B(i,i);
  }
}

void pyramidalBasis::df(const fullMatrix<double> &coord, fullMatrix<double> &dfm) const
{

  const int N = bergot->size(), NPts = coord.size1();

  double (*dfv)[3] = new double[N][3];
  dfm.resize (N, 3*NPts, false);

  for (int iPt=0; iPt<NPts; iPt++) {
    df(coord(iPt,0), coord(iPt,1), coord(iPt,2), dfv);
    for (int i = 0; i < N; i++) {
      dfm(i, 3*iPt) = dfv[i][0];
      dfm(i, 3*iPt+1) = dfv[i][1];
      dfm(i, 3*iPt+2) = dfv[i][2];
    }
  }

  delete[] dfv;

}

template<class T1> void LaplaceTerm<T1, T1>::get(MElement *ele, int npts, IntPt *GP, fullMatrix<double> &m) const
{
  int nbFF = BilinearTerm<T1, T1>::space1.getNumKeys(ele);
  double jac[3][3];
  m.resize(nbFF, nbFF);
  m.setAll(0.);
  for(int i = 0; i < npts; i++){
    const double u = GP[i].pt[0]; const double v = GP[i].pt[1]; const double w = GP[i].pt[2];
    const double weight = GP[i].weight; const double detJ = ele->getJacobian(u, v, w, jac);
    std::vector<typename TensorialTraits<T1>::GradType> Grads;
    BilinearTerm<T1, T1>::space1.gradf(ele, u, v, w, Grads);
    for(int j = 0; j < nbFF; j++)
    {
      for(int k = j; k < nbFF; k++)
      {
        double contrib = weight * detJ * dot(Grads[j], Grads[k]) * diffusivity;
        m(j, k) += contrib;
        if(j != k) m(k, j) += contrib;
      }
    }
  }
}

double taylorDistanceSq1D(const GradientBasis *gb, const fullMatrix<double> &nodesXYZ,
                          const std::vector<SVector3> &tanCAD)
{
  const int nV = nodesXYZ.size1();
  fullMatrix<double> dxyzdX(nV, 3);
  gb->getGradientsFromNodes(nodesXYZ, &dxyzdX, 0, 0);
//  const double dx = nodesXYZ(1, 0) - nodesXYZ(0, 0), dy = nodesXYZ(1, 1) - nodesXYZ(0, 1),
//               dz = nodesXYZ(1, 2) - nodesXYZ(0, 2), h = 0.5*sqrt(dx*dx+dy*dy+dz*dz)/double(nV-1);
  double distSq = 0.;
  for (int i=0; i<nV; i++) {
    SVector3 tanMesh(dxyzdX(i, 0), dxyzdX(i, 1), dxyzdX(i, 2));
    const double h = 0.25*tanMesh.normalize();                                            // Half of "local edge length"
    SVector3 diff = (dot(tanCAD[i], tanMesh) > 0) ?
                    tanCAD[i] - tanMesh : tanCAD[i] + tanMesh;
    distSq += h*h*diff.normSq();
  }
  return distSq;
}

double taylorDistanceSq2D(const GradientBasis *gb, const fullMatrix<double> &nodesXYZ,
                          const std::vector<SVector3> &normCAD)
{
  const int nV = nodesXYZ.size1();
  fullMatrix<double> dxyzdX(nV, 3), dxyzdY(nV, 3);
  gb->getGradientsFromNodes(nodesXYZ, &dxyzdX, &dxyzdY, 0);
  double distSq = 0.;
  for (int i=0; i<nV; i++) {
    const double nz = dxyzdX(i, 0) * dxyzdY(i, 1) - dxyzdX(i, 1) * dxyzdY(i, 0);
    const double ny = -dxyzdX(i, 0) * dxyzdY(i, 2) + dxyzdX(i, 2) * dxyzdY(i, 0);
    const double nx = dxyzdX(i, 1) * dxyzdY(i, 2) - dxyzdX(i, 2) * dxyzdY(i, 1);
    SVector3 normMesh(nx, ny, nz);
    const double h = 0.25*sqrt(normMesh.normalize());                                     // Half of sqrt of "local area", to be adjusted w.r.t. el. type?
    SVector3 diff = (dot(normCAD[i], normMesh) > 0) ?
                    normCAD[i] - normMesh : normCAD[i] + normMesh;
    distSq += h*h*diff.normSq();
  }
  return distSq;
}