// LinElas3d_CG_TetraP13_AsmGlbMat.cpp
// James Liu, ColoState; 2014/07--2018/02

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

#include "cell3d.h"
#include "mat3.h"
#include "PtVec3d.h"
#include "TetraMesh.h"
#include "FE_Tetra_CP13.h"


int LinElas3d_CG_TetraP13_AsmGlbMat(SparseBlockMatrix &GlbMat,
                                    double lambda, double mu,
                                    const TetraMesh &mesh)
{
  int *labelNode, labelVertex[4];

  // Setup
  int Num0D = mesh.numberNodes();
  int DOFs = 3 * Num0D;  // NOTE: 3 vector-valued basis functions for each node
  //
  int NumRows = DOFs;
  int NumColumns = NumRows;
  //
  int numRowBands = Num0D;
  int numColumnBands = numRowBands;
  //
  int *dimRowBand = new int[numRowBands];
  for (int i=0; i<Num0D; ++i)  dimRowBand[i] = 3;
  //
  int *dimColumnBand = new int[numColumnBands];
  for (int j=0; j<Num0D; ++j)  dimColumnBand[j] = 3;
  //
  int *numBlocksInRowBand = new int[numRowBands];
  int **columnBandIndex = new int*[numRowBands];

  // Setting up the global matrix: The sparsity structure
  for (int labeld=1; labeld<=mesh.numberNodes(); ++labeld) {
    labelNode = new int[mesh.numberNbrNodesNode(labeld)];
    mesh.getNodeNode(labeld, labelNode);

    int i = labeld - 1;
    numBlocksInRowBand[i] = mesh.numberNbrNodesNode(labeld);
    columnBandIndex[i] = new int[numBlocksInRowBand[i]];
/*
    std::cout << "labeld=" << labeld << "  ";
    std::cout << "NumNbrNodes=" << mesh.numberNbrNodesNode(labeld) << ":  ";
    for (int j=0; j<mesh.numberNbrNodesNode(labeld); ++j) {
      std::cout << labelNode[j] << "  ";
    }
    std::cout << "\n";
*/
    for (int j=0; j<numBlocksInRowBand[i]; ++j) {
      columnBandIndex[i][j] = labelNode[j];
    }
    delete[] labelNode;
  }

  // Setting up GlbMat: resize & release
  GlbMat.resize(numRowBands, numColumnBands, dimRowBand, dimColumnBand,
                numBlocksInRowBand, columnBandIndex);
  //
  for (int i=0; i<numRowBands; ++i)  delete[] columnBandIndex[i];
  delete[] columnBandIndex;
  delete[] numBlocksInRowBand;
  delete[] dimRowBand, dimColumnBand;
  
  // Assembling the global matrix
  FullMatrix EGGM(12,12), EDDM(12,12), EltMat(12,12), SubMat(3,3);
  for (int labele=1; labele<=mesh.numberElements(); ++labele) {
    Tetra tetra = mesh.element(labele);
    mesh.getElementNode(labele, labelVertex);
    
    EGGM = FE_Tetra_CP13_EltMatGradGrad(tetra);
    EDDM = FE_Tetra_CP13_EltMatDivDiv(tetra);
    EltMat = mu*EGGM + (mu+lambda)*EDDM;

    // std::cout << "labele=" << labele << "\n";
    // std::cout << EltMat << std::flush;
  
    for (int i=1; i<=4; ++i) {
      int ib = labelVertex[i-1];
      for (int j=1; j<=4; ++j) {
        int jb = labelVertex[j-1];
        SubMat = submatrix(EltMat, 1+3*(i-1), 1+3*(j-1), 3, 3);
        GlbMat.addBlock(ib, jb, SubMat);
      }
    }
  }

  return(0);  // If successful
}

// LinElas3d_CG_TetraP13_AsmGlbMat.cpp
