// LinElas3d_CG_TetraP13_ModiLinSys.cpp
// Modifying the discrete global linear system
// James Liu, ColoState; 2014/07--2018/02

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

#include "GaussQuad.h"
#include "mat3.h"
#include "PtVec3d.h"
#include "TetraMesh.h"


// JL20160901: TO BE REVISED FOR EFFECIENCY

int LinElas3d_CG_TetraP13_ModiLinSys(SparseBlockMatrix &GlbMat, Vector &GlbRHS,
                                     const TetraMesh &mesh,
                                     const Vector &GlbVecSource,
                                     const Vector &GlbVecDirichlet,
                                     const Vector &GlbVecNeumann)
{
  int *labelNode;
  // int DOFs = 3*mesh.numberNodes();
  int DOFs = GlbMat.columnSize();
  FullMatrix eye33(3,3), zero33(3,3);

  // Auxiliary
  eye33(1,1) = 1;  eye33(2,2) = 1;  eye33(3,3) = 1;

  // Modifying
  GlbRHS.resize(DOFs);
  GlbRHS = GlbVecSource - GlbVecNeumann;
  GlbRHS = GlbRHS - GlbMat * GlbVecDirichlet;
  for (int labeld=1; labeld<=mesh.numberNodes(); ++labeld) {
    int ld = labeld - 1;
    if (mesh.isBoundaryNode(labeld)>0) {
      // std::cout << "labeld=" << labeld << "  " << "ld=" << ld << "  ";
      int numNbrNds = mesh.numberNbrNodesNode(labeld);
      // std::cout << numNbrNds << "  ";
      labelNode = new int[numNbrNds];
      mesh.getNodeNode(labeld, labelNode);
      for (int j=0; j<numNbrNds; ++j) {
        // std::cout << labelNode[j] << "  ";
        if (labelNode[j]==labeld) {
          // Do nothing
        }
        else {
          GlbMat.setBlock(labelNode[j], labeld, zero33);
          GlbMat.setBlock(labeld, labelNode[j], zero33);
        }
      }
      // std::cout << "\n";
      GlbMat.setBlock(labeld, labeld, eye33);
      delete[] labelNode;
      // std::cout << "ld=" << ld << "\n" << std::flush;
      GlbRHS(3*ld+1) = GlbVecDirichlet(3*ld+1);
      GlbRHS(3*ld+2) = GlbVecDirichlet(3*ld+2);
      GlbRHS(3*ld+3) = GlbVecDirichlet(3*ld+3);
    }
  }
  // GlbRHS.save2file("GlbRHS.dat");

  return(0);  // If successful
}

// LinElas3d_CG_TetraP13_ModiLinSys.cpp
