#include "LapAssembler.hpp"
#include "ExactSolution.hpp"

template<>
void LapAssembler::assemble_boundary_face_contribution(const SpaMesh::Mesh<InputData::dim>& mesh,
                                                       PETScMatrixType& matrix, 
                                                       PETScVectorType& pRhs, 
				                       FEManagerType& FEManager)
{
  const FEManagerType::ShapeMatrixType& boundaryShapeMatrix           = FEManager.getBoundaryFaceElemShapeFunction();
  const FEManagerType::ShapeDerivMatrixType& boundaryShapeDerivMatrix = FEManager.getBoundaryFaceElemShapeFunctionDeriv();
  const FEManagerType::NormalVectorType& normalVecBoundary            = FEManager.getBoundaryFaceNormalVec();
  const FEManagerType::FaceJacobianVectorType& jacXweightVecBoundary  = FEManager.getBoundaryFaceJacobianWeightVec();
  const FEManagerType::FaceQpointVectorType& qpXVecBoundary           = FEManager.getBoundaryFaceQpointX();
  const LiftingOperatorType::LiftingMatrix& R11 = FEManager.getLifting11();

  Eigen::Matrix<double, InputData::n_dofs, InputData::n_dofs, Eigen::RowMajor> bbMatrix;
  Eigen::Matrix<double, InputData::n_dofs, 1> rhs;
  Eigen::Matrix<double, InputData::n_dofs, 1> bShapeDerivNormal;
  Eigen::Matrix<Eigen::Matrix<double, InputData::n_dofs, 1>, InputData::dim, 1> bc_vec;

  for (int elem = 0; elem != mesh.elemCard(); elem++) 
  {
    SpaMesh::Connectivity::ElemConnVector neighborsVec = mesh.elemNeighbors(elem);
    for (int i = 0; i < neighborsVec.size(); i++)
    {     
      int neighbor = neighborsVec[i];
      if (neighbor == elem)
      {
        FEManager.initBoundaryFaceFE(mesh, elem, i, InputData::p_order, InputData::q_order);
        bbMatrix.setZero();
        rhs.setZero();
        for (unsigned int d = 0; d < InputData::dim; d++)
          bc_vec(d).setZero();
        for (unsigned int j = 0; j < boundaryShapeMatrix.cols(); j++)
        {
          const double bc = ExactSolution::exact_solution(qpXVecBoundary.row(j));
          bShapeDerivNormal.setZero();
          for (unsigned int d = 0; d < InputData::dim; d++)
          {
            bShapeDerivNormal.noalias() += normalVecBoundary(j)(d)*boundaryShapeDerivMatrix(d).col(j);
            bc_vec(d).noalias() += boundaryShapeMatrix.col(j) * (bc * normalVecBoundary(j)(d) * jacXweightVecBoundary(j));
          }
          bbMatrix.noalias() -= jacXweightVecBoundary[j] * (boundaryShapeMatrix.col(j) * bShapeDerivNormal.transpose());
          bbMatrix.noalias() -= jacXweightVecBoundary[j] * (bShapeDerivNormal * (boundaryShapeMatrix.col(j)).transpose());
          rhs.noalias() -= (jacXweightVecBoundary[j] * bc) * bShapeDerivNormal;
        }
        FEManager.computeBoundaryFaceLifting(); 
        for (unsigned int d = 0; d < InputData::dim; d++)
        {
          bbMatrix.noalias() += InputData::penalty * (R11(d).transpose() * R11(d)); 
          rhs.noalias() += InputData::penalty * (R11(d).transpose() * bc_vec(d));
        }
        matrix.add(elem, elem, bbMatrix);
        pRhs.add(elem, rhs);
      }
    }
  }
}

