#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();
  const LiftingOperatorType::LiftingMatrix& R11 = FEManager.getLifting11(); 
  const LiftingOperatorType::LiftingMatrix& R12 = FEManager.getLifting12(); 
  const LiftingOperatorType::LiftingMatrix& R22 = FEManager.getLifting22(); 
  const LiftingOperatorType::LiftingMatrix& R21 = FEManager.getLifting21(); 

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

  for (SpaMesh::ElementIterator elemIt(mesh); elemIt; ++elemIt) 
  {
    int elem = elemIt.value();
    for (SpaMesh::NeighborIterator neighborIt(mesh,elemIt); neighborIt; ++neighborIt)
    {     
      int neighbor = neighborIt.value();
      if (elem < neighbor)
      {
        FEManager.initInternalFaceFE(mesh, elemIt, neighborIt, InputData::p_order, InputData::q_order);
        FEManager.computeInternalFaceLifting(); 
        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] * InputData::penalty * normalVec(j)(d) * bShapeMatrix.col(j) * (R11(d).transpose() * bShapeMatrix.col(j)).transpose(); 
            bbMatrix.noalias() += 0.5 * jacXweightVecSide[j] * InputData::penalty * normalVec(j)(d) * bShapeMatrix.col(j) * (R21(d).transpose() * fShapeMatrix.col(j)).transpose(); 
            ffMatrix.noalias() -= 0.5 * jacXweightVecSide[j] * InputData::penalty * normalVec(j)(d) * fShapeMatrix.col(j) * (R22(d).transpose() * fShapeMatrix.col(j)).transpose(); 
            ffMatrix.noalias() -= 0.5 * jacXweightVecSide[j] * InputData::penalty * normalVec(j)(d) * fShapeMatrix.col(j) * (R12(d).transpose() * bShapeMatrix.col(j)).transpose(); 

            bfMatrix.noalias() += 0.5 * jacXweightVecSide[j] * InputData::penalty * normalVec(j)(d) * bShapeMatrix.col(j) * (R22(d).transpose() * fShapeMatrix.col(j)).transpose(); 
            bfMatrix.noalias() += 0.5 * jacXweightVecSide[j] * InputData::penalty * normalVec(j)(d) * bShapeMatrix.col(j) * (R12(d).transpose() * bShapeMatrix.col(j)).transpose(); 
            fbMatrix.noalias() -= 0.5 * jacXweightVecSide[j] * InputData::penalty * normalVec(j)(d) * fShapeMatrix.col(j) * (R11(d).transpose() * bShapeMatrix.col(j)).transpose(); 
            fbMatrix.noalias() -= 0.5 * jacXweightVecSide[j] * InputData::penalty * normalVec(j)(d) * fShapeMatrix.col(j) * (R21(d).transpose() * fShapeMatrix.col(j)).transpose(); 
          }
          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());
        }
        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);
            matrix.coeffRef(neighbor*InputData::n_dofs+i, neighbor*InputData::n_dofs+j) += ffMatrix(i,j);
            matrix.coeffRef(elem*    InputData::n_dofs+i, neighbor*InputData::n_dofs+j) += bfMatrix(i,j);
            matrix.coeffRef(neighbor*InputData::n_dofs+i, elem*    InputData::n_dofs+j) += fbMatrix(i,j);
	  }
      }
    }
  }
}
   
   
   
