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

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

#include "GaussQuad.h"
#include "PtVec3d.h"
#include "HexaMesh.h"
// #include "Hdiv3d.h"
#include "WG3d_Hexa.h"


int LinElas3d_WG_HexaQ03Q03RT03Q0_AsmGlbMat1(SparseBlockMatrix &GlbMat,
                                             const double &lambda,
                                             const double &mu,
                                             const HexaMesh &mesh,
                                             const GaussQuad &GQH,
                                             const GaussQuad &GQQ)
{
  int labelElementA, labelElementB, labelFace[6], labelFaceNeighbor[11], sign[6];
  PtVec3d EmCntr, FcCntr, FcCntrNml;
  Hexa hexa;
  Quadri3d quadri, SQF[6];
  FullMatrix EltMat(21,21), SubMat(3,3), ESSM(21,21), EDDM(21,21);
 
  // Setup 
  int Num0E = mesh.numberElements();
  int Num1C = mesh.numberFaces();
  int DOFs = 3*Num0E + 3*Num1C;
  
  // Setting up GlbMat: The sparsity structure 
  int NumRows = 3*Num0E + 3*Num1C;
  int NumCols = NumRows;
  //
  int numRowBands = Num0E + Num1C;
  int numColumnBands = Num0E + Num1C;
  //
  int *dimRowBand = new int[numRowBands];
  for (int i=0; i<numRowBands; ++i)  dimRowBand[i] = 3;
  //
  int *dimColumnBand = new int[numColumnBands];
  for (int j=0; j<numColumnBands; ++j)  dimColumnBand[j] = 3;
  //
  int *numBlocksInRowBand = new int[numRowBands];
  int **columnBandIndex = new int*[numRowBands];
  //
  for (int ie=0; ie<Num0E; ++ie) {
    int labele = ie + 1;
    mesh.getElementFace(labele, labelFace);
	  numBlocksInRowBand[ie] = 7;  // 1 element(itself) + 6 faces
	  columnBandIndex[ie] = new int[numBlocksInRowBand[ie]];
	  columnBandIndex[ie][0] = labele;
	  for (int j=0; j<6; ++j)
	    columnBandIndex[ie][1+j] = Num0E + labelFace[j];
  }
  // 
  for (int jc=0; jc<Num1C; ++jc) {
    int labelc = jc + 1;
    mesh.getFaceElement(labelc, labelElementA, labelElementB);
    mesh.getFaceFace(labelc, labelFaceNeighbor);
    if (mesh.isBoundaryFace(labelc)==0) {  // An interior face 
      numBlocksInRowBand[Num0E+jc] = 13;  // 2 adjacent elements + 11 adjacent faces
	    columnBandIndex[Num0E+jc] = new int[numBlocksInRowBand[Num0E+jc]];
	    columnBandIndex[Num0E+jc][0] = labelElementA;
	    columnBandIndex[Num0E+jc][1] = labelElementB;
      for (int j=0; j<11; ++j)
        columnBandIndex[Num0E+jc][2+j] = Num0E + labelFaceNeighbor[j];
  	}
    if (mesh.isBoundaryFace(labelc)>0) {  // A boundary face 
      numBlocksInRowBand[Num0E+jc] = 7;  // 1 adjacent elements + 6 adjacent faces
	    columnBandIndex[Num0E+jc] = new int[numBlocksInRowBand[Num0E+jc]];
	    columnBandIndex[Num0E+jc][0] = labelElementA;
      for (int j=0; j<6; ++j)
        columnBandIndex[Num0E+jc][1+j] = Num0E + labelFaceNeighbor[j];
  	}
  }

  // 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 GlbMat
  std::cout << "le/1000= ";
  for (int labele=1; labele<=mesh.numberElements(); ++labele) {
    int le = labele - 1;
    if (le%1000==0)  std::cout << le/1000 << "  " << std::flush;
    mesh.getElementFace(labele,labelFace);
    hexa = mesh.element(labele);
    hexa.enrich();
    
    // Faces and related signs
    EmCntr = hexa.trilinearmapping(0.5, 0.5, 0.5);
    for (int j=0; j<6; ++j) {
      SQF[j] = mesh.face(labelFace[j]);
      sign[j] = 1;
      quadri = SQF[j];
      FcCntr = quadri.bilinearmapping(0.5, 0.5);
      FcCntrNml = quadri.normal(0.5, 0.5);
      if (dotProduct(FcCntrNml,FcCntr-EmCntr)<0)  sign[j] = -1;
    }

    // Computing elementwise strain-strain and div-div matrices
    ESSM = WG3d_HexaQ03Q03RT03Q0_NmlBas_EltStrnStrnMat(hexa, SQF, sign, GQH, GQQ);
    EDDM = WG3d_HexaQ03Q03RT03Q0_NmlBas_EltDivDivMat(hexa, SQF, sign, GQH, GQQ);
    EltMat = (2*mu)*ESSM + lambda*EDDM;
    
    // Interaction of element interior 
	  SubMat = submatrix(EltMat, 1, 1, 3, 3);
	  GlbMat.addBlock(labele, labele, SubMat);

	  // Interaction of element-interior and its 6 faces
    for (int j=0; j<6; ++j) {
	    SubMat = submatrix(EltMat, 1, 3*j+4, 3, 3);
	    GlbMat.addBlock(labele, Num0E+labelFace[j], SubMat);
	    SubMat = submatrix(EltMat, 3*j+4, 1, 3, 3);
	    GlbMat.addBlock(Num0E+labelFace[j], labele, SubMat);
    }

	  // Interaction of faces and faces
    for (int i=0; i<6; ++i) {
      // int ii = Num0E + labelFace[i];
      for (int j=0; j<6; ++j) {
        // int jj = Num0E + labelFace[j];
		    SubMat = submatrix(EltMat, 3*i+4, 3*j+4, 3, 3);
	      GlbMat.addBlock(Num0E+labelFace[i], Num0E+labelFace[j], SubMat);
      }
    }
  }
  std::cout << "done!\n";

  return(0);  // If successful
}

// LinElas3d_WG_HexaQ03Q03RT03Q0_AsmGlbMat1.cpp
