// LinElas3d_WG_HexaQ03Q03RT03Q0_ModiLinSys.cpp
// Solving linear elasticity in 3d by WG(Q0^3,Q0^3;RT[0]^3,Q0) on a hexa.mesh
// The Schur-complement approach
// James Liu, Graham Harper, ColoState; 2014/07--2017/11

#include "matrix.h"
#include "vector.h"

#include "GaussQuad.h"
#include "PtVec3d.h"
#include "HexaMesh.h"


int LinElas3d_WG_HexaQ03Q03RT03Q0_ModiLinSys(SparseBlockMatrix &GlbMatEC,
                                             SparseBlockMatrix &GlbMatCE,
                                             SparseBlockMatrix &GlbMatCC,
                                             Vector &GlbVecE, Vector &GlbVecC,
                                             const HexaMesh &mesh,
                                             const Vector &GlbVecD,
                                             const Vector &GlbVecN)
{
  int labelElementA, labelElementB, labelFaceNeighbor[11];
  
  // Auxilliary stuff
  FullMatrix zero3(3,3), eye3(3,3);
  eye3(1,1)=1; eye3(2,2)=1; eye3(3,3)=1;
  
  // Setup
  int Num0E = mesh.numberElements();
  int Num1F = mesh.numberFaces();
  
  // Modify system for element interiors
  GlbVecE = GlbVecE - GlbMatEC * GlbVecD;

  // Modifying system for element faces
  GlbVecC = GlbVecN - GlbMatCC * GlbVecD;
  for (int labelc=1; labelc<=Num1F; ++labelc) {
    int lc = labelc - 1;
    if (mesh.isBoundaryFace(labelc)>0) { // Dirichlet face (3 functions)
      GlbVecC[3*lc] = GlbVecD[3*lc];
      GlbVecC[3*lc+1] = GlbVecD[3*lc+1];
      GlbVecC[3*lc+2] = GlbVecD[3*lc+2];
    }
  }

  // Modifying GlbMatEC, GlbMatCE simultaneously
  for (int labelc=1; labelc<=Num1F; ++labelc) {
    if (mesh.isBoundaryFace(labelc)>0) {  // Dirichlet face
      mesh.getFaceElement(labelc, labelElementA, labelElementB);
      GlbMatEC.setBlock(labelElementA, labelc, zero3);
      GlbMatCE.setBlock(labelc, labelElementA, zero3);
    }
  }

  // Modifying GlbMatCC
  for (int labelc=1; labelc<=Num1F; ++labelc) {
    if (mesh.isBoundaryFace(labelc)>0) {  // Dirichlet face
      mesh.getFaceFace(labelc, labelFaceNeighbor);
      GlbMatCC.setBlock(labelc, labelc, eye3);
      for (int j=0; j<6; ++j) {
        if (labelFaceNeighbor[j]==labelc)  continue;
        GlbMatCC.setBlock(labelc, labelFaceNeighbor[j], zero3);
        GlbMatCC.setBlock(labelFaceNeighbor[j], labelc, zero3);
      }
    }
  }

  return(0);  // If successful
}

// LinElas3d_WG_HexaQ03Q03RT03Q0_ModiLinSys.cpp
