#include "LapAssembler.hpp"

void LapAssembler::assemble_volume_contribution(const MeshType& mesh,
                                                GlobalMatrixType& matrix, 
                                                GlobalVectorType& gRhs, 
                                                FEManagerType& FEManager)
{
  const FEManagerType::ShapeMatrixType& shapeMatrix           = FEManager.getElemShapeFunction();
  const FEManagerType::ShapeDerivMatrixType& shapeDerivMatrix = FEManager.getElemShapeFunctionDeriv();
  const FEManagerType::ElemJacobianVectorType& jacXweightVec  = FEManager.getElemJacobianWeightVec();
  const FEManagerType::ElemQpointVectorType& qpXVec           = FEManager.getElemQpointX();

  LocalMatrixType stiffnessMatrix;
  LocalVectorType rhs;
  
  for (SpaMesh::ElementIterator elemIt(mesh); elemIt; ++elemIt) 
  {
    int elem = elemIt.value();
    FEManager.initVolumeFE(mesh, elemIt, InputData::p_order, InputData::q_order);
    stiffnessMatrix.setZero();
    rhs.setZero();
    for (unsigned int j = 0; j < shapeMatrix.cols(); j++)
    {
      double forcingTerm = InputData::ExactSolution::forcingTerm(qpXVec.row(j));
      for (unsigned int d = 0; d < InputData::dim; d++)
      {
        stiffnessMatrix.noalias() += jacXweightVec[j] * (shapeDerivMatrix(d).col(j) * (shapeDerivMatrix(d).col(j)).transpose());
      }
      rhs.noalias() += shapeMatrix.col(j) * jacXweightVec[j] * forcingTerm;
    }
    for (unsigned int j = 0; j < InputData::n_dofs; j++)
      for (unsigned int i = 0; i < InputData::n_dofs; i++)
        matrix.coeffRef(elem*InputData::n_dofs+i,elem*InputData::n_dofs+j) += stiffnessMatrix(i,j);
    gRhs.segment<InputData::n_dofs>(elem*InputData::n_dofs) += rhs;
  }
}
