#include "LapAssembler.hpp"

template<>
void LapAssembler::assemble_volume_contribution(const SpaMesh::Mesh<InputData::dim>& mesh,
                                                PETScMatrixType& matrix, 
                                                PETScVectorType& pRhs, 
                                                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();
  Eigen::Matrix<double, InputData::n_dofs, InputData::n_dofs, Eigen::RowMajor> stiffnessMatrix;
  Eigen::Matrix<double, InputData::n_dofs, 1> 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 = ExactSolution::exact_second_deriv(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;
    }
    matrix.add(elem, elem, stiffnessMatrix);
    pRhs.add(elem, rhs);
  }
}
