#include "LapAssembler.hpp"
#include "ExactSolution.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();

  LocalMatrixType bbMatrix;
  LocalVectorType rhs;
  LocalVectorType bShapeDerivNormal;
  for (SpaMesh::FaceIterator faceIt(mesh); faceIt; ++faceIt)
  {
    std::pair<int,int> elemNeighborPair = faceIt.getNeighboringElems(mesh);
    if (elemNeighborPair.first == elemNeighborPair.second)
    {
      int elem = elemNeighborPair.first;
      FEManager.initBoundaryFaceFE(mesh, elem, faceIt, InputData::p_order, InputData::q_order);
      double sideVol = jacXweightVecBoundary.sum();
      double elemSize = FEManager.computeFESize(mesh, elem);
      const double h = (elemSize/sideVol) * (1./pow(InputData::p_order,2.));
      bbMatrix.setZero();
      rhs.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);
        bbMatrix.noalias() -= jacXweightVecBoundary[j] * (boundaryShapeMatrix.col(j) * bShapeDerivNormal.transpose());
        bbMatrix.noalias() -= jacXweightVecBoundary[j] * (bShapeDerivNormal * (boundaryShapeMatrix.col(j)).transpose());
        bbMatrix.noalias() += jacXweightVecBoundary[j] * (InputData::penalty/h) * (boundaryShapeMatrix.col(j) * (boundaryShapeMatrix.col(j)).transpose());
        rhs.noalias() -= (jacXweightVecBoundary[j] * bc) * bShapeDerivNormal;
        rhs.noalias() += (jacXweightVecBoundary[j] * InputData::penalty/h * bc) * boundaryShapeMatrix.col(j);
      }
      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;
    }
  }
}

