#include "LapAssembler.hpp"

void LapAssembler::assemble_boundary_face_contribution(const MeshType& mesh,
                                                       GlobalMatrixType& matrix, 
                                                       GlobalVectorType& gRhs, 
				                       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();

  LocalMatrixType bbMatrix;
  LocalVectorType rhs;
  LocalVectorType bShapeDerivNormal;
  Eigen::Matrix<LocalVectorType, InputData::dim, 1> bc_vec;
  for (SpaMesh::ElementIterator elemIt(mesh); elemIt; ++elemIt) 
  {
    int elem = elemIt.value();
    for (SpaMesh::NeighborIterator neighborIt(mesh,elemIt); neighborIt; ++neighborIt)
    {     
      int neighbor = neighborIt.value();
      if (neighbor == elem)
      {
        FEManager.initBoundaryFaceFE(mesh, elemIt, neighborIt.index(), 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 = InputData::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));
        }
        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) += bbMatrix(i,j);
        gRhs.segment<InputData::n_dofs>(elem*InputData::n_dofs) += rhs;
      }
    }
  }
}

