/*=========================================================================

  Program:   Visualization Toolkit
  Module:    vtkLagrangianTetra.cxx

  Copyright (c) Ken Martin, Will Schroeder, Bill Lorensen
  All rights reserved.
  See Copyright.txt or http://www.kitware.com/Copyright.htm for details.

     This software is distributed WITHOUT ANY WARRANTY; without even
     the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
     PURPOSE.  See the above copyright notice for more information.

=========================================================================*/
#include "vtkLagrangianTetra.h"

#include "vtkObjectFactory.h"
#include "vtkMath.h"
#include "vtkLine.h"
#include "vtkLagrangianTriangle.h"
#include "vtkTetra.h"
#include "vtkDoubleArray.h"
#include "vtkPoints.h"

vtkStandardNewMacro(vtkLagrangianTetra);
//----------------------------------------------------------------------------
vtkLagrangianTetra::vtkLagrangianTetra() :
  LegacyParametricCoordinates(NULL)
{
  this->Face = vtkLagrangianTriangle::New();
  this->Tetra = vtkTetra::New();
  this->Scalars = vtkDoubleArray::New();
  this->Scalars->SetNumberOfTuples(4);

  this->Points->SetNumberOfPoints(4);
  this->PointIds->SetNumberOfIds(4);
  for (int i = 0; i < 4; i++)
    {
    this->Points->SetPoint(i, 0.0, 0.0, 0.0);
    this->PointIds->SetId(i,0);
    }
}

//----------------------------------------------------------------------------
vtkLagrangianTetra::~vtkLagrangianTetra()
{
  delete this->LegacyParametricCoordinates;
  this->Face->Delete();
  this->Scalars->Delete();
}

//----------------------------------------------------------------------------
vtkCell *vtkLagrangianTetra::GetEdge(int edgeId)
{
  // TODO: Fill me in!
  return 0;
}

//----------------------------------------------------------------------------
vtkCell *vtkLagrangianTetra::GetFace(int faceId)
{
  assert(faceId >= 0 && faceId < 4);

  vtkIdType order = this->GetOrder();

  vtkIdType nPoints = (order + 1)*(order + 2)/2;
  this->Face->GetPointIds()->SetNumberOfIds(nPoints);
  this->Face->GetPoints()->SetNumberOfPoints(nPoints);

  static const vtkIdType faceBCoords[4][3] = {{0,1,3}, {0,2,3},
                                              {0,1,2}, {1,2,3}};
  // when faceMinCoord[faceId] == min, we are on face <faceId>
  static const vtkIdType faceMinCoord[4] = {2,1,3,0};

  vtkIdType tetBCoords[4], triBCoords[3];
  for (vtkIdType p = 0; p < nPoints; p++)
    {
    vtkLagrangianTriangle::BarycentricIndex(p,triBCoords,order);

    for (vtkIdType coord = 0; coord < 3; coord++)
      {
      tetBCoords[faceBCoords[faceId][coord]] = triBCoords[coord];
      }
    tetBCoords[faceMinCoord[faceId]] = 0;

    vtkIdType pointIndex = vtkLagrangianTetra::Index(tetBCoords, order);
    this->Face->GetPointIds()->SetId(p,pointIndex);
    this->Face->GetPoints()->SetPoint(p,this->Points->GetPoint(pointIndex));
    }

  return this->Face;
}

//----------------------------------------------------------------------------
void vtkLagrangianTetra::Initialize()
{

}

//----------------------------------------------------------------------------
vtkIdType vtkLagrangianTetra::NumberOfSubtetras()
{
  vtkIdType order = this->GetOrder();

  return order*(order+1)*(4*(order-1) + (order+2))/6;
}

//----------------------------------------------------------------------------
void vtkLagrangianTetra::SubtetraBarycentricPointIndices(
  vtkIdType cellIndex, vtkIdType (&pointBIndices)[4][4])
{
  // We tesselllate a tetrahedron into a tetrahedral-octahedral honeycomb, and
  // then discretize each octahedron into 4 tetrahedra. The pattern is as
  // follows: for each additional level in our tetrahedron (propagating
  // downwards in parametric z), a pattern of upside-down and rightside-up
  // triangles are formed. The rightside-up triangles form tetrahedra with the
  // single point above them, and the upside-down triangles form octahedra with
  // the righteside-up triangles above them.

  assert(cellIndex < this->NumberOfSubtetras());

  vtkIdType order = this->GetOrder();

  if (order == 1)
    {
    for (vtkIdType i=0;i<4;i++)
      {
      for (vtkIdType j=0;j<4;j++)
        {
        pointBIndices[i][j] = ((i + 3)%4 == j ? 1 : 0);
        }
      }
    return;
    }

  vtkIdType nRightSideUp = order*(order + 1)*(order + 2)/6;

  if (cellIndex < nRightSideUp)
    {
    // there are nRightSideUp subtetras whose orientation is the same as the
    // projected tetra. We traverse them here.
    vtkLagrangianTetra::BarycentricIndex(cellIndex, pointBIndices[0], order-1);

    pointBIndices[0][3] += 1;

    pointBIndices[1][0] = pointBIndices[0][0];
    pointBIndices[1][1] = pointBIndices[0][1] + 1;
    pointBIndices[1][2] = pointBIndices[0][2];
    pointBIndices[1][3] = pointBIndices[0][3] - 1;

    pointBIndices[2][0] = pointBIndices[0][0] + 1;
    pointBIndices[2][1] = pointBIndices[0][1];
    pointBIndices[2][2] = pointBIndices[0][2];
    pointBIndices[2][3] = pointBIndices[0][3] - 1;

    pointBIndices[3][0] = pointBIndices[0][0];
    pointBIndices[3][1] = pointBIndices[0][1];
    pointBIndices[3][2] = pointBIndices[0][2] + 1;
    pointBIndices[3][3] = pointBIndices[0][3] - 1;
    }
  else
    {
    // the remaining subtetras are embedded in octahedra, so we need to identify
    // and subdivide the octahedra. We traverse them here.
    cellIndex -= nRightSideUp;

    vtkIdType octIndex = cellIndex/4;
    vtkIdType tetIndex = cellIndex%4;

    vtkIdType octBIndices[6][4];

    vtkIdType ord[6] = {2,1,0,4,5,3};

    if (order == 2)
      {
      octBIndices[2][0] = octBIndices[2][1] = octBIndices[2][2] =
        octBIndices[2][3] = 0;
      }
    else
      {
      vtkLagrangianTetra::BarycentricIndex(octIndex, octBIndices[2], order-2);
      }
    octBIndices[2][1] += 1;
    octBIndices[2][3] += 1;

    octBIndices[1][0] = octBIndices[2][0] + 1;
    octBIndices[1][1] = octBIndices[2][1];
    octBIndices[1][2] = octBIndices[2][2];
    octBIndices[1][3] = octBIndices[2][3] - 1;

    octBIndices[0][0] = octBIndices[2][0] + 1;
    octBIndices[0][1] = octBIndices[2][1] - 1;
    octBIndices[0][2] = octBIndices[2][2];
    octBIndices[0][3] = octBIndices[2][3];

    octBIndices[3][0] = octBIndices[0][0] - 1;
    octBIndices[3][1] = octBIndices[0][1];
    octBIndices[3][2] = octBIndices[0][2] + 1;
    octBIndices[3][3] = octBIndices[0][3];

    octBIndices[4][0] = octBIndices[3][0] + 1;
    octBIndices[4][1] = octBIndices[3][1];
    octBIndices[4][2] = octBIndices[3][2];
    octBIndices[4][3] = octBIndices[3][3] - 1;

    octBIndices[5][0] = octBIndices[3][0];
    octBIndices[5][1] = octBIndices[3][1] + 1;
    octBIndices[5][2] = octBIndices[3][2];
    octBIndices[5][3] = octBIndices[3][3] - 1;

    return this->TetraFromOctahedron(tetIndex,octBIndices,pointBIndices);
    }
}
//----------------------------------------------------------------------------
void vtkLagrangianTetra::TetraFromOctahedron(
  vtkIdType cellIndex, const vtkIdType (&octBIndices)[6][4],
  vtkIdType (&tetraBIndices)[4][4])
{
  static int LinearTetras[3][4][4] =
    { { {2,0,1,4}, {2,1,5,4}, {2,5,3,4}, {2,3,0,4}},
      { {0,4,1,5}, {0,1,2,5}, {0,2,3,5}, {0,3,4,5}},
      { {1,5,2,3}, {1,2,0,3}, {1,0,4,3}, {1,4,5,3}} };

  // TODO: intelligently select which of the three linearizations to reduce
  // artifacts. For now, we always choose the first linearization.
  vtkIdType linearization = 0;

  for (vtkIdType i = 0; i < 4; i++)
    {
    for (vtkIdType j = 0; j < 4; j++)
      {
      tetraBIndices[i][j] =
        octBIndices[LinearTetras[linearization][cellIndex][i]][j];
      }
    }
}


//----------------------------------------------------------------------------
int vtkLagrangianTetra::CellBoundary(int subId, double pcoords[3],
                                     vtkIdList *pts)
{
  // TODO: Fill me in!
  return 0;
}

//----------------------------------------------------------------------------
int vtkLagrangianTetra::EvaluatePosition(double* x, double* closestPoint,
                                         int& subId, double pcoords[3],
                                         double& minDist2, double *weights)
{
  double pc[3], dist2, tempWeights[3], closest[3];
  int returnStatus=0, status, ignoreId;
  vtkIdType minBIndices[4][4], bindices[4][4], minIndices[4], pointIndices[4];

  vtkIdType order = this->GetOrder();
  vtkIdType numberOfSubtetras = this->NumberOfSubtetras();

  minDist2 = VTK_DOUBLE_MAX;
  for (vtkIdType subCellId = 0; subCellId < numberOfSubtetras; subCellId++)
    {
    this->SubtetraBarycentricPointIndices(subCellId, bindices);

    for (vtkIdType i=0;i<4;i++)
      {
      pointIndices[i] = this->ToIndex(bindices[i]);
      this->Face->Points->SetPoint(i, this->Points->GetPoint(pointIndices[i]));
      }

    status = this->Face->EvaluatePosition(x, closest, ignoreId, pc, dist2,
                                          tempWeights);

    if ( status != -1 && dist2 < minDist2 )
      {
      returnStatus = status;
      minDist2 = dist2;
      subId = subCellId;
      pcoords[0] = pc[0];
      pcoords[1] = pc[1];
      for (vtkIdType i=0;i<4;i++)
        {
        minIndices[i] = pointIndices[i];
        for (vtkIdType j=0;j<4;j++)
          {
          minBIndices[i][j] = bindices[i][j];
          }
        }
      }
    }

  // adjust parametric coordinates
  if ( returnStatus != -1 )
    {
      pcoords[0] = ((1. - pcoords[0])*minBIndices[0][0] +
                    pcoords[0]*minBIndices[1][0])/order;
      pcoords[1] = ((1. - pcoords[1])*minBIndices[0][1] +
                    pcoords[1]*minBIndices[1][1])/order;
      pcoords[2] = ((1. - pcoords[2])*minBIndices[0][2] +
                    pcoords[2]*minBIndices[1][2])/order;

    if(closestPoint!=0)
      {
      // Compute both closestPoint and weights
      this->EvaluateLocation(subId,pcoords,closestPoint,weights);
      }
    else
      {
      // Compute weights only
      this->InterpolateFunctions(pcoords,weights);
      }
    }

  return returnStatus;
}

//----------------------------------------------------------------------------
void vtkLagrangianTetra::EvaluateLocation(int& vtkNotUsed(subId),
                                             double pcoords[3],
                                             double x[3], double *weights)
{
  x[0] = x[1] = x[2] = 0.;

  this->InterpolateFunctions(pcoords,weights);

  double p[3];
  vtkIdType nPoints = this->GetPoints()->GetNumberOfPoints();
  for (vtkIdType idx = 0; idx < nPoints; idx++)
    {
    this->Points->GetPoint(idx,p);
    for (vtkIdType jdx = 0; jdx < 3; jdx++)
      {
      x[jdx] += p[jdx]*weights[idx];
      }
    }
}

//----------------------------------------------------------------------------
void vtkLagrangianTetra::Contour(double value,
                                 vtkDataArray* cellScalars,
                                 vtkIncrementalPointLocator* locator,
                                 vtkCellArray *verts,
                                 vtkCellArray* lines,
                                 vtkCellArray* polys,
                                 vtkPointData* inPd,
                                 vtkPointData* outPd,
                                 vtkCellData* inCd,
                                 vtkIdType cellId,
                                 vtkCellData* outCd)
{
  vtkIdType bindices[4][4];
  vtkIdType numberOfSubtetras = this->NumberOfSubtetras();

  for (vtkIdType subCellId = 0; subCellId < numberOfSubtetras; subCellId++)
    {
    this->SubtetraBarycentricPointIndices(subCellId, bindices);

    for (vtkIdType i=0;i<4;i++)
      {
      vtkIdType pointIndex = this->ToIndex(bindices[i]);
      this->Tetra->Points->SetPoint(i, this->Points->GetPoint(pointIndex));
      if ( outPd )
        {
        this->Tetra->PointIds->SetId(i, this->PointIds->GetId(pointIndex));
        }
      this->Scalars->SetTuple(i, cellScalars->GetTuple(pointIndex));
      }

    this->Tetra->Contour(value, this->Scalars, locator, verts,
                         lines, polys, inPd, outPd, inCd, cellId, outCd);

    }
}

//----------------------------------------------------------------------------
void vtkLagrangianTetra::Clip(double value,
                              vtkDataArray* cellScalars,
                              vtkIncrementalPointLocator* locator,
                              vtkCellArray* polys,
                              vtkPointData* inPd,
                              vtkPointData* outPd,
                              vtkCellData* inCd,
                              vtkIdType cellId,
                              vtkCellData* outCd,
                              int insideOut)
{
  vtkIdType bindices[4][4];
  vtkIdType numberOfSubtetras = this->NumberOfSubtetras();

  for (vtkIdType subCellId = 0; subCellId < numberOfSubtetras; subCellId++)
    {
    this->SubtetraBarycentricPointIndices(subCellId, bindices);

    for (vtkIdType i=0;i<3;i++)
      {
      vtkIdType pointIndex = this->ToIndex(bindices[i]);
      this->Tetra->Points->SetPoint(i, this->Points->GetPoint(pointIndex));
      if ( outPd )
        {
        this->Tetra->PointIds->SetId(i, this->PointIds->GetId(pointIndex));
        }
      this->Scalars->SetTuple(i, cellScalars->GetTuple(pointIndex));
      }

    this->Tetra->Clip(value, this->Scalars, locator, polys, inPd, outPd,
                      inCd, cellId, outCd, insideOut);
    }
}

//----------------------------------------------------------------------------
int vtkLagrangianTetra::IntersectWithLine(double* p1,
                                          double* p2,
                                          double tol,
                                          double& t,
                                          double* x,
                                          double* pcoords,
                                          int& subId)
{
  vtkIdType bindices[4][4];
  vtkIdType numberOfSubtetras = this->NumberOfSubtetras();
  int subTest;

  t = VTK_DOUBLE_MAX;
  double tTmp;
  double xMin[3], pcoordsMin[3];

  for (vtkIdType subCellId = 0; subCellId < numberOfSubtetras; subCellId++)
    {
    this->SubtetraBarycentricPointIndices(subCellId, bindices);

    for (vtkIdType i=0;i<4;i++)
      {
      vtkIdType pointIndex = this->ToIndex(bindices[i]);
      this->Tetra->Points->SetPoint(i, this->Points->GetPoint(pointIndex));
      }

    if (this->Tetra->IntersectWithLine(p1, p2, tol, tTmp, xMin, pcoordsMin,
                                       subTest) && tTmp < t)
      {
      for (vtkIdType i=0;i<3;i++)
        {
        x[i] = xMin[i];
        pcoords[i] = pcoordsMin[i];
        }
      t = tTmp;
      }
    }
  return (t == VTK_DOUBLE_MAX ? 0 : 1);
}

//----------------------------------------------------------------------------
int vtkLagrangianTetra::Triangulate(int vtkNotUsed(index), vtkIdList *ptIds,
                                      vtkPoints *pts)
{
  pts->Reset();
  ptIds->Reset();

  vtkIdType bindices[4][4];
  vtkIdType numberOfSubtetras = this->NumberOfSubtetras();

  for (vtkIdType subCellId = 0; subCellId < numberOfSubtetras; subCellId++)
    {
    this->SubtetraBarycentricPointIndices(subCellId, bindices);

    for (vtkIdType i=0;i<4;i++)
      {
      vtkIdType pointIndex = this->ToIndex(bindices[i]);
      ptIds->InsertId(4*subCellId+i,this->PointIds->GetId(pointIndex));
      pts->InsertPoint(4*subCellId+i,this->Points->GetPoint(pointIndex));
      }
    }
  return 1;
}

//----------------------------------------------------------------------------
void vtkLagrangianTetra::Derivatives(int vtkNotUsed(subId),
                                       double vtkNotUsed(pcoords)[3],
                                       double *vtkNotUsed(values),
                                       int vtkNotUsed(dim),
                                       double *vtkNotUsed(derivs))
{
  // TODO: Fill me in!
  return;
}

//----------------------------------------------------------------------------

double* vtkLagrangianTetra::GetParametricCoords()
{
  if (!this->LegacyParametricCoordinates)
    {
    vtkIdType order = this->GetOrder();
    double order_d = static_cast<vtkIdType>(order);

    vtkIdType nPoints = (order + 1)*(order + 2)*(order + 3)/6;
    this->LegacyParametricCoordinates = new double[3*nPoints];

    vtkIdType pIdx = 0;
    vtkIdType bindex[4];
    for (vtkIdType p = 0; p < nPoints; p++)
      {
      this->ToBarycentricIndex(p,bindex);
      this->LegacyParametricCoordinates[pIdx] = bindex[0]/order_d;
      this->LegacyParametricCoordinates[pIdx + 1] = bindex[1]/order_d;
      this->LegacyParametricCoordinates[pIdx + 2] = bindex[2]/order_d;
      pIdx += 3;
      }
    }

  return this->LegacyParametricCoordinates;
}

//----------------------------------------------------------------------------
int vtkLagrangianTetra::GetParametricCenter(double pcoords[3])
{
  pcoords[0] = pcoords[1] = pcoords[2] = 0.25;
  return 0;
}

//----------------------------------------------------------------------------
double vtkLagrangianTetra::GetParametricDistance(double pcoords[3])
{
  int i;
  double pDist, pDistMax=0.0;
  double pc[4];

  pc[0] = pcoords[0];
  pc[1] = pcoords[1];
  pc[2] = pcoords[2];
  pc[3] = 1.0 - pcoords[0] - pcoords[1] - pcoords[2];

  for (i=0; i<4; i++)
    {
    if ( pc[i] < 0.0 )
      {
      pDist = -pc[i];
      }
    else if ( pc[i] > 1.0 )
      {
      pDist = pc[i] - 1.0;
      }
    else //inside the cell in the parametric direction
      {
      pDist = 0.0;
      }
    if ( pDist > pDistMax )
      {
      pDistMax = pDist;
      }
    }

  return pDistMax;
}

//----------------------------------------------------------------------------
void vtkLagrangianTetra::InterpolateFunctions(double pcoords[3],
                                                 double* weights)
{
  // Adapted from P. Silvester, "High-Order Polynomial Triangular Finite
  // Elements for Potential Problems". Int. J. Engng Sci. Vol. 7, pp. 849-861.
  // Pergamon Press, 1969.

  double tau[4] = {pcoords[0], pcoords[1], pcoords[2],
                   1. - pcoords[0] - pcoords[1] - pcoords[2]};

  vtkIdType n = this->GetOrder();

  for (vtkIdType idx=0; idx<this->GetPoints()->GetNumberOfPoints(); idx++)
    {
    weights[idx] = 1.;
    vtkIdType lambda[4];
    this->ToBarycentricIndex(idx,lambda);

    for (vtkIdType dim=0;dim<4;dim++)
      {
        weights[idx] *= vtkLagrangianTriangle::eta(n, lambda[dim], tau[dim]);
      }
    }
}

//----------------------------------------------------------------------------
void vtkLagrangianTetra::InterpolateDerivs(double pcoords[3],
                                              double* derivs)
{
  double tau[4] = {pcoords[0], pcoords[1], pcoords[2],
                   1. - pcoords[0] - pcoords[1] - pcoords[2]};

  vtkIdType n = this->GetOrder();

  vtkIdType nPoints = this->GetPoints()->GetNumberOfPoints();

  for (vtkIdType idx = 0; idx < nPoints; idx++)
    {
    vtkIdType lambda[4];
    this->ToBarycentricIndex(idx,lambda);

    double eta_alpha = vtkLagrangianTriangle::eta(n,lambda[0],tau[0]);
    double eta_beta = vtkLagrangianTriangle::eta(n,lambda[1],tau[1]);
    double eta_gamma = vtkLagrangianTriangle::eta(n,lambda[2],tau[2]);
    double eta_delta = vtkLagrangianTriangle::eta(n,lambda[3],tau[3]);

    double d_eta_alpha = vtkLagrangianTriangle::d_eta(n,lambda[0],tau[0]);
    double d_eta_beta = vtkLagrangianTriangle::d_eta(n,lambda[1],tau[1]);
    double d_eta_gamma = vtkLagrangianTriangle::d_eta(n,lambda[2],tau[2]);
    double d_eta_delta = vtkLagrangianTriangle::d_eta(n,lambda[3],tau[3]);

    double d_f_d_tau1 = (d_eta_alpha*eta_beta*eta_gamma*eta_delta -
                         eta_alpha*eta_beta*eta_gamma*d_eta_delta);
    double d_f_d_tau2 = (eta_alpha*d_eta_beta*eta_gamma*eta_delta -
                         eta_alpha*eta_beta*eta_gamma*d_eta_delta);
    double d_f_d_tau3 = (eta_alpha*eta_beta*d_eta_gamma*eta_delta -
                         eta_alpha*eta_beta*eta_gamma*d_eta_delta);
    // double d_f_d_tau4 = (eta_alpha*eta_beta*eta_gamma*d_eta_delta -
    //                      d_eta_alpha*eta_beta*eta_gamma*eta_delta -
    //                      eta_alpha*d_eta_beta*eta_gamma*eta_delta -
    //                      eta_alpha*eta_beta*d_eta_gamma*eta_delta);

    derivs[idx] = d_f_d_tau1;
    derivs[nPoints + idx] = d_f_d_tau2;
    derivs[2*nPoints + idx] = d_f_d_tau3;
    }
}

//----------------------------------------------------------------------------
vtkIdType vtkLagrangianTetra::GetOrder()
{
  // when order = n, # points = (n+1)*(n+2)*(n+3)/6
  // We almost certainly want to cache this

  vtkIdType order = 1;
  vtkIdType nPoints = this->GetPoints()->GetNumberOfPoints();
  vtkIdType nPointsForOrder = 4;

  while (nPointsForOrder < nPoints)
  {
    order++;
    nPointsForOrder = (order+1)*(order+2)*(order+3)/6;
  }

  assert (nPoints == nPointsForOrder);
  return order;
}

//----------------------------------------------------------------------------
void vtkLagrangianTetra::ToBarycentricIndex(vtkIdType index,
                                               vtkIdType* bindex)
{
  return vtkLagrangianTetra::BarycentricIndex(index,bindex,this->GetOrder());
}

//----------------------------------------------------------------------------
vtkIdType vtkLagrangianTetra::ToIndex(const vtkIdType* bindex)
{
  return vtkLagrangianTetra::Index(bindex, this->GetOrder());
}

//----------------------------------------------------------------------------
// this version counts vertices, then triangle layers from bottom to top.
#ifdef ZSTACK
void vtkLagrangianTetra::BarycentricIndex(vtkIdType index, vtkIdType* bindex,
                                          vtkIdType order)
{
  // "Barycentric index" is a set of 4 integers, each running from 0 to
  // <Order>. It is the index of a point in the tetrahedron in barycentric
  // coordinates.

  assert(order >= 1);

  bindex[0] = bindex[1] = bindex[2] = bindex[3] = 0;

  // when vertexMaxCoords[vertexId] == max, we are on vertex <vertexId>
  static const vtkIdType vertexMaxCoords[4] = {3,0,1,2};
  for (vtkIdType vertex = 0; vertex < 4; vertex++)
    {
    if (index == vertex)
      {
      bindex[vertexMaxCoords[vertex]] = order;
      return;
      }
    }

  // decrement one to account for the apex point of the tetrahedron that is
  // not present for the triangle computation
  index -= 1;

  vtkIdType projectedOrder = order;
  vtkIdType nDecrement = (projectedOrder+1)*(projectedOrder+2)/2;
  while (index >= nDecrement)
    {
    projectedOrder--;
    bindex[2]++;
    index -= nDecrement;
    nDecrement = (projectedOrder+1)*(projectedOrder+2)/2;
    }

  vtkIdType projectedBIndex[3];
  vtkLagrangianTriangle::BarycentricIndex(index,projectedBIndex,projectedOrder);
  bindex[0] = projectedBIndex[0];
  bindex[1] = projectedBIndex[1];
  bindex[3] = projectedBIndex[2];

  return;
}

//----------------------------------------------------------------------------
// this version counts vertices, then triangle layers from bottom to top.
vtkIdType vtkLagrangianTetra::Index(const vtkIdType* bindex, vtkIdType order)
{
  vtkIdType index = 0;

  assert(order >= 1);
  assert(bindex[0] + bindex[1] + bindex[2] + bindex[3] == order);

  // when vertexMaxCoords[vertexId] == max, we are on vertex <vertexId>
  static const vtkIdType vertexMaxCoords[4] = {3,0,1,2};

  for (vtkIdType vertex = 0; vertex < 4; vertex++)
    {
    if (bindex[vertexMaxCoords[vertex]] == order)
      {
      // we are on a vertex
      return index;
      }
    index++;
    }

  // account for the triangle vertices already being counted as tetra vertices
  index -= 3;

  vtkIdType projectedOrder;
  for (projectedOrder = order; projectedOrder>(order-bindex[2]);
       projectedOrder--)
    {
    index += (projectedOrder+1)*(projectedOrder+2)/2;
    }

  vtkIdType projectedBIndex[3] = {bindex[0],bindex[1],bindex[3]};

  return index + vtkLagrangianTriangle::Index(projectedBIndex,projectedOrder);
}
#else
//----------------------------------------------------------------------------
// This version uses a different ordering: first vertices, then edges, then
// faces, then repeat with the internal tetra
void vtkLagrangianTetra::BarycentricIndex(vtkIdType index, vtkIdType* bindex,
                                          vtkIdType order)
{
  // "Barycentric index" is a set of 4 integers, each running from 0 to
  // <Order>. It is the index of a point in the tetrahedron in barycentric
  // coordinates.

  assert(order >= 1);

  static const vtkIdType normalizedBarycentricVertices[4][4] = {{0,0,0,1},
                                                                {1,0,0,0},
                                                                {0,1,0,0},
                                                                {0,0,1,0}};
  static const vtkIdType edgeVertices[6][2] = {{0,1},{1,2},{2,0},
                                               {1,3},{3,0},{2,3}};
  vtkIdType max = order;
  vtkIdType min = 0;

  // scope into the correct tetra
  while (index >= 2*(order*order + 1) && index != 0 && order > 3)
    {
    index -= 2*(order*order + 1);
    max-=3;
    min++;
    order -= 4;
    }

  // when vertexMaxCoords[vertexId] == max, we are on vertex <vertexId>
  static const vtkIdType vertexMaxCoords[4] = {3,0,1,2};

  if (index < 4)
    {
    // we are on a vertex
    for (vtkIdType coord = 0; coord < 4; coord++)
      {
      bindex[coord] = (coord == vertexMaxCoords[index] ? max : min);
      }
    return;
    }
  else if (index - 4 < 6*(order - 1))
    {
    // we are on an edge
    vtkIdType edgeId = (index - 4) / (order - 1);
    vtkIdType vertexId = (index - 4) % (order - 1);
    for (vtkIdType coord = 0; coord < 4; coord++)
      {
      bindex[coord] =
        min + (normalizedBarycentricVertices[edgeVertices[edgeId][0]][coord] *
               (max - min - 1 - vertexId) +
               normalizedBarycentricVertices[edgeVertices[edgeId][1]][coord] *
               (1 + vertexId));
      }
    return;
    }
  else
    {
    // we are on a face
    vtkIdType faceId = (index - 4 - 6*(order - 1))/((order - 2)*(order - 1)/2);
    vtkIdType vertexId = (index -4- 6*(order - 1))%((order - 2)*(order - 1)/2);

    vtkIdType projectedBIndex[3];
    if (order == 3)
      {
      projectedBIndex[0] = projectedBIndex[1] = projectedBIndex[2] = 0;
      }
    else
      {
        vtkLagrangianTriangle::BarycentricIndex(vertexId, projectedBIndex,
                                                order-3);
      }

    // static const vtkIdType faceVertices[4][3] = {{0,1,2}, {0,1,3},
    //                                              {1,2,3}, {0,2,3}};

    static const vtkIdType faceBCoords[4][3] = {{0,1,3}, {0,2,3},
                                                {0,1,2}, {1,2,3}};

    // when faceMinCoord[faceId] == min, we are on face <faceId>
    static const vtkIdType faceMinCoord[4] = {2,1,3,0};

      for (vtkIdType i = 0; i < 3; i++)
        {
        bindex[faceBCoords[faceId][i]] = (min + 1 + projectedBIndex[i]);
        }
      bindex[faceMinCoord[faceId]] = min;
      return;
    }
}

//----------------------------------------------------------------------------
// This version uses a different ordering: first vertices, then edges, then
// faces, then repeat with the internal tetra
vtkIdType vtkLagrangianTetra::Index(const vtkIdType* bindex, vtkIdType order)
{
  static const vtkIdType normalizedBarycentricVertices[4][4] = {{0,0,0,1},
                                                                {1,0,0,0},
                                                                {0,1,0,0},
                                                                {0,0,1,0}};
  static const vtkIdType edgeVertices[6][2] = {{0,1},{1,2},{2,0},
                                               {1,3},{3,0},{2,3}};

  vtkIdType index = 0;

  assert(order >= 1);
  assert(bindex[0] + bindex[1] + bindex[2] + bindex[3] == order);

  vtkIdType max = order;
  vtkIdType min = 0;

  vtkIdType bmin =
    std::min(std::min(std::min(bindex[0], bindex[1]), bindex[2]), bindex[3]);

  // scope into the correct tetra
  while (bmin > min)
    {
    index += 2*(order*order + 1);
    max-=3;
    min++;
    order -= 4;
    }

  // when vertexMaxCoords[vertexId] == max, we are on vertex <vertexId>
  static const vtkIdType vertexMaxCoords[4] = {3,0,1,2};

  for (vtkIdType vertex = 0; vertex < 4; vertex++)
    {
    if (bindex[vertexMaxCoords[vertex]] == max)
      {
      // we are on a vertex
      return index;
      }
    index++;
    }

  // when edgeMinCoords[edgeId][0] == 0 && edgeMinCoords[edgeId][1] == min, we
  // are on edge <edgeId>
  static const vtkIdType edgeMinCoords[6][2] = {{1,2},{2,3},{0,2},
                                                {1,3},{0,1},{0,3}};

  // edgeCountingCoord[edgeId] this is the coordinate that increments when
  // traversing edge <edgeId>. It is just the coordinate of the nonzero
  // component of the second vertex of the edge.
  static const vtkIdType edgeCountingCoord[6] = {0,1,3,2,3,2};

  for (vtkIdType edge = 0; edge < 6; edge++)
    {
    if (bindex[edgeMinCoords[edge][0]] == min &&
        bindex[edgeMinCoords[edge][1]] == min)
      {
      // we are on an edge
      return index + bindex[edgeCountingCoord[edge]] - (min + 1);
      }
    index += max - (min + 1);
    }

  // the points that comprise each face
  // static const vtkIdType faceVertices[4][3] = {{0,1,2}, {0,1,3},
  //                                              {1,2,3}, {0,2,3}};

  static const vtkIdType faceBCoords[4][3] = {{0,1,3}, {0,2,3},
                                              {0,1,2}, {1,2,3}};
  // when faceMinCoord[faceId] == min, we are on face <faceId>
  static const vtkIdType faceMinCoord[4] = {2,1,3,0};

  for (vtkIdType face = 0; face < 4; face++)
    {
    if (bindex[faceMinCoord[face]] == min)
      {
      // we are on a face
      vtkIdType projectedBIndex[3];
      for (vtkIdType i = 0; i < 3; i++)
        {
        projectedBIndex[i] = bindex[faceBCoords[face][i]] - min;
        }
      // we must subtract the indices of the face's vertices and edges, which
      // total to 3*order
      return (index + vtkLagrangianTriangle::Index(projectedBIndex,order)
              - 3*order);
      }
    index += (order+1)*(order+2)/2 - 3*order;
    }
  return index;
}
#endif
//----------------------------------------------------------------------------
void vtkLagrangianTetra::WorldToBarycentric(double* coords,
                                            double* bcoords)
{
  double p0[3], p1[3], p2[3], p3[3];
  this->GetPoints()->GetPoint(0, p0);
  this->GetPoints()->GetPoint(1, p1);
  this->GetPoints()->GetPoint(2, p2);
  this->GetPoints()->GetPoint(3, p3);

  double volume = vtkTetra::ComputeVolume(p0, p1, p2, p3);

  bcoords[0] = vtkTetra::ComputeVolume(coords, p1, p2, p3) / volume;
  bcoords[1] = vtkTetra::ComputeVolume(p0, coords, p2, p3) / volume;
  bcoords[2] = vtkTetra::ComputeVolume(p0, p1, coords, p3) / volume;
  bcoords[3] = vtkTetra::ComputeVolume(p0, p1, p2, coords) / volume;
}

//----------------------------------------------------------------------------
void vtkLagrangianTetra::BarycentricToWorld(const double* bcoords,
                                            double* coords)
{
  double p0[3], p1[3], p2[3], p3[3];
  this->GetPoints()->GetPoint(0, p0);
  this->GetPoints()->GetPoint(1, p1);
  this->GetPoints()->GetPoint(2, p2);
  this->GetPoints()->GetPoint(3, p3);

  vtkTetra::TetraCenter(p0,p1,p2,p3,coords);
  for (vtkIdType i=0;i<3;i++)
    {
    coords[i] += (bcoords[0]*p0[i] + bcoords[1]*p1[i] + bcoords[2]*p2[i] +
                  bcoords[3]*p3[i]);
    }
}

//----------------------------------------------------------------------------
void vtkLagrangianTetra::PrintSelf(ostream& os, vtkIndent indent)
{
  this->Superclass::PrintSelf(os,indent);
}
