// 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>.


#include "GmshDefines.h"
#include "GmshMessage.h"
#include <vector>
#include "polynomialBasis.h"
#include "pyramidalBasis.h"
#include "pointsGenerators.h"
#include "BasisFactory.h"
#include "Numeric.h"

// Sub Control Points
static std::vector< fullMatrix<double> > generateSubPointsLine(int order)
{
  std::vector< fullMatrix<double> > subPoints(2);

  subPoints[0] = gmshGenerateMonomialsLine(order);
  subPoints[0].scale(.5/order);

  subPoints[1].copy(subPoints[0]);
  subPoints[1].add(.5);

  return subPoints;
}

static std::vector< fullMatrix<double> > generateSubPointsTriangle(int order)
{
  std::vector< fullMatrix<double> > subPoints(4);
  fullMatrix<double> prox;

  subPoints[0] = gmshGenerateMonomialsTriangle(order);
  subPoints[0].scale(.5/order);

  subPoints[1].copy(subPoints[0]);
  prox.setAsProxy(subPoints[1], 0, 1);
  prox.add(.5);

  subPoints[2].copy(subPoints[0]);
  prox.setAsProxy(subPoints[2], 1, 1);
  prox.add(.5);

  subPoints[3].copy(subPoints[0]);
  subPoints[3].scale(-1.);
  subPoints[3].add(.5);

  return subPoints;
}

static std::vector< fullMatrix<double> > generateSubPointsQuad(int order)
{
  std::vector< fullMatrix<double> > subPoints(4);
  fullMatrix<double> prox;

  subPoints[0] = gmshGenerateMonomialsQuadrangle(order);
  subPoints[0].scale(.5/order);

  subPoints[1].copy(subPoints[0]);
  prox.setAsProxy(subPoints[1], 0, 1);
  prox.add(.5);

  subPoints[2].copy(subPoints[0]);
  prox.setAsProxy(subPoints[2], 1, 1);
  prox.add(.5);

  subPoints[3].copy(subPoints[1]);
  prox.setAsProxy(subPoints[3], 1, 1);
  prox.add(.5);

  return subPoints;
}

static std::vector< fullMatrix<double> > generateSubPointsTetrahedron(int order)
{
  std::vector< fullMatrix<double> > subPoints(8);
  fullMatrix<double> prox1;
  fullMatrix<double> prox2;

  subPoints[0] = gmshGenerateMonomialsTetrahedron(order);
  subPoints[0].scale(.5/order);

  subPoints[1].copy(subPoints[0]);
  prox1.setAsProxy(subPoints[1], 0, 1);
  prox1.add(.5);

  subPoints[2].copy(subPoints[0]);
  prox1.setAsProxy(subPoints[2], 1, 1);
  prox1.add(.5);

  subPoints[3].copy(subPoints[0]);
  prox1.setAsProxy(subPoints[3], 2, 1);
  prox1.add(.5);

  // u := .5-u-w
  // v := .5-v-w
  // w := w
  subPoints[4].copy(subPoints[0]);
  prox1.setAsProxy(subPoints[4], 0, 2);
  prox1.scale(-1.);
  prox1.add(.5);
  prox1.setAsProxy(subPoints[4], 0, 1);
  prox2.setAsProxy(subPoints[4], 2, 1);
  prox1.add(prox2, -1.);
  prox1.setAsProxy(subPoints[4], 1, 1);
  prox1.add(prox2, -1.);

  // u := u
  // v := .5-v
  // w := w+v
  subPoints[5].copy(subPoints[0]);
  prox1.setAsProxy(subPoints[5], 2, 1);
  prox2.setAsProxy(subPoints[5], 1, 1);
  prox1.add(prox2);
  prox2.scale(-1.);
  prox2.add(.5);

  // u := .5-u
  // v := v
  // w := w+u
  subPoints[6].copy(subPoints[0]);
  prox1.setAsProxy(subPoints[6], 2, 1);
  prox2.setAsProxy(subPoints[6], 0, 1);
  prox1.add(prox2);
  prox2.scale(-1.);
  prox2.add(.5);

  // u := u+w
  // v := v+w
  // w := .5-w
  subPoints[7].copy(subPoints[0]);
  prox1.setAsProxy(subPoints[7], 0, 1);
  prox2.setAsProxy(subPoints[7], 2, 1);
  prox1.add(prox2);
  prox1.setAsProxy(subPoints[7], 1, 1);
  prox1.add(prox2);
  prox2.scale(-1.);
  prox2.add(.5);


  return subPoints;
}

static std::vector< fullMatrix<double> > generateSubPointsPrism(int order)
{
  std::vector< fullMatrix<double> > subPoints(8);
  fullMatrix<double> prox;

  subPoints[0] = gmshGenerateMonomialsPrism(order);
  subPoints[0].scale(.5/order);

  subPoints[1].copy(subPoints[0]);
  prox.setAsProxy(subPoints[1], 0, 1);
  prox.add(.5);

  subPoints[2].copy(subPoints[0]);
  prox.setAsProxy(subPoints[2], 1, 1);
  prox.add(.5);

  subPoints[3].copy(subPoints[0]);
  prox.setAsProxy(subPoints[3], 0, 2);
  prox.scale(-1.);
  prox.add(.5);

  subPoints[4].copy(subPoints[0]);
  prox.setAsProxy(subPoints[4], 2, 1);
  prox.add(.5);

  subPoints[5].copy(subPoints[1]);
  prox.setAsProxy(subPoints[5], 2, 1);
  prox.add(.5);

  subPoints[6].copy(subPoints[2]);
  prox.setAsProxy(subPoints[6], 2, 1);
  prox.add(.5);

  subPoints[7].copy(subPoints[3]);
  prox.setAsProxy(subPoints[7], 2, 1);
  prox.add(.5);

  return subPoints;
}

static std::vector< fullMatrix<double> > generateSubPointsHex(int order)
{
  std::vector< fullMatrix<double> > subPoints(8);
  fullMatrix<double> prox;

  subPoints[0] = gmshGenerateMonomialsHexahedron(order);
  subPoints[0].scale(.5/order);

  subPoints[1].copy(subPoints[0]);
  prox.setAsProxy(subPoints[1], 0, 1);
  prox.add(.5);

  subPoints[2].copy(subPoints[0]);
  prox.setAsProxy(subPoints[2], 1, 1);
  prox.add(.5);

  subPoints[3].copy(subPoints[1]);
  prox.setAsProxy(subPoints[3], 1, 1);
  prox.add(.5);

  subPoints[4].copy(subPoints[0]);
  prox.setAsProxy(subPoints[4], 2, 1);
  prox.add(.5);

  subPoints[5].copy(subPoints[1]);
  prox.setAsProxy(subPoints[5], 2, 1);
  prox.add(.5);

  subPoints[6].copy(subPoints[2]);
  prox.setAsProxy(subPoints[6], 2, 1);
  prox.add(.5);

  subPoints[7].copy(subPoints[3]);
  prox.setAsProxy(subPoints[7], 2, 1);
  prox.add(.5);

  return subPoints;
}

static std::vector< fullMatrix<double> > generateSubPointsPyr(int order)
{
  if (order == 0) {
    std::vector< fullMatrix<double> > subPoints(4);
    fullMatrix<double> prox;

    subPoints[0] = JacobianBasis::generateJacMonomialsPyramid(order);
    subPoints[0].scale(.5/(order+2));

    subPoints[1].copy(subPoints[0]);
    prox.setAsProxy(subPoints[1], 0, 1);
    prox.add(.5);

    subPoints[2].copy(subPoints[0]);
    prox.setAsProxy(subPoints[2], 1, 1);
    prox.add(.5);

    subPoints[3].copy(subPoints[1]);
    prox.setAsProxy(subPoints[3], 1, 1);
    prox.add(.5);

    return subPoints;
  }

  std::vector< fullMatrix<double> > subPoints(8);
  fullMatrix<double> ref, prox;

  subPoints[0] = JacobianBasis::generateJacMonomialsPyramid(order);
  int nPts = subPoints[0].size1();
  for (int i = 0; i < nPts; ++i) {
    const double factor = .5 / (order + 2 - subPoints[0](i, 2));
    subPoints[0](i, 0) = subPoints[0](i, 0) * factor;
    subPoints[0](i, 1) = subPoints[0](i, 1) * factor;
    subPoints[0](i, 2) = subPoints[0](i, 2) * .5 / (order + 2);
  }

  subPoints[1].copy(subPoints[0]);
  prox.setAsProxy(subPoints[1], 0, 1);
  prox.add(.5);

  subPoints[2].copy(subPoints[0]);
  prox.setAsProxy(subPoints[2], 1, 1);
  prox.add(.5);

  subPoints[3].copy(subPoints[1]);
  prox.setAsProxy(subPoints[3], 1, 1);
  prox.add(.5);

  subPoints[4].copy(subPoints[0]);
  prox.setAsProxy(subPoints[4], 2, 1);
  prox.add(.5);

  subPoints[5].copy(subPoints[1]);
  prox.setAsProxy(subPoints[5], 2, 1);
  prox.add(.5);

  subPoints[6].copy(subPoints[2]);
  prox.setAsProxy(subPoints[6], 2, 1);
  prox.add(.5);

  subPoints[7].copy(subPoints[3]);
  prox.setAsProxy(subPoints[7], 2, 1);
  prox.add(.5);

  for (int i = 0; i < 8; ++i) {
    for (int j = 0; j < nPts; ++j) {
      const double factor = (1. - subPoints[i](j, 2));
      subPoints[i](j, 0) = subPoints[i](j, 0) * factor;
      subPoints[i](j, 1) = subPoints[i](j, 1) * factor;
    }
  }

  return subPoints;
}

// Matrices generation
static int nChoosek(int n, int k)
{
  if (n < k || k < 0) {
    Msg::Error("Wrong argument for combination.");
    return 1;
  }

  if (k > n/2) k = n-k;
  if (k == 1)
    return n;
  if (k == 0)
    return 1;

  int c = 1;
  for (int i = 1; i <= k; i++, n--) (c *= n) /= i;
  return c;
}

static fullMatrix<double> generateBez2LagMatrix
  (const fullMatrix<double> &exponent, const fullMatrix<double> &point,
   int order, int dimSimplex)
{
  if(exponent.size1() != point.size1() || exponent.size2() != point.size2()){
    Msg::Fatal("Wrong sizes for bez2lag matrix generation %d %d -- %d %d",
      exponent.size1(),point.size1(),
      exponent.size2(),point.size2());
    return fullMatrix<double>(1, 1);
  }

  int ndofs = exponent.size1();
  int dim = exponent.size2();

  fullMatrix<double> bez2Lag(ndofs, ndofs);
  for (int i = 0; i < ndofs; i++) {
    for (int j = 0; j < ndofs; j++) {
      double dd = 1.;

      {
        double pointCompl = 1.;
        int exponentCompl = order;
        for (int k = 0; k < dimSimplex; k++) {
          dd *= nChoosek(exponentCompl, (int) exponent(i, k))
            * pow(point(j, k), exponent(i, k));
          pointCompl -= point(j, k);
          exponentCompl -= (int) exponent(i, k);
        }
        dd *= pow(pointCompl, exponentCompl);
      }

      for (int k = dimSimplex; k < dim; k++)
        dd *= nChoosek(order, (int) exponent(i, k))
            * pow(point(j, k), exponent(i, k))
            * pow(1. - point(j, k), order - exponent(i, k));

      bez2Lag(j, i) = dd;
    }
  }
  return bez2Lag;
}

static fullMatrix<double> generateBez2LagMatrixPyramid
  (const fullMatrix<double> &exponent, const fullMatrix<double> &point,
   int order)
{
  if(exponent.size1() != point.size1() || exponent.size2() != point.size2() ||
      exponent.size2() != 3){
    Msg::Fatal("Wrong sizes for bez2lag matrix generation %d %d -- %d %d",
      exponent.size1(),point.size1(),
      exponent.size2(),point.size2());
    return fullMatrix<double>(1, 1);
  }

  int ndofs = exponent.size1();

  fullMatrix<double> bez2Lag(ndofs, ndofs);
  for (int i = 0; i < ndofs; i++) {
    for (int j = 0; j < ndofs; j++) {
      bez2Lag(i, j) =
          nChoosek(order, exponent(j, 2))
          * pow(point(i, 2), exponent(j, 2)) //FIXME use pow_int
          * pow(1. - point(i, 2), order - exponent(j, 2));

      double p = order + 2 - exponent(j, 2);
      double denom = 1. - point(i, 2);
      bez2Lag(i, j) *=
            nChoosek(p, exponent(j, 0))
          * nChoosek(p, exponent(j, 1))
          * pow(point(i, 0) / denom, exponent(j, 0))
          * pow(point(i, 1) / denom, exponent(j, 1))
          * pow(1. - point(i, 0) / denom, p - exponent(j, 0))
          * pow(1. - point(i, 1) / denom, p - exponent(j, 1));
    }
  }
  return bez2Lag;
}

static fullMatrix<double> generateSubDivisor
  (const fullMatrix<double> &exponents, const std::vector< fullMatrix<double> > &subPoints,
   const fullMatrix<double> &lag2Bez, int order, int dimSimplex)
{
  if (exponents.size1() != lag2Bez.size1() || exponents.size1() != lag2Bez.size2()){
    Msg::Fatal("Wrong sizes for Bezier Divisor %d %d -- %d %d",
      exponents.size1(), lag2Bez.size1(),
      exponents.size1(), lag2Bez.size2());
    return fullMatrix<double>(1, 1);
  }

  int nbPts = lag2Bez.size1();
  int nbSubPts = nbPts * subPoints.size();

  fullMatrix<double> intermediate2(nbPts, nbPts);
  fullMatrix<double> subDivisor(nbSubPts, nbPts);

  for (unsigned int i = 0; i < subPoints.size(); i++) {
    fullMatrix<double> intermediate1 =
      generateBez2LagMatrix(exponents, subPoints[i], order, dimSimplex);
    lag2Bez.mult(intermediate1, intermediate2);
    subDivisor.copy(intermediate2, 0, nbPts, 0, nbPts, i*nbPts, 0);
  }
  return subDivisor;
}

static fullMatrix<double> generateSubDivisorPyramid
  (const fullMatrix<double> &exponents, const std::vector< fullMatrix<double> > &subPoints,
   const fullMatrix<double> &lag2Bez, int order)
{
  if (exponents.size1() != lag2Bez.size1() || exponents.size1() != lag2Bez.size2()){
    Msg::Fatal("Wrong sizes for Bezier Divisor %d %d -- %d %d",
      exponents.size1(), lag2Bez.size1(),
      exponents.size1(), lag2Bez.size2());
    return fullMatrix<double>(1, 1);
  }

  int nbPts = lag2Bez.size1();
  int nbSubPts = nbPts * subPoints.size();

  fullMatrix<double> intermediate2(nbPts, nbPts);
  fullMatrix<double> subDivisor(nbSubPts, nbPts);

  for (unsigned int i = 0; i < subPoints.size(); i++) {
    fullMatrix<double> intermediate1 =
      generateBez2LagMatrixPyramid(exponents, subPoints[i], order);
    lag2Bez.mult(intermediate1, intermediate2);
    subDivisor.copy(intermediate2, 0, nbPts, 0, nbPts, i*nbPts, 0);
  }
  return subDivisor;
}

void bezierBasis::_construct(int parentType, int p)
{
  order = p;
  fullMatrix<double> exponents;
  std::vector< fullMatrix<double> > subPoints;

  if (parentType == TYPE_PYR) {
    dim = 3;
    numLagCoeff = 8;
    exponents = JacobianBasis::generateJacMonomialsPyramid(order);
    subPoints = generateSubPointsPyr(order);

    numDivisions = static_cast<int>(subPoints.size());

    fullMatrix<double> bezierPoints = exponents;
    bezierPoints.scale(1. / (order + 2));

    matrixBez2Lag = generateBez2LagMatrixPyramid(exponents, bezierPoints, order);
    matrixBez2Lag.invert(matrixLag2Bez);
    subDivisor = generateSubDivisorPyramid(exponents, subPoints, matrixLag2Bez, order);
    return;
  }

  int dimSimplex;
  switch (parentType) {
    case TYPE_PNT :
      dim = 0;
      numLagCoeff = 1;
      dimSimplex = 0;
      exponents = gmshGenerateMonomialsLine(0);
      subPoints.push_back(gmshGeneratePointsLine(0));
      break;
    case TYPE_LIN : {
      dim = 1;
      numLagCoeff = 2;
      dimSimplex = 0;
      exponents = gmshGenerateMonomialsLine(order);
      subPoints = generateSubPointsLine(order);
      break;
    }
    case TYPE_TRI : {
      dim = 2;
      numLagCoeff = 3;
      dimSimplex = 2;
      exponents = gmshGenerateMonomialsTriangle(order);
      subPoints = generateSubPointsTriangle(order);
      break;
    }
    case TYPE_QUA : {
      dim = 2;
      numLagCoeff = 4;
      dimSimplex = 0;
      exponents = gmshGenerateMonomialsQuadrangle(order);
      subPoints = generateSubPointsQuad(order);
      break;
    }
    case TYPE_TET : {
      dim = 3;
      numLagCoeff = 4;
      dimSimplex = 3;
      exponents = gmshGenerateMonomialsTetrahedron(order);
      subPoints = generateSubPointsTetrahedron(order);
      break;
    }
    case TYPE_PRI : {
      dim = 3;
      numLagCoeff = 6;
      dimSimplex = 2;
      exponents = gmshGenerateMonomialsPrism(order);
      subPoints = generateSubPointsPrism(order);
      break;
    }
    case TYPE_HEX : {
      dim = 3;
      numLagCoeff = 8;
      dimSimplex = 0;
      exponents = gmshGenerateMonomialsHexahedron(order);
      subPoints = generateSubPointsHex(order);
      break;
    }
    default : {
      Msg::Error("Unknown function space of parentType %d : "
          "reverting to TET_1", parentType);
      dim = 3;
      order = 0;
      numLagCoeff = 4;
      dimSimplex = 3;
      exponents = gmshGenerateMonomialsTetrahedron(order);
      subPoints = generateSubPointsTetrahedron(order);
      break;
    }
  }
  numDivisions = static_cast<int>(subPoints.size());

  fullMatrix<double> bezierPoints = exponents;
  bezierPoints.scale(1./order);

  matrixBez2Lag = generateBez2LagMatrix(exponents, bezierPoints, order, dimSimplex);
  matrixBez2Lag.invert(matrixLag2Bez);
  subDivisor = generateSubDivisor(exponents, subPoints, matrixLag2Bez, order, dimSimplex);
}
