#include "LapAssembler.hpp"

void LapAssembler::assemble_internal_face_contribution(const MeshType& mesh,
                                                       GlobalMatrixType& matrix, 
                                                       GlobalVectorType& gRhs, 
				                       FEManagerType& FEManager)
{
  const FEManagerType::ShapeMatrixType& bShapeMatrix             = FEManager.getInternalFaceElemShapeFunction();
  const FEManagerType::ShapeMatrixType& fShapeMatrix             = FEManager.getInternalFaceNeighborShapeFunction();
  const FEManagerType::ShapeDerivMatrixType& bShapeDerivMatrix   = FEManager.getInternalFaceElemShapeFunctionDeriv();     
  const FEManagerType::ShapeDerivMatrixType& fShapeDerivMatrix   = FEManager.getInternalFaceNeighborShapeFunctionDeriv(); 
  const FEManagerType::NormalVectorType& normalVec               = FEManager.getInternalFaceNormalVec();
  const FEManagerType::FaceJacobianVectorType& jacXweightVecSide = FEManager.getInternalFaceJacobianWeightVec();
  const FEManagerType::FaceQpointVectorType& qpXVecSide          = FEManager.getInternalFaceQpointX();

  LocalMatrixType ffMatrix, bbMatrix, fbMatrix, bfMatrix;
  LocalVectorType fShapeDerivNormal, bShapeDerivNormal;

  for (SpaMesh::FaceIterator faceIt(mesh); faceIt; ++faceIt)
  {
    std::pair<int,int> elemNeighborPair = faceIt.getNeighboringElems(mesh);
    if (elemNeighborPair.first < elemNeighborPair.second)
    {
      FEManager.initInternalFaceFE(mesh, elemNeighborPair.first, elemNeighborPair.second, faceIt, InputData::p_order, InputData::q_order);
      const double elemSize = FEManager.computeFESize(mesh, elemNeighborPair.first);
      const double neighborSize = FEManager.computeFESize(mesh, elemNeighborPair.second);
      const double sideVol = jacXweightVecSide.sum();
      const double h = (0.5*(elemSize+neighborSize)/(sideVol)) * (1./pow(InputData::p_order,2.));
      ffMatrix.setZero();
      bbMatrix.setZero();
      fbMatrix.setZero();
      bfMatrix.setZero();
      for (unsigned int j = 0; j < bShapeMatrix.cols(); j++)
      {
        fShapeDerivNormal.setZero();
        bShapeDerivNormal.setZero();
        for (unsigned int d = 0; d < InputData::dim; d++)
        {
          fShapeDerivNormal.noalias() += fShapeDerivMatrix(d).col(j) * normalVec(j)(d);
          bShapeDerivNormal.noalias() += bShapeDerivMatrix(d).col(j) * normalVec(j)(d);
        }
        bbMatrix.noalias() -= 0.5 * jacXweightVecSide[j] * (bShapeMatrix.col(j) * bShapeDerivNormal.transpose());
        bbMatrix.noalias() -= 0.5 * jacXweightVecSide[j] * (bShapeDerivNormal *  (bShapeMatrix.col(j)).transpose());
        ffMatrix.noalias() += 0.5 * jacXweightVecSide[j] * (fShapeMatrix.col(j) * fShapeDerivNormal.transpose());
        ffMatrix.noalias() += 0.5 * jacXweightVecSide[j] * (fShapeDerivNormal *  (fShapeMatrix.col(j)).transpose());
        bfMatrix.noalias() -= 0.5 * jacXweightVecSide[j] * (bShapeMatrix.col(j) * fShapeDerivNormal.transpose());
        bfMatrix.noalias() += 0.5 * jacXweightVecSide[j] * (bShapeDerivNormal *  (fShapeMatrix.col(j)).transpose());
        fbMatrix.noalias() += 0.5 * jacXweightVecSide[j] * (fShapeMatrix.col(j) * bShapeDerivNormal.transpose());
        fbMatrix.noalias() -= 0.5 * jacXweightVecSide[j] * (fShapeDerivNormal *  (bShapeMatrix.col(j)).transpose());
        bbMatrix.noalias() += (InputData::penalty/h) * jacXweightVecSide[j] * (bShapeMatrix.col(j) * (bShapeMatrix.col(j)).transpose());
        ffMatrix.noalias() += (InputData::penalty/h) * jacXweightVecSide[j] * (fShapeMatrix.col(j) * (fShapeMatrix.col(j)).transpose());
        fbMatrix.noalias() -= (InputData::penalty/h) * jacXweightVecSide[j] * (fShapeMatrix.col(j) * (bShapeMatrix.col(j)).transpose());
        bfMatrix.noalias() -= (InputData::penalty/h) * jacXweightVecSide[j] * (bShapeMatrix.col(j) * (fShapeMatrix.col(j)).transpose());
      }
      for (unsigned int j = 0; j < InputData::n_dofs; j++)
        for (unsigned int i = 0; i < InputData::n_dofs; i++)
        {
          matrix.coeffRef(elemNeighborPair.first* InputData::n_dofs+i, elemNeighborPair.first* InputData::n_dofs+j) += bbMatrix(i,j);
          matrix.coeffRef(elemNeighborPair.second*InputData::n_dofs+i, elemNeighborPair.second*InputData::n_dofs+j) += ffMatrix(i,j);
          matrix.coeffRef(elemNeighborPair.first* InputData::n_dofs+i, elemNeighborPair.second*InputData::n_dofs+j) += bfMatrix(i,j);
          matrix.coeffRef(elemNeighborPair.second*InputData::n_dofs+i, elemNeighborPair.first* InputData::n_dofs+j) += fbMatrix(i,j);
        }
    }
  }
}
   
   
   
