#ifdef PETSC_RCS_HEADER
static char vcid[] = "$Id: elemvec2d.c,v 1.14 2000/01/31 17:51:54 knepley Exp $";
#endif

#include "src/grid/gridimpl.h"         /*I "grid.h" I*/
#include "src/mesh/impls/triangular/triimpl.h"
#include "elemvec2d.h"

/*--------------------------------------------- Element Vector Functions --------------------------------------------*/
#undef  __FUNCT__
#define __FUNCT__ "PrintVecIndices_Private"
int PrintVecIndices_Private(Grid grid, const char option[], int field, int node, int comp, int rowStart, int *rowIdx) {
  MPI_Comm   comm;
  int        rank;
  int        var;
  PetscTruth opt;
  int        ierr;

  PetscFunctionBegin;
  ierr = PetscOptionsHasName(PETSC_NULL, option, &opt);                                                   CHKERRQ(ierr);
  if (opt == PETSC_FALSE) PetscFunctionReturn(0);
  ierr = PetscObjectGetComm((PetscObject) grid, &comm);                                                   CHKERRQ(ierr);
  ierr = MPI_Comm_rank(comm, &rank);                                                                      CHKERRQ(ierr);
  PetscPrintf(PETSC_COMM_SELF, "[%d]field: %d node: %d\n", rank, field, node);
  for(var = rowStart-comp; var < rowStart; var++)
    PetscPrintf(PETSC_COMM_SELF, "[%d]rowIdx[%d]: %d\n", rank, var, rowIdx[var]);
  PetscFunctionReturn(0);
}

#undef  __FUNCT__
#define __FUNCT__ "GridCalcElementVecIndices_Triangular_2D"
/*
  Reduction:

  When the variables associated with boundary conditions are being reduced, the element vector is compressed
  to account for their absence. This reduction only happens in the global numbering. When using the local
  numbering, the original variable numbers are replaced by the corresponding index in grid->bdReduceVec
  which contains the boundary values. In order to distinguish these indices, they are stored as -(idx+1).
  The boundary values are retieved by GridLocalToElement(). The useOldStructs flag signals that we are
  interpolating from a previous mesh, and thus the constraints must be calculated from the previous values.
*/
int GridCalcElementVecIndices_Triangular_2D(Grid grid, Mesh mesh, int elem, VarOrdering order, VarOrdering reduceOrder,
                                            PetscTruth localNumbering, PetscTruth useOldStructs, ElementVec vec)
{
  int                   numCorners   = mesh->numCorners;
  int                   numNodes     = mesh->numNodes;
  PetscConstraintObject constCtx     = grid->constraintCtx;
  int                  *firstVar     = order->firstVar;
  int                  *offsets      = order->offsets;
  int                  *localOffsets = order->localOffsets;
  int                 **localStart   = order->localStart;
  int                  *rowIdx       = vec->indices;
  int                   rank         = mesh->part->rank;
  int                   size         = vec->size;
  PetscScalar          *array        = vec->array;
  FieldClassMap         map;
  int                   numFields;
  int                  *fields;
  int                 **fieldClasses, **reduceFieldClasses;
  int                  *classes;
  PetscTruth            isReduced, isConstrained;
  int                  *isConst;
  int                  *reduceFirstVar     = PETSC_NULL;
  int                  *reduceOffsets      = PETSC_NULL;
  int                  *reduceLocalOffsets = PETSC_NULL;
  int                 **reduceLocalStart   = PETSC_NULL;
  int                   field, node, nclass, comp, startVar, diff, inc;
  int                   i, f, count, source, corner, row;
  int                   ierr;

  PetscFunctionBegin;
  ierr = VarOrderingGetClassMap(order, &map);                                                            CHKERRQ(ierr);
  numFields          = map->numFields;
  fields             = map->fields;
  fieldClasses       = map->fieldClasses;
  reduceFieldClasses = map->reduceFieldClasses;
  classes            = map->classes;
  isReduced          = map->isReduced;
  isConstrained      = map->isConstrained;
  isConst            = map->isClassConstrained;
  if (isReduced == PETSC_TRUE) {
    reduceFirstVar     = reduceOrder->firstVar;
    reduceOffsets      = reduceOrder->offsets;
    reduceLocalOffsets = reduceOrder->localOffsets;
    reduceLocalStart   = reduceOrder->localStart;
  }
  for(f = 0, count = 0, source = 0, inc = 0; f < numFields; f++) {
    field = fields[f];
    comp  = grid->fields[field].disc->comp;

    for(corner = 0; corner < numCorners; corner++) {
      ierr = MeshGetNodeFromElement(mesh, elem, corner, &node);                                           CHKERRQ(ierr);
      nclass = classes[node];
      if ((isReduced == PETSC_TRUE) && (reduceFieldClasses[f][nclass])) {
        if (localNumbering == PETSC_FALSE) {
          /* Increment source pointer in value array, which corresponds to skipping constrained variables */
          source += comp;
          inc    -= comp;
        } else {
          if (node >= numNodes)
            startVar = reduceLocalOffsets[node-numNodes] + reduceLocalStart[field][nclass];
          else
            startVar = reduceOffsets[node] - reduceFirstVar[rank] + reduceLocalStart[field][nclass];
          for(i = 0; i < comp; i++, count++) {
            rowIdx[count] = -(startVar + i + 1);
          }
#ifdef PETSC_USE_BOPT_g
          ierr = PrintVecIndices_Private(grid, "-trace_vec_assembly", field, node, comp, count, rowIdx); CHKERRQ(ierr);
#endif
        }
      } else if ((isConstrained == PETSC_TRUE) && (isConst[nclass]) && (grid->fields[field].isConstrained == PETSC_TRUE)) {
        /* We guarantee that count <= source here */
        diff = grid->fields[field].constraintCompDiff;
        inc += diff;
        /* Move rows to operate on */
        if (count < source) {
          for(row = 0; row < comp; row++)
            array[count+row] = array[source+row];
        }
        source += comp;
        /* We must replace this field with the constraint field and prevent overwriting in element vector */
        if (source - count < comp + diff) {
          /* Move old fields */
          for(row = size-(diff+1); row >= source; row--)
            array[row+diff] = array[row];
          source += diff;
        }
        /* Apply P^T to get constraint fields */
        ierr = (*constCtx->ops->constrainelemvec)(constCtx, mesh, order, field, node, count, localNumbering,
                                                  useOldStructs, CONSTRAINT_ROW, vec);
        CHKERRQ(ierr);
        count += comp + diff;
      } else if (fieldClasses[f][nclass]) {
        if (localNumbering == PETSC_FALSE) {
          startVar = offsets[node] + localStart[field][nclass];
        } else {
          if (node >= numNodes)
            startVar = localOffsets[node-numNodes] + localStart[field][nclass];
          else
            startVar = offsets[node] - firstVar[rank] + localStart[field][nclass];
        }
        for(i = 0; i < comp; i++, count++, source++) {
          rowIdx[count] = startVar + i;
          array[count]  = array[source];
        }
#ifdef PETSC_USE_BOPT_g
        ierr = PrintVecIndices_Private(grid, "-trace_vec_assembly", field, node, comp, count, rowIdx);   CHKERRQ(ierr);
#endif
      }
    }
  }
#ifdef PETSC_USE_BOPT_g
  if (count != vec->reduceSize + inc) {
    SETERRQ3(PETSC_ERR_PLIB, "Invalid variable numbering elem: %d size: %d count: %d\n", elem, vec->reduceSize+inc, count);
  }
  if (count > vec->size) {
    SETERRQ3(PETSC_ERR_PLIB, "Number of indices %d in elem %d exceeded maximum size %d\n", count, elem, vec->size);
  }
#endif
  /* Set true vector size */
  vec->reduceSize = count;

  PetscFunctionReturn(0);
}

#undef  __FUNCT__
#define __FUNCT__ "GridProjectElementVec_Triangular_2D"
/*
  Projection:

  We would like a mechanism to convert from the constrained to unconstrained variables, and vice versa,
  for an element vector. This code is also embedded in the element vector index calculation, and could
  perhaps be split out. This function is generally called to take a constrained element vector to
  unconstrained variables before an element integral is performed.
*/
int GridProjectElementVec_Triangular_2D(Grid grid, Mesh mesh, int elem, VarOrdering order, VarOrdering reduceOrder,
                                        PetscTruth constrain, PetscTruth useOldStructs, ElementVec vec)
{
  int                   numCorners   = mesh->numCorners;
  PetscConstraintObject constCtx     = grid->constraintCtx;
  int                   size         = vec->size;
  int                  *rowIdx       = vec->indices;
  PetscScalar          *array        = vec->array;
  FieldClassMap         map;
  int                   numFields;
  int                  *fields;
  int                 **fieldClasses, **reduceFieldClasses;
  int                  *classes;
  PetscTruth            isReduced, isConstrained;
  int                  *isConst;
  int                   field, node, nclass, comp, diff, inc;
  int                   i, f, count, source, corner, row;
  int                   ierr;

  PetscFunctionBegin;
  ierr = VarOrderingGetClassMap(order, &map);                                                            CHKERRQ(ierr);
  numFields          = map->numFields;
  fields             = map->fields;
  fieldClasses       = map->fieldClasses;
  classes            = map->classes;
  isReduced          = map->isReduced;
  isConstrained      = map->isConstrained;
  isConst            = map->isClassConstrained;
  reduceFieldClasses = map->reduceFieldClasses;
  if (isConstrained == PETSC_FALSE)
    PetscFunctionReturn(0);
  for(f = 0, count = 0, source = 0, inc = 0; f < numFields; f++) {
    field = fields[f];
    comp  = grid->fields[field].disc->comp;

    for(corner = 0; corner < numCorners; corner++) {
      ierr = MeshGetNodeFromElement(mesh, elem, corner, &node);                                           CHKERRQ(ierr);
      nclass = classes[node];
      if ((isReduced == PETSC_TRUE) && (reduceFieldClasses[f][nclass])) {
        for(i = 0; i < comp; i++, count++, source++) {
          rowIdx[count] = rowIdx[source];
          array[count]  = array[source];
        }
      } else if ((isConst[nclass]) && (grid->fields[field].isConstrained == PETSC_TRUE)) {
        /* We guarantee that count <= source here */
        if (constrain == PETSC_TRUE) {
          diff  = grid->fields[field].constraintCompDiff;
        } else {
          diff  = -grid->fields[field].constraintCompDiff;
          comp += grid->fields[field].constraintCompDiff;
        }
        inc += diff;
        /* Move rows to operate on */
        if (count < source) {
          for(row = 0; row < comp; row++)
            array[count+row] = array[source+row];
        }
        source += comp;
        /* We must replace this field with the constraint field and prevent overwriting in element vector */
        if (source - count < comp + diff) {
          /* Move old fields */
          for(row = size-(diff+1); row >= source; row--)
            array[row+diff] = array[row];
          source += diff;
        }
        if (constrain == PETSC_TRUE) {
          /* Apply P^T to get constraint fields */
          ierr = (*constCtx->ops->constrainelemvec)(constCtx, mesh, order, field, node, count, PETSC_FALSE,
                                                    useOldStructs, CONSTRAINT_ROW, vec);
          CHKERRQ(ierr);
          count += comp + diff;
        } else {
          /* Apply P to get unconstrained fields */
          ierr = (*constCtx->ops->constrainelemvec)(constCtx, mesh, order, field, node, count, PETSC_FALSE,
                                                    useOldStructs, CONSTRAINT_ROW_TRANS, vec);
          CHKERRQ(ierr);
          count += comp + diff;
          comp  -= grid->fields[field].constraintCompDiff;
        }
      } else if (fieldClasses[f][nclass]) {
        for(i = 0; i < comp; i++, count++, source++) {
          rowIdx[count] = rowIdx[source];
          array[count]  = array[source];
        }
      }
    }
  }
#ifdef PETSC_USE_BOPT_g
  if (count != vec->reduceSize + inc) {
    SETERRQ3(PETSC_ERR_PLIB, "Invalid variable numbering elem: %d size: %d count: %d\n", elem, vec->reduceSize+inc, count);
  }
  if (count > vec->size) {
    SETERRQ3(PETSC_ERR_PLIB, "Number of indices %d in elem %d exceeded maximum size %d\n", count, elem, vec->size);
  }
#endif
  /* Set true vector size */
  vec->reduceSize = count;

  PetscFunctionReturn(0);
}

#undef  __FUNCT__
#define __FUNCT__ "GridCalcBoundaryElementVecIndices_Triangular_2D"
int GridCalcBoundaryElementVecIndices_Triangular_2D(Grid grid, int bd, int edge, int midnode, VarOrdering order,
                                                    VarOrdering reduceOrder, PetscTruth localNumbering, ElementVec vec)
{
  Mesh_Triangular      *tri           = (Mesh_Triangular *) grid->mesh->data;
  int                  *edges         = tri->edges;
  int                   numNodes      = grid->mesh->numNodes;
  PetscConstraintObject constCtx      = grid->constraintCtx;
  int                  *firstVar      = order->firstVar;
  int                  *offsets       = order->offsets;
  int                  *localOffsets  = order->localOffsets;
  int                 **localStart    = order->localStart;
  int                  *rowIdx        = vec->indices;
  int                   size          = vec->size;
  PetscScalar          *array         = vec->array;
  int                   rank;
  FieldClassMap         map;
  int                   numFields;
  int                  *fields;
  int                 **fieldClasses, **reduceFieldClasses;
  int                  *classes;
  PetscTruth            isReduced, isConstrained;
  int                  *isConst;
  int                  *reduceFirstVar     = PETSC_NULL;
  int                  *reduceOffsets      = PETSC_NULL;
  int                  *reduceLocalOffsets = PETSC_NULL;
  int                 **reduceLocalStart   = PETSC_NULL;
  int                   field, node, nclass, comp, startVar, diff, inc;
  int                   i, f, count, source, corner, row;
  int                   ierr;

  PetscFunctionBegin;
  ierr = MPI_Comm_rank(grid->comm, &rank);                                                               CHKERRQ(ierr);
  ierr = VarOrderingGetClassMap(order, &map);                                                            CHKERRQ(ierr);
  numFields          = map->numFields;
  fields             = map->fields;
  fieldClasses       = map->fieldClasses;
  reduceFieldClasses = map->reduceFieldClasses;
  classes            = map->classes;
  isReduced          = map->isReduced;
  isConstrained      = map->isConstrained;
  isConst            = map->isClassConstrained;
  if (isReduced == PETSC_TRUE) {
    reduceFirstVar     = reduceOrder->firstVar;
    reduceOffsets      = reduceOrder->offsets;
    reduceLocalOffsets = reduceOrder->localOffsets;
    reduceLocalStart   = reduceOrder->localStart;
  }
  for(f = 0, count = 0, source = 0, inc = 0; f < numFields; f++) {
    field = fields[f];
    comp  = grid->fields[field].disc->bdDisc->comp;

    for(corner = 0; corner < 2; corner++) {
      node   = edges[edge*2+corner];
      nclass = classes[node];

      if ((isReduced == PETSC_TRUE) && (reduceFieldClasses[f][nclass])) {
        if (localNumbering == PETSC_FALSE) {
          /* Increment source pointer in value array, which corresponds to skipping constrained variables */
          source += comp;
          inc    -= comp;
        } else {
          if (node >= numNodes)
            startVar = reduceLocalOffsets[node-numNodes] + reduceLocalStart[field][nclass];
          else
            startVar = reduceOffsets[node] - reduceFirstVar[rank] + reduceLocalStart[field][nclass];
          for(i = 0; i < comp; i++, count++) {
            rowIdx[count] = -(startVar + i + 1);
          }
#ifdef PETSC_USE_BOPT_g
          ierr = PrintVecIndices_Private(grid, "-trace_vec_assembly", field, node, comp, count, rowIdx); CHKERRQ(ierr);
#endif
        }
      } else if ((isConstrained == PETSC_TRUE) && (isConst[nclass]) && (grid->fields[field].isConstrained == PETSC_TRUE)) {
        /* We guarantee that count <= source here */
        diff = grid->fields[field].constraintCompDiff;
        inc += diff;
        /* Move rows to operate on */
        if (count < source) {
          for(row = 0; row < comp; row++)
            array[count+row] = array[source+row];
        }
        source += comp;
        /* We must replace this field with the constraint field and prevent overwriting in element vector */
        if (source - count < comp + diff) {
          /* Move old fields */
          for(row = size-(diff+1); row >= source; row--)
            array[row+diff] = array[row];
          source += diff;
        }
        /* Apply P^T to get constraint fields */
        ierr = (*constCtx->ops->constrainelemvec)(constCtx, grid->mesh, order, field, node, count, localNumbering,
                                                  PETSC_FALSE, CONSTRAINT_ROW, vec);
        CHKERRQ(ierr);
        count += comp + diff;
      } else if (fieldClasses[f][nclass]) {
        if (localNumbering == PETSC_FALSE) {
          startVar = offsets[node] + localStart[field][nclass];
        } else {
          if (node >= numNodes)
            startVar = localOffsets[node-numNodes] + localStart[field][nclass];
          else
            startVar = offsets[node] - firstVar[rank] + localStart[field][nclass];
        }
        for(i = 0; i < comp; i++, count++, source++) {
          rowIdx[count] = startVar + i;
          array[count]  = array[source];
        }
#ifdef PETSC_USE_BOPT_g
        ierr = PrintVecIndices_Private(grid, "-trace_vec_assembly", field, node, comp, count, rowIdx); CHKERRQ(ierr);
#endif
      }
    }

    if (midnode >= 0) {
      nclass = classes[midnode];

      if ((isReduced == PETSC_TRUE) && (reduceFieldClasses[f][nclass])) {
        if (localNumbering == PETSC_FALSE) {
          /* Increment source pointer in value array, which corresponds to skipping constrained variables */
          source += comp;
          inc    -= comp;
        } else {
          if (midnode >= numNodes)
            startVar = reduceLocalOffsets[midnode-numNodes] + reduceLocalStart[field][nclass];
          else
            startVar = reduceOffsets[midnode] - reduceFirstVar[rank] + reduceLocalStart[field][nclass];
          for(i = 0; i < comp; i++, count++) {
            rowIdx[count] = -(startVar + i + 1);
          }
#ifdef PETSC_USE_BOPT_g
          ierr = PrintVecIndices_Private(grid, "-trace_vec_assembly", field, midnode, comp, count, rowIdx); CHKERRQ(ierr);
#endif
        }
      } else if ((isConstrained == PETSC_TRUE) && (isConst[nclass]) && (grid->fields[field].isConstrained == PETSC_TRUE)) {
        /* We guarantee that count <= source here */
        diff = grid->fields[field].constraintCompDiff;
        inc += diff;
        /* Move rows to operate on */
        if (count < source) {
          for(row = 0; row < comp; row++)
            array[count+row] = array[source+row];
        }
        source += comp;
        /* We must replace this field with the constraint field and prevent overwriting in element vector */
        if (source - count < comp + diff) {
          /* Move old fields */
          for(row = size-(diff+1); row >= source; row--)
            array[row+diff] = array[row];
          source += diff;
        }
        /* Apply P^T to get constraint fields */
        ierr = (*constCtx->ops->constrainelemvec)(constCtx, grid->mesh, order, field, midnode, count, localNumbering,
                                                  PETSC_FALSE, CONSTRAINT_ROW, vec);
        CHKERRQ(ierr);
        count += comp + diff;
      } else if (fieldClasses[f][nclass]) {
        if (localNumbering == PETSC_FALSE) {
          startVar = offsets[midnode] + localStart[field][nclass];
        } else {
          if (midnode >= numNodes)
            startVar = localOffsets[midnode-numNodes] + localStart[field][nclass];
          else
            startVar = offsets[midnode] - firstVar[rank] + localStart[field][nclass];
        }
        for(i = 0; i < comp; i++, count++, source++) {
          rowIdx[count] = startVar + i;
          array[count]  = array[source];
        }
#ifdef PETSC_USE_BOPT_g
        ierr = PrintVecIndices_Private(grid, "-trace_vec_assembly", field, midnode, comp, count, rowIdx); CHKERRQ(ierr);
#endif
      }
    }
  }
#ifdef PETSC_USE_BOPT_g
  if (count != vec->reduceSize + inc) {
    SETERRQ3(PETSC_ERR_PLIB, "Invalid variable numbering edge: %d size: %d count: %d\n", edge, vec->reduceSize+inc, count);
  }
  if (count > vec->size) {
    SETERRQ3(PETSC_ERR_PLIB, "Number of indices %d on edge %d exceeded maximum size %d\n", count, edge, vec->size);
  }
#endif
  /* Set true vector size */
  vec->reduceSize = count;

  PetscFunctionReturn(0);
}

/*--------------------------------------------- Element Matrix Functions --------------------------------------------*/
/*
  Element Matrix Structure:

  The structure of the element matrix is diagrammed in src/gvec/grid/gridimpl.h.

  Reduction:

  When the variables associated with boundary conditions are being reduced, the element matrix is compressed
  to account for their absence. This reduction only happens in the global numbering. When using the local
  numbering, the original variable numbers are replaced by the corresponding index in grid->bdReduceVec
  which contains the boundary values. In order to distinguish these indices, they are stored as -(idx+1).
  The boundary values are retieved by GridLocalToElement().
*/
#undef  __FUNCT__
#define __FUNCT__ "GridCalcElementMatIndices_Triangular_2D"
int GridCalcElementMatIndices_Triangular_2D(Grid grid, int elem, VarOrdering sOrder, VarOrdering tOrder,
                                            VarOrdering reduceOrder, PetscTruth localNumbering, ElementMat mat)
{
  Mesh                  mesh;
  int                   numCorners      = grid->mesh->numCorners;
  PetscConstraintObject constCtx        = grid->constraintCtx;
  int                  *colFirstVar     = sOrder->firstVar;
  int                  *colOffsets      = sOrder->offsets;
  int                  *colLocalOffsets = sOrder->localOffsets;
  int                 **colLocalStart   = sOrder->localStart;
  int                  *rowFirstVar     = tOrder->firstVar;
  int                  *rowOffsets      = tOrder->offsets;
  int                  *rowLocalOffsets = tOrder->localOffsets;
  int                 **rowLocalStart   = tOrder->localStart;
  int                  *rowIdx          = mat->rowIndices;
  int                  *colIdx          = mat->colIndices;
  int                   rowSize         = mat->reduceRowSize;
  int                   colSize         = mat->reduceColSize;
  int                   origRowSize     = mat->reduceRowSize;
  int                   origColSize     = mat->reduceColSize;
  int                  *newCols         = mat->reduceCols;
  PetscScalar          *array           = mat->array;
  PetscScalar          *tempArray       = mat->tempArray;
  int                   rank            = grid->mesh->part->rank;
  FieldClassMap         rowMap,                  colMap;
  int                   numNodes;
  int                   numRowFields,            numColFields;
  int                  *rowFields,              *colFields;
  int                 **rowFieldClasses,       **colFieldClasses;
  int                 **rowReduceFieldClasses, **colReduceFieldClasses;
  int                  *rowClasses,             *colClasses;
  PetscTruth            rowIsReduced,            colIsReduced;
  PetscTruth            rowIsConstrained,        colIsConstrained;
  int                  *rowIsConst,             *colIsConst;
  int                  *reduceFirstVar     = PETSC_NULL;
  int                  *reduceOffsets      = PETSC_NULL;
  int                  *reduceLocalOffsets = PETSC_NULL;
  int                 **reduceLocalStart   = PETSC_NULL;
  int                   field, node, nclass, comp, startVar, diff, inc;
  int                   i, f, corner, rowCount, rowSource, colCount, colSource, row, col, newCol;
  int                   ierr;

  PetscFunctionBegin;
  ierr = GridGetMesh(grid, &mesh);                                                                       CHKERRQ(ierr);
  ierr = VarOrderingGetClassMap(tOrder, &rowMap);                                                        CHKERRQ(ierr);
  ierr = VarOrderingGetClassMap(sOrder, &colMap);                                                        CHKERRQ(ierr);
  numNodes              = rowMap->numNodes;
  numRowFields          = rowMap->numFields;
  rowFields             = rowMap->fields;
  rowFieldClasses       = rowMap->fieldClasses;
  rowReduceFieldClasses = rowMap->reduceFieldClasses;
  rowClasses            = rowMap->classes;
  rowIsReduced          = rowMap->isReduced;
  rowIsConstrained      = rowMap->isConstrained;
  rowIsConst            = rowMap->isClassConstrained;
  numColFields          = colMap->numFields;
  colFields             = colMap->fields;
  colFieldClasses       = colMap->fieldClasses;
  colReduceFieldClasses = colMap->reduceFieldClasses;
  colClasses            = colMap->classes;
  colIsReduced          = colMap->isReduced;
  colIsConstrained      = colMap->isConstrained;
  colIsConst            = colMap->isClassConstrained;
  if ((rowIsReduced == PETSC_TRUE) || (colIsReduced == PETSC_TRUE)) {
    reduceFirstVar     = reduceOrder->firstVar;
    reduceOffsets      = reduceOrder->offsets;
    reduceLocalOffsets = reduceOrder->localOffsets;
    reduceLocalStart   = reduceOrder->localStart;
  }
  for(f = 0, rowCount = 0, rowSource = 0, inc = 0; f < numRowFields; f++) {
    field = rowFields[f];
    comp  = grid->fields[field].disc->comp;

    for(corner = 0; corner < numCorners; corner++) {
      ierr = MeshGetNodeFromElement(mesh, elem, corner, &node);                                           CHKERRQ(ierr);
      nclass = rowClasses[node];

      if ((rowIsReduced == PETSC_TRUE) && (rowReduceFieldClasses[f][nclass])) {
        if (localNumbering == PETSC_FALSE) {
          /* Increment source pointer in value array, which corresponds to skipping constrained variables */
          rowSource += comp;
          inc       -= comp;
        } else {
          if (node >= numNodes)
            startVar = reduceLocalOffsets[node-numNodes] + reduceLocalStart[field][nclass];
          else
            startVar = reduceOffsets[node] - reduceFirstVar[rank] + reduceLocalStart[field][nclass];
          for(i = 0; i < comp; i++, rowCount++, rowSource++) {
            rowIdx[rowCount] = -(startVar + i + 1);
          }
        }
      } else if ((rowIsConstrained == PETSC_TRUE) && (rowIsConst[nclass]) && (grid->fields[field].isConstrained == PETSC_TRUE)) {
        /* We guarantee that rowCount <= rowSource here */
        diff = grid->fields[field].constraintCompDiff;
        inc += diff;
        /* Move rows to operate on */
        if (rowCount < rowSource) {
          for(row = 0; row < comp; row++)
            for(col = 0; col < colSize; col++)
              array[(rowCount+row)*colSize+col] = array[(rowSource+row)*colSize+col];
        }
        rowSource += comp;
        /* We must replace this field with the constraint field and prevent overwriting in element matrix */
        if (rowSource - rowCount < comp + diff) {
          /* Move old fields */
          for(row = rowSize-1; row >= rowSource; row--)
            for(col = 0; col < colSize; col++)
              array[(row+diff)*colSize+col] = array[row*colSize+col];
          rowSource += diff;
          rowSize   += diff;
        }
        /* Apply P^T to get constraint fields */
        ierr = (*constCtx->ops->constrainelemmat)(constCtx, grid->mesh, tOrder, field, node, rowCount, CONSTRAINT_ROW, mat);
        CHKERRQ(ierr);
        rowCount += comp + diff;
      } else if (rowFieldClasses[f][nclass]) {
        if (localNumbering == PETSC_FALSE) {
          startVar = rowOffsets[node] + rowLocalStart[field][nclass];
        } else {
          if (node >= numNodes)
            startVar = rowLocalOffsets[node-numNodes] + rowLocalStart[field][nclass];
          else
            startVar = rowOffsets[node] - rowFirstVar[rank] + rowLocalStart[field][nclass];
        }
        for(i = 0; i < comp; i++, rowCount++, rowSource++) {
          rowIdx[rowCount] = startVar + i;
          /* Shrink rows -- I do not see a way around shrinking the whole matrix */
          if (rowCount != rowSource) {
            for(col = 0; col < colSize; col++)
              array[rowCount*colSize+col] = array[rowSource*colSize+col];
          }
        }
      }
    }
  }
  if (rowCount != origRowSize + inc) {
    SETERRQ3(PETSC_ERR_PLIB, "Invalid row numbering elem: %d size: %d count: %d\n", elem, origRowSize+inc, rowCount);
  }
  if (rowSize  > mat->rowSize) {
    SETERRQ3(PETSC_ERR_PLIB, "Number of row indices %d in elem %d exceeded maximum size %d\n", rowSize, elem, mat->rowSize);
  }
  if (rowCount > mat->rowSize) {
    SETERRQ3(PETSC_ERR_PLIB, "Number of row indices %d in elem %d exceeded maximum size %d\n", rowCount, elem, mat->rowSize);
  }
  mat->reduceRowSize = rowCount;

  /* Calculate the columns ordering after reduction --
       colCount  - Number of columns stacked into mat
       colSource - Current column of mat being accessed
       inc       - The difference in size of the constrained matrix from the original
       newCols   - The column reordering, newCols[colSource] is the current column in the matrix, -1 for not present
  */
  for(f = 0, colCount = 0, colSource = 0, newCol = origColSize, inc = 0; f < numColFields; f++) {
    field = colFields[f];
    comp  = grid->fields[field].disc->comp;

    for(corner = 0; corner < numCorners; corner++) {
      ierr = MeshGetNodeFromElement(mesh, elem, corner, &node);                                           CHKERRQ(ierr);
      nclass = colClasses[node];
 
      if ((colIsReduced == PETSC_TRUE) && (colReduceFieldClasses[f][nclass])) {
        if (localNumbering == PETSC_FALSE) {
          /* Increment source pointer in value array, which corresponds to skipping constrained variables */
          for(i = 0; i < comp; i++, colSource++) {
            newCols[colSource] = -1;
          }
          inc       -= comp;
        } else {
          /* Put in negative indices corresponding to boundary values */
          for(i = 0; i < comp; i++, colCount++, colSource++) {
            newCols[colSource] = colCount;
          }
        }
      } else if ((colIsConstrained == PETSC_TRUE) && (colIsConst[nclass]) && (grid->fields[field].isConstrained == PETSC_TRUE)) {
        /* We must replace this field with the constraint field and prevent overwriting in element matrix */
        diff = grid->fields[field].constraintCompDiff;
        inc += diff;
        if (diff > 0) {
          /* Assume new field were previously in the last columns */
          for(i = 0; i < comp; i++, colCount++, colSource++)
            newCols[colSource] = colCount;
          for(i = 0; i < diff; i++, colCount++, newCol++)
            newCols[newCol]    = colCount;
        } else {
          /* Just squeeze matrix */
          for(i = 0; i < comp + diff; i++, colCount++, colSource++)
            newCols[colSource] = colCount;
          for(i = comp+diff; i < comp; i++, colSource++)
            newCols[colSource] = -1;
        }
      } else if (colFieldClasses[f][nclass]) {
        for(i = 0; i < comp; i++, colCount++, colSource++) {
          newCols[colSource] = colCount;
        }
      }
    }
  }
#ifdef PETSC_USE_BOPT_g
  if (colCount != origColSize + inc) {
    SETERRQ3(PETSC_ERR_PLIB, "Invalid column numbering elem: %d size: %d count: %d\n", elem, origColSize+inc, colCount);
  }
  if (colCount > mat->colSize) {
    SETERRQ3(PETSC_ERR_PLIB, "Number of column indices %d in elem %d exceeded maximum size %d\n", colCount, elem, mat->colSize);
  }
#endif
  mat->reduceColSize = colCount;

  /* Reform the element matrix: newCols[original col] = new col */
  if (mat->reduceColSize != origColSize) {
    if (colIsConstrained == PETSC_TRUE) {
      for(row = 0; row < mat->reduceRowSize; row++)
        for(col = 0; col < origColSize; col++)
          if (newCols[col] >= 0)
            tempArray[row*mat->reduceColSize+newCols[col]] = array[row*origColSize+col];
      ierr = PetscMemcpy(array, tempArray, mat->reduceRowSize*mat->reduceColSize * sizeof(PetscScalar));  CHKERRQ(ierr);
    } else {
      /* Can copy in place if no constraints were applied since BC just delete entries */
      for(row = 0; row < mat->reduceRowSize; row++)
        for(col = 0; col < origColSize; col++)
          if (newCols[col] >= 0)
            array[row*mat->reduceColSize+newCols[col]] = array[row*origColSize+col];
    }
  }

  /* Calculate indices and constrained matrix elements */
  for(f = 0, colCount = 0; f < numColFields; f++) {
    field = colFields[f];
    comp  = grid->fields[field].disc->comp;

    for(corner = 0; corner < numCorners; corner++) {
      ierr = MeshGetNodeFromElement(mesh, elem, corner, &node);                                           CHKERRQ(ierr);
      nclass = colClasses[node];
 
      if ((colIsReduced == PETSC_TRUE) && (colReduceFieldClasses[f][nclass])) {
        if (localNumbering == PETSC_TRUE) {
          if (node >= numNodes)
            startVar = reduceLocalOffsets[node-numNodes] + reduceLocalStart[field][nclass];
          else
            startVar = reduceOffsets[node] - reduceFirstVar[rank] + reduceLocalStart[field][nclass];
          for(i = 0; i < comp; i++, colCount++) {
            colIdx[colCount] = -(startVar + i + 1);
          }
        }
      } else if ((colIsConstrained == PETSC_TRUE) && (colIsConst[nclass]) && (grid->fields[field].isConstrained == PETSC_TRUE)) {
        /* Apply P to get constraint fields */
        ierr = (*constCtx->ops->constrainelemmat)(constCtx, grid->mesh, sOrder, field, node, colCount, CONSTRAINT_COL, mat);
        CHKERRQ(ierr);
        colCount  += comp + grid->fields[field].constraintCompDiff;
      } else if (colFieldClasses[f][nclass]) {
        if (localNumbering == PETSC_FALSE) {
          startVar = colOffsets[node] + colLocalStart[field][nclass];
        } else {
          if (node >= numNodes)
            startVar = colLocalOffsets[node-numNodes] + colLocalStart[field][nclass];
          else
            startVar = colOffsets[node] - colFirstVar[rank] + colLocalStart[field][nclass];
        }
        for(i = 0; i < comp; i++, colCount++) {
          colIdx[colCount] = startVar + i;
        }
      }
    }
  }

  PetscFunctionReturn(0);
}

#undef  __FUNCT__
#define __FUNCT__ "GridCalcBoundaryElementMatIndices_Triangular_2D"
int GridCalcBoundaryElementMatIndices_Triangular_2D(Grid grid, int bd, int edge, int midnode, VarOrdering sOrder,
                                                    VarOrdering tOrder, VarOrdering reduceOrder, PetscTruth localNumbering,
                                                    ElementMat mat)
{
  Mesh_Triangular      *tri             = (Mesh_Triangular *) grid->mesh->data;
  int                   numCorners      = grid->mesh->numCorners;
  int                  *elements        = tri->faces;
  int                  *edges           = tri->edges;
  PetscConstraintObject constCtx        = grid->constraintCtx;
  int                  *colFirstVar     = sOrder->firstVar;
  int                  *colOffsets      = sOrder->offsets;
  int                  *colLocalOffsets = sOrder->localOffsets;
  int                 **colLocalStart   = sOrder->localStart;
  int                  *rowFirstVar     = tOrder->firstVar;
  int                  *rowOffsets      = tOrder->offsets;
  int                  *rowLocalOffsets = tOrder->localOffsets;
  int                 **rowLocalStart   = tOrder->localStart;
  int                  *rowIdx          = mat->rowIndices;
  int                  *colIdx          = mat->colIndices;
  int                   rowSize         = mat->reduceRowSize;
  int                   colSize         = mat->reduceColSize;
  int                   origRowSize     = mat->reduceRowSize;
  int                   origColSize     = mat->reduceColSize;
  int                  *newCols         = mat->reduceCols;
  PetscScalar          *array           = mat->array;
  PetscScalar          *tempArray       = mat->tempArray;
  int                   rank            = grid->mesh->part->rank;
  FieldClassMap         rowMap,                  colMap;
  int                   numNodes;
  int                   numRowFields,            numColFields;
  int                  *rowFields,              *colFields;
  int                 **rowFieldClasses,       **colFieldClasses;
  int                 **rowReduceFieldClasses, **colReduceFieldClasses;
  int                  *rowClasses,             *colClasses;
  PetscTruth            rowIsReduced,            colIsReduced;
  PetscTruth            rowIsConstrained,        colIsConstrained;
  int                  *rowIsConst,             *colIsConst;
  int                  *reduceFirstVar     = PETSC_NULL;
  int                  *reduceOffsets      = PETSC_NULL;
  int                  *reduceLocalOffsets = PETSC_NULL;
  int                 **reduceLocalStart   = PETSC_NULL;
  int                   field, node, nclass, comp, startVar, diff, inc;
  int                   i, f, elem, corner, rowCount, rowSource, colCount, colSource, row, col, newCol;
  int                   ierr;

  PetscFunctionBegin;
  ierr = VarOrderingGetClassMap(tOrder, &rowMap);                                                        CHKERRQ(ierr);
  ierr = VarOrderingGetClassMap(sOrder, &colMap);                                                        CHKERRQ(ierr);
  numNodes              = rowMap->numNodes;
  numRowFields          = rowMap->numFields;
  rowFields             = rowMap->fields;
  rowFieldClasses       = rowMap->fieldClasses;
  rowReduceFieldClasses = rowMap->reduceFieldClasses;
  rowClasses            = rowMap->classes;
  rowIsReduced          = rowMap->isReduced;
  rowIsConstrained      = rowMap->isConstrained;
  rowIsConst            = rowMap->isClassConstrained;
  numColFields          = colMap->numFields;
  colFields             = colMap->fields;
  colFieldClasses       = colMap->fieldClasses;
  colReduceFieldClasses = colMap->reduceFieldClasses;
  colClasses            = colMap->classes;
  colIsReduced          = colMap->isReduced;
  colIsConstrained      = colMap->isConstrained;
  colIsConst            = colMap->isClassConstrained;
  if ((rowIsReduced == PETSC_TRUE) || (colIsReduced == PETSC_TRUE)) {
    reduceFirstVar     = reduceOrder->firstVar;
    reduceOffsets      = reduceOrder->offsets;
    reduceLocalOffsets = reduceOrder->localOffsets;
    reduceLocalStart   = reduceOrder->localStart;
  }
  ierr = MeshGetBdElementFromEdge(grid->mesh, edge, &elem);                                              CHKERRQ(ierr);
  for(f = 0, rowCount = 0, rowSource = 0, inc = 0; f < numRowFields; f++) {
    field = rowFields[f];
    comp  = grid->fields[field].disc->comp;

    for(corner = 0; corner < numCorners; corner++) {
      node   = elements[elem*numCorners+corner];
      nclass = rowClasses[node];

      if ((rowIsReduced == PETSC_TRUE) && (rowReduceFieldClasses[f][nclass])) {
        if (localNumbering == PETSC_FALSE) {
          /* Increment source pointer in value array, which corresponds to skipping constrained variables */
          rowSource += comp;
          inc       -= comp;
        } else {
          if (node >= numNodes)
            startVar = reduceLocalOffsets[node-numNodes] + reduceLocalStart[field][nclass];
          else
            startVar = reduceOffsets[node] - reduceFirstVar[rank] + reduceLocalStart[field][nclass];
          for(i = 0; i < comp; i++, rowCount++, rowSource++) {
            rowIdx[rowCount] = -(startVar + i + 1);
          }
        }
      } else if ((rowIsConstrained == PETSC_TRUE) && (rowIsConst[nclass]) && (grid->fields[field].isConstrained == PETSC_TRUE)) {
        /* We guarantee that rowCount <= rowSource here */
        diff = grid->fields[field].constraintCompDiff;
        inc += diff;
        /* Move rows to operate on */
        if (rowCount < rowSource) {
          for(row = 0; row < comp; row++)
            for(col = 0; col < colSize; col++)
              array[(rowCount+row)*colSize+col] = array[(rowSource+row)*colSize+col];
        }
        rowSource += comp;
        /* We must replace this field with the constraint field and prevent overwriting in element matrix */
        if (rowSource - rowCount < comp + diff) {
          /* Move old fields */
          for(row = rowSize-1; row >= rowSource; row--)
            for(col = 0; col < colSize; col++)
              array[(row+diff)*colSize+col] = array[row*colSize+col];
          rowSource += diff;
          rowSize   += diff;
        }
        /* Apply P^T to get constraint fields */
        ierr = (*constCtx->ops->constrainelemmat)(constCtx, grid->mesh, tOrder, field, node, rowCount, CONSTRAINT_ROW, mat);
        CHKERRQ(ierr);
        rowCount += comp + diff;
      } else if (rowFieldClasses[f][nclass]) {
        if (localNumbering == PETSC_FALSE) {
          startVar = rowOffsets[node] + rowLocalStart[field][nclass];
        } else {
          if (node >= numNodes)
            startVar = rowLocalOffsets[node-numNodes] + rowLocalStart[field][nclass];
          else
            startVar = rowOffsets[node] - rowFirstVar[rank] + rowLocalStart[field][nclass];
        }
        for(i = 0; i < comp; i++, rowCount++, rowSource++) {
          rowIdx[rowCount] = startVar + i;
          /* Shrink rows -- I do not see a way around shrinking the whole matrix */
          if (rowCount != rowSource) {
            for(col = 0; col < colSize; col++)
              array[rowCount*colSize+col] = array[rowSource*colSize+col];
          }
        }
      }
    }
  }
  if (rowCount != origRowSize + inc) {
    SETERRQ3(PETSC_ERR_PLIB, "Invalid row numbering edge: %d size: %d count: %d\n", edge, origRowSize+inc, rowCount);
  }
  if (rowSize  > mat->rowSize) {
    SETERRQ3(PETSC_ERR_PLIB, "Number of row indices %d on edge %d exceeded maximum size %d\n", rowSize, edge, mat->rowSize);
  }
  if (rowCount > mat->rowSize) {
    SETERRQ3(PETSC_ERR_PLIB, "Number of row indices %d on edge %d exceeded maximum size %d\n", rowCount, edge, mat->rowSize);
  }
  mat->reduceRowSize = rowCount;

  /* Calculate the columns ordering after reduction --
       colCount  - Number of columns stacked into mat
       colSource - Current column of mat being accessed
       inc       - The difference in size of the constrained matrix from the original
       newCols   - The column reordering, newCols[colSource] is the current column in the matrix, -1 for not present
  */
  for(f = 0, colCount = 0, colSource = 0, newCol = origColSize, inc = 0; f < numColFields; f++) {
    field = colFields[f];
    comp  = grid->fields[field].disc->bdDisc->comp;

    for(corner = 0; corner < 2; corner++) {
      node   = edges[edge*2+corner];
      nclass = colClasses[node];

      if ((colIsReduced == PETSC_TRUE) && (colReduceFieldClasses[f][nclass])) {
        if (localNumbering == PETSC_FALSE) {
          /* Increment source pointer in value array, which corresponds to skipping constrained variables */
          for(i = 0; i < comp; i++, colSource++) {
            newCols[colSource] = -1;
          }
          inc       -= comp;
        } else {
          /* Put in negative indices corresponding to boundary values */
          for(i = 0; i < comp; i++, colCount++, colSource++) {
            newCols[colSource] = colCount;
          }
        }
      } else if ((colIsConstrained == PETSC_TRUE) && (colIsConst[nclass]) && (grid->fields[field].isConstrained == PETSC_TRUE)) {
        /* We must replace this field with the constraint field and prevent overwriting in element matrix */
        diff = grid->fields[field].constraintCompDiff;
        inc += diff;
        if (diff > 0) {
          /* Assume new field were previously in the last columns */
          for(i = 0; i < comp; i++, colCount++, colSource++)
            newCols[colSource] = colCount;
          for(i = 0; i < diff; i++, colCount++, newCol++)
            newCols[newCol]    = colCount;
        } else {
          /* Just squeeze matrix */
          for(i = 0; i < comp + diff; i++, colCount++, colSource++)
            newCols[colSource] = colCount;
          for(i = comp+diff; i < comp; i++, colSource++)
            newCols[colSource] = -1;
        }
      } else if (colFieldClasses[f][nclass]) {
        for(i = 0; i < comp; i++, colCount++, colSource++) {
          newCols[colSource] = colCount;
        }
      }
    }

    if (midnode >= 0) {
      nclass = colClasses[midnode];

      if ((colIsReduced == PETSC_TRUE) && (colReduceFieldClasses[f][nclass])) {
        if (localNumbering == PETSC_FALSE) {
          /* Increment source pointer in value array, which corresponds to skipping constrained variables */
          for(i = 0; i < comp; i++, colSource++) {
            newCols[colSource] = -1;
          }
          inc       -= comp;
        } else {
          /* Put in negative indices corresponding to boundary values */
          for(i = 0; i < comp; i++, colCount++, colSource++) {
            newCols[colSource] = colCount;
          }
        }
      } else if ((colIsConstrained == PETSC_TRUE) && (colIsConst[nclass]) && (grid->fields[field].isConstrained == PETSC_TRUE)) {
        /* We must replace this field with the constraint field and prevent overwriting in element matrix */
        diff = grid->fields[field].constraintCompDiff;
        inc += diff;
        if (diff > 0) {
          /* Assume new field were previously in the last columns */
          for(i = 0; i < comp; i++, colCount++, colSource++)
            newCols[colSource] = colCount;
          for(i = 0; i < diff; i++, colCount++, newCol++)
            newCols[newCol]    = colCount;
        } else {
          /* Just squeeze matrix */
          for(i = 0; i < comp + diff; i++, colCount++, colSource++)
            newCols[colSource] = colCount;
          for(i = comp+diff; i < comp; i++, colSource++)
            newCols[colSource] = -1;
        }
      } else if (colFieldClasses[f][nclass]) {
        for(i = 0; i < comp; i++, colCount++, colSource++) {
          newCols[colSource] = colCount;
        }
      }
    }
  }
#ifdef PETSC_USE_BOPT_g
  if (colCount != origColSize + inc) {
    SETERRQ3(PETSC_ERR_PLIB, "Invalid column numbering edge: %d size: %d count: %d\n", edge, origColSize+inc, colCount);
  }
  if (colCount > mat->colSize) {
    SETERRQ3(PETSC_ERR_PLIB, "Number of column indices %d on edge %d exceeded maximum size %d\n", colCount, edge, mat->colSize);
  }
#endif
  mat->reduceColSize = colCount;

  /* Reform the element matrix: newCols[original col] = new col */
  if (mat->reduceColSize != origColSize) {
    if (colIsConstrained == PETSC_TRUE) {
      for(row = 0; row < mat->reduceRowSize; row++)
        for(col = 0; col < origColSize; col++)
          if (newCols[col] >= 0)
            tempArray[row*mat->reduceColSize+newCols[col]] = array[row*origColSize+col];
      ierr = PetscMemcpy(array, tempArray, mat->reduceRowSize*mat->reduceColSize * sizeof(PetscScalar));  CHKERRQ(ierr);
    } else {
      /* Can copy in place if no constraints were applied since BC just delete entries */
      for(row = 0; row < mat->reduceRowSize; row++)
        for(col = 0; col < origColSize; col++)
          if (newCols[col] >= 0)
            array[row*mat->reduceColSize+newCols[col]] = array[row*origColSize+col];
    }
  }

  /* Calculate indices and constrained matrix elements */
  for(f = 0, colCount = 0; f < numColFields; f++) {
    field = colFields[f];
    comp  = grid->fields[field].disc->comp;

    for(corner = 0; corner < 2; corner++) {
      node   = edges[edge*2+corner];
      nclass = colClasses[node];
 
      if ((colIsReduced == PETSC_TRUE) && (colReduceFieldClasses[f][nclass])) {
        if (localNumbering == PETSC_TRUE) {
          if (node >= numNodes)
            startVar = reduceLocalOffsets[node-numNodes] + reduceLocalStart[field][nclass];
          else
            startVar = reduceOffsets[node] - reduceFirstVar[rank] + reduceLocalStart[field][nclass];
          for(i = 0; i < comp; i++, colCount++) {
            colIdx[colCount] = -(startVar + i + 1);
          }
        }
      } else if ((colIsConstrained == PETSC_TRUE) && (colIsConst[nclass]) && (grid->fields[field].isConstrained == PETSC_TRUE)) {
        /* Apply P to get constraint fields */
        ierr = (*constCtx->ops->constrainelemmat)(constCtx, grid->mesh, sOrder, field, node, colCount, CONSTRAINT_COL, mat);
        CHKERRQ(ierr);
        colCount  += comp + grid->fields[field].constraintCompDiff;
      } else if (colFieldClasses[f][nclass]) {
        if (localNumbering == PETSC_FALSE) {
          startVar = colOffsets[node] + colLocalStart[field][nclass];
        } else {
          if (node >= numNodes)
            startVar = colLocalOffsets[node-numNodes] + colLocalStart[field][nclass];
          else
            startVar = colOffsets[node] - colFirstVar[rank] + colLocalStart[field][nclass];
        }
        for(i = 0; i < comp; i++, colCount++) {
          colIdx[colCount] = startVar + i;
        }
      }
    }

    if (midnode >= 0) {
      nclass = colClasses[midnode];
 
      if ((colIsReduced == PETSC_TRUE) && (colReduceFieldClasses[f][nclass])) {
        if (localNumbering == PETSC_TRUE) {
          if (midnode >= numNodes)
            startVar = reduceLocalOffsets[midnode-numNodes] + reduceLocalStart[field][nclass];
          else
            startVar = reduceOffsets[midnode] - reduceFirstVar[rank] + reduceLocalStart[field][nclass];
          for(i = 0; i < comp; i++, colCount++) {
            colIdx[colCount] = -(startVar + i + 1);
          }
        }
      } else if ((colIsConstrained == PETSC_TRUE) && (colIsConst[nclass]) && (grid->fields[field].isConstrained == PETSC_TRUE)) {
        /* Apply P to get constraint fields */
        ierr = (*constCtx->ops->constrainelemmat)(constCtx, grid->mesh, sOrder, field, midnode, colCount, CONSTRAINT_COL, mat);
        CHKERRQ(ierr);
        colCount  += comp + grid->fields[field].constraintCompDiff;
      } else if (colFieldClasses[f][nclass]) {
        if (localNumbering == PETSC_FALSE) {
          startVar = colOffsets[midnode] + colLocalStart[field][nclass];
        } else {
          if (midnode >= numNodes)
            startVar = colLocalOffsets[midnode-numNodes] + colLocalStart[field][nclass];
          else
            startVar = colOffsets[midnode] - colFirstVar[rank] + colLocalStart[field][nclass];
        }
        for(i = 0; i < comp; i++, colCount++) {
          colIdx[colCount] = startVar + i;
        }
      }
    }
  }

  PetscFunctionReturn(0);
}