PetscSolverFeti.cc

//
// Software License for AMDiS
//
// Copyright (c) 2010 Dresden University of Technology 
// All rights reserved.
// Authors: Simon Vey, Thomas Witkowski et al.
//
// This file is part of AMDiS
//
// See also license.opensource.txt in the distribution.


#include "AMDiS.h"
#include "MatrixVector.h"
#include "parallel/PetscHelper.h"
#include "parallel/PetscSolverFeti.h"
#include "parallel/PetscSolverFetiDebug.h"
#include "parallel/PetscSolverFetiMonitor.h"
#include "parallel/PetscSolverFetiStructs.h"
#include "parallel/PetscSolverFetiOperators.h"
#include "parallel/PetscSolverFetiTimings.h"
#include "parallel/StdMpi.h"
#include "parallel/MpiHelper.h"
#include "parallel/PetscSolverGlobalMatrix.h"
#include "io/VtkWriter.h"

namespace AMDiS {

  using namespace std;

  PetscSolverFeti::PetscSolverFeti()
    : PetscSolver(),
      schurPrimalSolver(0),
      multiLevelTest(false),
      subdomain(NULL),
      massMatrixSolver(NULL),
      meshLevel(0),
      rStartInterior(0),
      nGlobalOverallInterior(0),
      printTimings(false),
      stokesMode(false),
      augmentedLagrange(false),
      pressureComponent(-1)
  {
    FUNCNAME("PetscSolverFeti::PetscSolverFeti()");

    string preconditionerName = "";
    Parameters::get("parallel->solver->precon", preconditionerName);
    if (preconditionerName == "" || preconditionerName == "none") {
      MSG("Create FETI-DP solver with no preconditioner!\n");
      fetiPreconditioner = FETI_NONE;
    } else if (preconditionerName == "dirichlet") {
      MSG("Create FETI-DP solver with Dirichlet preconditioner!\n");
      fetiPreconditioner = FETI_DIRICHLET;
    } else if (preconditionerName == "lumped") {
      MSG("Create FETI-DP solver with lumped preconditioner!\n");
      fetiPreconditioner = FETI_LUMPED;
    } else {
      ERROR_EXIT("Preconditioner \"%s\" not available!\n", 
		 preconditionerName.c_str());
    }

    Parameters::get("parallel->feti->schur primal solver", schurPrimalSolver);
    TEST_EXIT(schurPrimalSolver == 0 || schurPrimalSolver == 1)
      ("Wrong solver \"%d\"for the Schur primal complement!\n", 
       schurPrimalSolver);

    Parameters::get("parallel->multi level test", multiLevelTest);
    if (multiLevelTest)
      meshLevel = 1;

    Parameters::get("parallel->print timings", printTimings);

    Parameters::get("parallel->feti->augmented lagrange", augmentedLagrange);
  }


  void PetscSolverFeti::initialize(vector<const FiniteElemSpace*> feSpaces)
  {
    FUNCNAME("PetscSolverFeti::initialize()");

    TEST_EXIT_DBG(meshLevel + 1 == meshDistributor->getMeshLevelData().getLevelNumber())
      ("Mesh hierarchy does not contain %d levels!\n", meshLevel + 1);

    MeshLevelData& levelData = meshDistributor->getMeshLevelData();
    vector<const FiniteElemSpace*>& uniqueFe = meshDistributor->getFeSpaces();


    stokesMode = false;
    Parameters::get("parallel->feti->stokes mode", stokesMode);
    if (stokesMode) {
      Parameters::get("parallel->feti->pressure component", pressureComponent);
      TEST_EXIT(pressureComponent >= 0)
	("FETI-DP in Stokes mode, no pressure component defined!\n");

      pressureFeSpace = feSpaces[pressureComponent];
    }
			   
    if (subdomain == NULL) {
      subdomain = new PetscSolverGlobalMatrix();

      if (meshLevel == 0) {
	subdomain->setMeshDistributor(meshDistributor, 
				      mpiCommGlobal, mpiCommLocal);
      } else {
	subdomain->setMeshDistributor(meshDistributor, 
				      levelData.getMpiComm(meshLevel - 1),
				      levelData.getMpiComm(meshLevel));
	subdomain->setLevel(meshLevel);
      }
    }

    primalDofMap.init(levelData, feSpaces, uniqueFe);   
    dualDofMap.init(levelData, feSpaces, uniqueFe, false);
    localDofMap.init(levelData, feSpaces, uniqueFe, meshLevel != 0);
    lagrangeMap.init(levelData, feSpaces, uniqueFe);

    if (stokesMode)
      interfaceDofMap.init(levelData, feSpaces, uniqueFe);

    if (fetiPreconditioner == FETI_DIRICHLET) {
      TEST_EXIT(meshLevel == 0)
	("Dirichlet preconditioner not yet implemented for multilevel FETI-DP\n");

      interiorDofMap.init(levelData, feSpaces, uniqueFe, false);
    }
  }


  void PetscSolverFeti::createDirichletData(Matrix<DOFMatrix*> &mat)
  {
    FUNCNAME("PetscSolverFeti::createDirichletData()");

    bool removeDirichletRows = false;
    Parameters::get("parallel->feti->remove dirichlet", removeDirichletRows);
    if (!removeDirichletRows)
      return;

    int nComponents = mat.getSize();
    for (int i = 0; i < nComponents; i++) {
      DOFMatrix* dofMat = mat[i][i];
      if (!dofMat)
	continue;
      
      const FiniteElemSpace *feSpace = dofMat->getRowFeSpace();
      std::set<DegreeOfFreedom>& dRows = dofMat->getDirichletRows();
      if (dirichletRows.count(feSpace)) {
	// === Run test if Dirichlet rows are all the same in all components ===
	// === of one FE space.                                              ===
	TEST_EXIT(dirichletRows[feSpace].size() == dRows.size())
	  ("Wrong number of dirichlet rows!\n");
	
	for (std::set<DegreeOfFreedom>::iterator it0 = dirichletRows[feSpace].begin(),
	       it1 = dRows.begin(); it1 != dRows.end(); ++it0, ++it1) {
	  TEST_EXIT(*it0 == *it1)("Wrong DOFs %d %d!\n", *it0, *it1);
	}
      } else {
	dirichletRows[feSpace] = dRows;
      }
    }
  }


  void PetscSolverFeti::createFetiData()
  {
    FUNCNAME("PetscSolverFeti::createFetiData()");

    double timeCounter = MPI::Wtime();

    TEST_EXIT(meshDistributor)("No mesh distributor object defined!\n");
    TEST_EXIT(meshDistributor->getFeSpaces().size() > 0)
      ("No FE space defined in mesh distributor!\n");

    MeshLevelData& levelData = meshDistributor->getMeshLevelData();

    primalDofMap.clear();
    dualDofMap.clear();
    lagrangeMap.clear();
    localDofMap.clear();
    if (fetiPreconditioner == FETI_DIRICHLET)
      interiorDofMap.clear();

    primalDofMap.setDofComm(meshDistributor->getDofComm());
    lagrangeMap.setDofComm(meshDistributor->getDofComm());

    primalDofMap.setMpiComm(levelData.getMpiComm(0), 0);
    dualDofMap.setMpiComm(levelData.getMpiComm(0), 0);
    lagrangeMap.setMpiComm(levelData.getMpiComm(0), 0);
    localDofMap.setMpiComm(levelData.getMpiComm(meshLevel), meshLevel);
    if (fetiPreconditioner == FETI_DIRICHLET)
      interiorDofMap.setMpiComm(levelData.getMpiComm(meshLevel), meshLevel);

    if (meshLevel == 0)
      localDofMap.setDofComm(meshDistributor->getDofComm());
    else
      localDofMap.setDofComm(meshDistributor->getDofCommSd());

    if (stokesMode) {
      interfaceDofMap.clear();
      interfaceDofMap.setDofComm(meshDistributor->getDofComm());
      interfaceDofMap.setMpiComm(levelData.getMpiComm(0), 0);
    }

    for (unsigned int i = 0; i < meshDistributor->getFeSpaces().size(); i++) {
      const FiniteElemSpace *feSpace = meshDistributor->getFeSpace(i);
    
      createPrimals(feSpace);  

      createDuals(feSpace);

      createInterfaceNodes(feSpace);

      createIndexB(feSpace);     
    }

    primalDofMap.update();
    dualDofMap.update();
    localDofMap.update();
    if (fetiPreconditioner == FETI_DIRICHLET)
      interiorDofMap.update();

    if (stokesMode)
      interfaceDofMap.update();

    for (unsigned int i = 0; i < meshDistributor->getFeSpaces().size(); i++) {
      const FiniteElemSpace *feSpace = meshDistributor->getFeSpace(i);
      createLagrange(feSpace);
      createAugmentedLagrange(feSpace);
    }

    lagrangeMap.update();


    // === ===

    if (meshLevel == 0) {
      rStartInterior = 0;
      nGlobalOverallInterior = localDofMap.getOverallDofs();
    } else {
      MeshLevelData& levelData = meshDistributor->getMeshLevelData();

      int groupRowsInterior = 0;
      if (levelData.getMpiComm(1).Get_rank() == 0)
	groupRowsInterior = localDofMap.getOverallDofs();

      mpi::getDofNumbering(mpiCommGlobal, groupRowsInterior,
			   rStartInterior, nGlobalOverallInterior);

      int tmp = 0;
      if (levelData.getMpiComm(1).Get_rank() == 0)
	tmp = rStartInterior;

      levelData.getMpiComm(1).Allreduce(&tmp, &rStartInterior, 1, MPI_INT, MPI_SUM);
    }

    MSG("FETI-DP data created on mesh level %d\n", meshLevel);
    for (unsigned int i = 0; i < meshDistributor->getFeSpaces().size(); i++) {
      const FiniteElemSpace *feSpace = meshDistributor->getFeSpace(i);
      MSG("FETI-DP data for %d-ith FE space: %p\n", i, feSpace);

      if (feSpace == pressureFeSpace) {
	MSG("  nRankInterface = %d  nOverallInterface = %d\n",
	    interfaceDofMap[feSpace].nRankDofs, 
	    interfaceDofMap[feSpace].nOverallDofs);
      } else {
	MSG("  nRankPrimals = %d   nLocalPrimals = %d  nOverallPrimals = %d\n", 
	    primalDofMap[feSpace].nRankDofs, 
	    primalDofMap[feSpace].nLocalDofs,
	    primalDofMap[feSpace].nOverallDofs);
	
	MSG("  nRankDuals = %d  nOverallDuals = %d\n",
	    dualDofMap[feSpace].nRankDofs, 
	    dualDofMap[feSpace].nOverallDofs);
	
	MSG("  nRankLagrange = %d  nOverallLagrange = %d\n",
	    lagrangeMap[feSpace].nRankDofs, 
	    lagrangeMap[feSpace].nOverallDofs);

// 	TEST_EXIT_DBG(localDofMap[feSpace].size() + primalDofMap[feSpace].size() == 
// 		      static_cast<unsigned int>(feSpace->getAdmin()->getUsedDofs()))
// 	  ("Should not happen!\n");	
      }
    }

    subdomain->setDofMapping(&localDofMap);
    subdomain->setCoarseSpaceDofMapping(&primalDofMap); 
    if (stokesMode)
      subdomain->setCoarseSpaceDofMapping(&interfaceDofMap, pressureComponent);

    if (printTimings) {
      MPI::COMM_WORLD.Barrier();
      timeCounter = MPI::Wtime() - timeCounter;
      MSG("FETI-DP timing 01: %.5f seconds (creation of basic data structures)\n", 
	  timeCounter);
    }
  }


  void PetscSolverFeti::createPrimals(const FiniteElemSpace *feSpace)
  {
    FUNCNAME("PetscSolverFeti::createPrimals()");  

    if (feSpace == pressureFeSpace)
      return;

    // === Define all vertices on the interior boundaries of the macro mesh ===
    // === to be primal variables.                                          ===

    // Set of DOF indices that are considered to be primal variables.
    DofContainerSet& vertices = 
      meshDistributor->getBoundaryDofInfo(feSpace, meshLevel).geoDofs[VERTEX];

    DofIndexSet primals;
    for (DofContainerSet::iterator it = vertices.begin(); 
	 it != vertices.end(); ++it) {
      if (dirichletRows[feSpace].count(**it))
	continue;

      if (meshLevel == 0) {
	primals.insert(**it);
      } else {
	double e = 1e-8;
	WorldVector<double> c;
	feSpace->getMesh()->getDofIndexCoords(*it, feSpace, c);

	if (fabs(c[0]) < e || fabs(c[1]) < e ||
	    fabs(c[0] - 25.0) < e || fabs(c[1] - 25.0) < e ||
	    (fabs(c[0] - 12.5) < e && fabs(c[1] - 12.5) < e)) {
	  MSG("PRIMAL COORD %f %f\n", c[0], c[1]);
	  primals.insert(**it);
	}
      }
    }


    // === Calculate the number of primals that are owned by the rank and ===
    // === create local indices of the primals starting at zero.          ===

    for (DofIndexSet::iterator it = primals.begin(); it != primals.end(); ++it)
      if (meshDistributor->getDofMap()[feSpace].isRankDof(*it))
	primalDofMap[feSpace].insertRankDof(*it);
      else
  	primalDofMap[feSpace].insertNonRankDof(*it);
  }


  void PetscSolverFeti::createDuals(const FiniteElemSpace *feSpace)
  {
    FUNCNAME("PetscSolverFeti::createDuals()");

    if (feSpace == pressureFeSpace)
      return;

    // === Create global index of the dual nodes on each rank. ===

    DofContainer allBoundaryDofs;
    meshDistributor->getAllBoundaryDofs(feSpace, meshLevel, allBoundaryDofs);
    
    for (DofContainer::iterator it = allBoundaryDofs.begin();
	 it != allBoundaryDofs.end(); ++it) {
      if (dirichletRows[feSpace].count(**it))
	continue;

      if (isPrimal(feSpace, **it))
	continue;

      if (meshLevel == 0) {
	dualDofMap[feSpace].insertRankDof(**it);
      } else {
	if (meshDistributor->getDofMapSd()[feSpace].isRankDof(**it))
	  dualDofMap[feSpace].insertRankDof(**it);
      }	  
    }
  }

  
  void PetscSolverFeti::createInterfaceNodes(const FiniteElemSpace *feSpace)
  {
    FUNCNAME("PetscSolverFeti::createInterfaceNodes()");

    if (feSpace != pressureFeSpace)
      return;

    DofContainer allBoundaryDofs;
    meshDistributor->getAllBoundaryDofs(feSpace, meshLevel, allBoundaryDofs);

    for (DofContainer::iterator it = allBoundaryDofs.begin();
	 it != allBoundaryDofs.end(); ++it) {
      if (dirichletRows[feSpace].count(**it))
	continue;      
      
      if (meshDistributor->getDofMap()[feSpace].isRankDof(**it))
	interfaceDofMap[feSpace].insertRankDof(**it);
      else
	interfaceDofMap[feSpace].insertNonRankDof(**it);      
    }
  }


  void PetscSolverFeti::createLagrange(const FiniteElemSpace *feSpace)
  {
    FUNCNAME("PetscSolverFeti::createLagrange()");

    if (feSpace == pressureFeSpace)
      return;

    boundaryDofRanks[feSpace].clear();

    // Stores for all rank owned communication DOFs, if the counterpart is
    // a rank owned DOF in its subdomain. Thus, the following map stores to
    // each rank number all DOFs that fulfill this requirenment.
    map<int, std::set<DegreeOfFreedom> > sdRankDofs;

    if (meshLevel > 0) {
      StdMpi<vector<int> > stdMpi(mpiCommGlobal);

      for (DofComm::Iterator it(meshDistributor->getDofComm().getRecvDofs(), 
				meshLevel, feSpace);
	   !it.end(); it.nextRank()) {

	vector<int> subdomainRankDofs;
	subdomainRankDofs.reserve(it.getDofs().size());

	for (; !it.endDofIter(); it.nextDof()) {
	  if (meshDistributor->getDofMapSd()[feSpace].isRankDof(it.getDofIndex()))
	    subdomainRankDofs.push_back(1);
	  else
	    subdomainRankDofs.push_back(0);
	}

	stdMpi.send(it.getRank(), subdomainRankDofs);
      }	     

      for (DofComm::Iterator it(meshDistributor->getDofComm().getSendDofs(), 
				meshLevel, feSpace);
	   !it.end(); it.nextRank())
	stdMpi.recv(it.getRank());

      stdMpi.startCommunication();

      for (DofComm::Iterator it(meshDistributor->getDofComm().getSendDofs(), 
				meshLevel, feSpace); 
	   !it.end(); it.nextRank())
	for (; !it.endDofIter(); it.nextDof())
	  if (!isPrimal(feSpace, it.getDofIndex()))
	    if (stdMpi.getRecvData(it.getRank())[it.getDofCounter()] == 1)
	      sdRankDofs[it.getRank()].insert(it.getDofIndex());
    }

    if (dualDofMap[feSpace].nLocalDofs == 0)
      return;


    // === Create for each dual node that is owned by the rank, the set ===
    // === of ranks that contain this node (denoted by W(x_j)).         ===

    int mpiRank = meshDistributor->getMpiRank();
    for (DofComm::Iterator it(meshDistributor->getDofComm().getSendDofs(), 
			      meshLevel, feSpace); 
	 !it.end(); it.nextRank()) {
      for (; !it.endDofIter(); it.nextDof()) {
	if (!isPrimal(feSpace, it.getDofIndex())) {
	  boundaryDofRanks[feSpace][it.getDofIndex()].insert(mpiRank);

 	  if (meshLevel == 0 ||
 	      (meshLevel > 0 && sdRankDofs[it.getRank()].count(it.getDofIndex())))
	    boundaryDofRanks[feSpace][it.getDofIndex()].insert(it.getRank());	  
	}
      }
    }


    // === Communicate these sets for all rank owned dual nodes to other ===
    // === ranks that also have this node.                               ===

    StdMpi<vector<std::set<int> > > stdMpi(meshDistributor->getMpiComm());

    for (DofComm::Iterator it(meshDistributor->getDofComm().getSendDofs(), meshLevel, feSpace);
	 !it.end(); it.nextRank())
      for (; !it.endDofIter(); it.nextDof())
	if (!isPrimal(feSpace, it.getDofIndex()))
 	  if (meshLevel == 0 ||
 	      (meshLevel > 0 && sdRankDofs[it.getRank()].count(it.getDofIndex())))
	    stdMpi.getSendData(it.getRank()).push_back(boundaryDofRanks[feSpace][it.getDofIndex()]);

    stdMpi.updateSendDataSize();

    for (DofComm::Iterator it(meshDistributor->getDofComm().getRecvDofs(), meshLevel, feSpace); 
	 !it.end(); it.nextRank()) {
      bool recvFromRank = false;
      for (; !it.endDofIter(); it.nextDof()) {
	if (!isPrimal(feSpace, it.getDofIndex())) {
 	  if (meshLevel == 0 ||
 	      (meshLevel > 0 && 
 	       meshDistributor->getDofMapSd()[feSpace].isRankDof(it.getDofIndex()))) {
	    recvFromRank = true;
	    break;
	  }
	}
      }

      if (recvFromRank)
	stdMpi.recv(it.getRank());
    }

    stdMpi.startCommunication();

    for (DofComm::Iterator it(meshDistributor->getDofComm().getRecvDofs(), meshLevel, feSpace); 
	 !it.end(); it.nextRank()) {
      int i = 0;
      for (; !it.endDofIter(); it.nextDof())
	if (!isPrimal(feSpace, it.getDofIndex()))
 	  if (meshLevel == 0 ||
 	      (meshLevel > 0 && 
 	       meshDistributor->getDofMapSd()[feSpace].isRankDof(it.getDofIndex())))	    
	    boundaryDofRanks[feSpace][it.getDofIndex()] = 
	      stdMpi.getRecvData(it.getRank())[i++];
	  else
	    lagrangeMap[feSpace].insertNonRankDof(it.getDofIndex());
    }


    // === Reserve for each dual node, on the rank that owns this node, the ===
    // === appropriate number of Lagrange constraints.                      ===

    int nRankLagrange = 0;
    DofMap& dualMap = dualDofMap[feSpace].getMap();
    for (DofMap::iterator it = dualMap.begin(); it != dualMap.end(); ++it) {
      if (meshDistributor->getDofMap()[feSpace].isRankDof(it->first)) {
	lagrangeMap[feSpace].insertRankDof(it->first, nRankLagrange);
	int degree = boundaryDofRanks[feSpace][it->first].size();
	nRankLagrange += (degree * (degree - 1)) / 2;
      } else {
	lagrangeMap[feSpace].insertNonRankDof(it->first);
      }
    }
    lagrangeMap[feSpace].nRankDofs = nRankLagrange;
  }


  void PetscSolverFeti::createAugmentedLagrange(const FiniteElemSpace *feSpace)
  {
    FUNCNAME("PetscSolverFeti::createAugmentedLagrange()");

    if (!augmentedLagrange)
      return;
  }


  void PetscSolverFeti::createIndexB(const FiniteElemSpace *feSpace)
  {
    FUNCNAME("PetscSolverFeti::createIndexB()");

    DOFAdmin* admin = feSpace->getAdmin();

    // === To ensure that all interior node on each rank are listen first in ===
    // === the global index of all B nodes, insert all interior nodes first, ===
    // === without defining a correct index.                                 ===

    int nLocalInterior = 0;    
    for (int i = 0; i < admin->getUsedSize(); i++) {
      if (admin->isDofFree(i) ||
	  isPrimal(feSpace, i) ||
	  isDual(feSpace, i) ||
	  isInterface(feSpace, i) ||
	  dirichletRows[feSpace].count(i))
	continue;      

      if (meshLevel == 0) {
	localDofMap[feSpace].insertRankDof(i, nLocalInterior);
	
	if (fetiPreconditioner == FETI_DIRICHLET)
	  interiorDofMap[feSpace].insertRankDof(i, nLocalInterior);
	
	nLocalInterior++;	
      } else {
	if (meshDistributor->getDofMapSd()[feSpace].isRankDof(i))
	  localDofMap[feSpace].insertRankDof(i);
	else
	  localDofMap[feSpace].insertNonRankDof(i);
	
	TEST_EXIT_DBG(fetiPreconditioner == FETI_NONE)
	  ("Not yet implemnted!\n");	
      }
    }
    
    // === And finally, add the global indicies of all dual nodes. ===

    for (DofMap::iterator it = dualDofMap[feSpace].getMap().begin();
	 it != dualDofMap[feSpace].getMap().end(); ++it) {
      if (meshLevel == 0) {
	localDofMap[feSpace].insertRankDof(it->first);
      } else {
	if (meshDistributor->getDofMapSd()[feSpace].isRankDof(it->first))
	  localDofMap[feSpace].insertRankDof(it->first);
	else 
	  localDofMap[feSpace].insertNonRankDof(it->first);
      }
    }
  }


  void PetscSolverFeti::createMatLagrange(vector<const FiniteElemSpace*> &feSpaces)
  {
    FUNCNAME("PetscSolverFeti::createMatLagrange()");

    double wtime = MPI::Wtime();
    int mpiRank = meshDistributor->getMpiRank();

    // === Create distributed matrix for Lagrange constraints. ===

    MatCreateAIJ(mpiCommGlobal,
		 lagrangeMap.getRankDofs(), localDofMap.getRankDofs(),
		 lagrangeMap.getOverallDofs(), nGlobalOverallInterior,
		 2, PETSC_NULL, 2, PETSC_NULL,
		 &mat_lagrange);
    MatSetOption(mat_lagrange, MAT_NEW_NONZERO_ALLOCATION_ERR, PETSC_FALSE);

    Vec vec_scale_lagrange;
    lagrangeMap.createVec(vec_scale_lagrange);

    // === Create for all duals the corresponding Lagrange constraints. On ===
    // === each rank we traverse all pairs (n, m) of ranks, with n < m,    ===
    // === that contain this node. If the current rank number is r, and    ===
    // === n == r, the rank sets 1.0 for the corresponding constraint, if  ===
    // === m == r, than the rank sets -1.0 for the corresponding           ===
    // === constraint.                                                     ===

    for (unsigned int k = 0; k < feSpaces.size(); k++) {
      DofMap &dualMap = dualDofMap[feSpaces[k]].getMap();

      for (DofMap::iterator it = dualMap.begin(); it != dualMap.end(); ++it) {
	TEST_EXIT_DBG(boundaryDofRanks[feSpaces[k]].count(it->first))
	  ("Should not happen!\n");
	
	// Global index of the first Lagrange constriant for this node.
	int index = lagrangeMap.getMatIndex(k, it->first);

	// Copy set of all ranks that contain this dual node.
	vector<int> W(boundaryDofRanks[feSpaces[k]][it->first].begin(), 
		      boundaryDofRanks[feSpaces[k]][it->first].end());
	// Number of ranks that contain this dual node.
	int degree = W.size();

	TEST_EXIT_DBG(degree > 1)("Should not happen!\n");
	
	int counter = 0;
	for (int i = 0; i < degree; i++) {
	  for (int j = i + 1; j < degree; j++) {
	    if (W[i] == mpiRank || W[j] == mpiRank) {
	      MatSetValue(mat_lagrange, 
			  index + counter, 
			  localDofMap.getMatIndex(k, it->first) + rStartInterior,
			  (W[i] == mpiRank ? 1.0 : -1.0),
			  INSERT_VALUES);
	    }
	    counter++;
	  }
	}

	// === Create scaling factors for scaling the lagrange matrix, which ===
	// === is required for FETI-DP preconditioners.                      ===
	
	if (meshDistributor->getDofMap()[feSpaces[k]].isRankDof(it->first)) {
	  int nConstraints = (degree * (degree - 1)) / 2;
	  for (int i = 0; i < nConstraints; i++) {
	    VecSetValue(vec_scale_lagrange,
			index + i,
			1.0 / static_cast<double>(degree),
			INSERT_VALUES);
	  }
	}
      }
    }

    MatAssemblyBegin(mat_lagrange, MAT_FINAL_ASSEMBLY);
    MatAssemblyEnd(mat_lagrange, MAT_FINAL_ASSEMBLY);


    // === If required, create \ref mat_lagrange_scaled ===

    VecAssemblyBegin(vec_scale_lagrange);
    VecAssemblyEnd(vec_scale_lagrange);

    if (fetiPreconditioner != FETI_NONE || stokesMode) {
      MatDuplicate(mat_lagrange, MAT_COPY_VALUES, &mat_lagrange_scaled);
      MatDiagonalScale(mat_lagrange_scaled, vec_scale_lagrange, PETSC_NULL);
    }

    VecDestroy(&vec_scale_lagrange);


    // === Print final timings. ===

    if (printTimings) {
      MPI::COMM_WORLD.Barrier();
      MSG("FETI-DP timing 05: %.5f seconds (creation of lagrange constraint matrix)\n", 
	  MPI::Wtime() - wtime);
    }
  }

  bool PetscSolverFeti::testWirebasketEdge(BoundaryObject &edge, const FiniteElemSpace *feSpace)
  {
    Element *el = edge.el;
    int i0 = el->getVertexOfEdge(edge.ithObj, 0);
    int i1 = el->getVertexOfEdge(edge.ithObj, 1);
    DegreeOfFreedom d0 = el->getDof(i0, 0);
    DegreeOfFreedom d1 = el->getDof(i1, 0);
    WorldVector<double> c0, c1;
    el->getMesh()->getDofIndexCoords(d0, feSpace, c0);
    el->getMesh()->getDofIndexCoords(d1, feSpace, c1);
    bool xe = fabs(c0[0] - c1[0]) < 1e-8;
    bool ye = fabs(c0[1] - c1[1]) < 1e-8;
    bool ze = fabs(c0[2] - c1[2]) < 1e-8;
    int counter = static_cast<int>(xe) + static_cast<int>(ye) + static_cast<int>(ze);
    return (counter == 2);
  }

  void PetscSolverFeti::createMatAugmentedLagrange(vector<const FiniteElemSpace*> &feSpaces)
  {
    FUNCNAME("PetscSolverFeti::createMatAugmentedLagrange()");

    if (!augmentedLagrange)
      return;

    double wtime = MPI::Wtime();

    nOverallEdges = 0;
    InteriorBoundary &intBound = meshDistributor->getIntBoundary();
    std::set<BoundaryObject> allEdges;
    for (InteriorBoundary::iterator it(intBound.getOwn()); !it.end(); ++it)
      if (it->rankObj.subObj == EDGE && 
	  testWirebasketEdge(it->rankObj, feSpaces[0]) && 
	  allEdges.count(it->rankObj) == 0)
	allEdges.insert(it->rankObj);

    nRankEdges = allEdges.size();    
    int rStartEdges = 0;
    mpi::getDofNumbering(mpiCommGlobal, nRankEdges, rStartEdges, nOverallEdges);

    MSG("nRankEdges = %d, nOverallEdges = %d\n", nRankEdges, nOverallEdges);

    nRankEdges *= feSpaces.size();
    rStartEdges *= feSpaces.size();
    nOverallEdges *= feSpaces.size();

    MatCreateAIJ(mpiCommGlobal,
		 nRankEdges, lagrangeMap.getRankDofs(),
		 nOverallEdges, lagrangeMap.getOverallDofs(),
		 1, PETSC_NULL, 1, PETSC_NULL, 
		 &mat_augmented_lagrange);
    MatSetOption(mat_augmented_lagrange, MAT_NEW_NONZERO_ALLOCATION_ERR, PETSC_FALSE);

    int rowCounter = rStartEdges;
    for (std::set<BoundaryObject>::iterator edgeIt = allEdges.begin(); 
	 edgeIt != allEdges.end(); ++edgeIt) {

      for (int i = 0; i < feSpaces.size(); i++) {
	DofContainer edgeDofs;
	edgeIt->el->getAllDofs(feSpaces[i], *edgeIt, edgeDofs);
	
	MSG("SIZE = %d\n", edgeDofs.size());
	
	for (DofContainer::iterator it = edgeDofs.begin();
	     it != edgeDofs.end(); ++it) {
	  TEST_EXIT_DBG(isPrimal(feSpaces[i], **it) == false)
	    ("Should not be primal!\n");
	  
	  if (dirichletRows[feSpaces[i]].count(**it))
	    continue;
	  
	  int col = lagrangeMap.getMatIndex(i, **it);
	  double value = 1.0;
	  MatSetValue(mat_augmented_lagrange, rowCounter, col, value, INSERT_VALUES);
	}
	
	rowCounter++;
      }      
    }

    MatAssemblyBegin(mat_augmented_lagrange, MAT_FINAL_ASSEMBLY);
    MatAssemblyEnd(mat_augmented_lagrange, MAT_FINAL_ASSEMBLY);

    if (printTimings) {
      MPI::COMM_WORLD.Barrier();
      MSG("FETI-DP timing 05a: %.5f seconds (creation of augmented lagrange constraint matrix)\n", 
	  MPI::Wtime() - wtime);
    }
  }


  void PetscSolverFeti::createSchurPrimalKsp(vector<const FiniteElemSpace*> &feSpaces)
  {
    FUNCNAME("PetscSolverFeti::createSchurPrimalKsp()");

    if (schurPrimalSolver == 0) {
      MSG("Create iterative schur primal solver!\n");

      if (augmentedLagrange == false) {
	schurPrimalData.subSolver = subdomain;
	
	localDofMap.createVec(schurPrimalData.tmp_vec_b, nGlobalOverallInterior);
	primalDofMap.createVec(schurPrimalData.tmp_vec_primal);
	
	MatCreateShell(mpiCommGlobal,
		       primalDofMap.getRankDofs(), 
		       primalDofMap.getRankDofs(), 
		       primalDofMap.getOverallDofs(), 
		       primalDofMap.getOverallDofs(),
		       &schurPrimalData, 
		       &mat_schur_primal);
	MatShellSetOperation(mat_schur_primal, MATOP_MULT, 
			     (void(*)(void))petscMultMatSchurPrimal);	
      } else {
	schurPrimalAugmentedData.subSolver = subdomain;
	schurPrimalAugmentedData.nestedVec = true;

	localDofMap.createVec(schurPrimalAugmentedData.tmp_vec_b0, nGlobalOverallInterior);
	localDofMap.createVec(schurPrimalAugmentedData.tmp_vec_b1, nGlobalOverallInterior);
	primalDofMap.createVec(schurPrimalAugmentedData.tmp_vec_primal);
	lagrangeMap.createVec(schurPrimalAugmentedData.tmp_vec_lagrange);

	schurPrimalAugmentedData.mat_lagrange = &mat_lagrange;
	schurPrimalAugmentedData.mat_augmented_lagrange = &mat_augmented_lagrange;

	MatCreateShell(mpiCommGlobal,
		       primalDofMap.getRankDofs() + nRankEdges, 
		       primalDofMap.getRankDofs() + nRankEdges, 
		       primalDofMap.getOverallDofs() + nOverallEdges, 
		       primalDofMap.getOverallDofs() + nOverallEdges,
		       &schurPrimalAugmentedData, 
		       &mat_schur_primal);
	MatShellSetOperation(mat_schur_primal, MATOP_MULT, 
			     (void(*)(void))petscMultMatSchurPrimalAugmented);
      }

      KSPCreate(mpiCommGlobal, &ksp_schur_primal);
      KSPSetOperators(ksp_schur_primal, mat_schur_primal, mat_schur_primal, SAME_NONZERO_PATTERN);
      KSPSetOptionsPrefix(ksp_schur_primal, "schur_primal_");
      KSPSetType(ksp_schur_primal, KSPGMRES);
      KSPSetFromOptions(ksp_schur_primal);
    } else {
      MSG("Create direct schur primal solver!\n");

      double wtime = MPI::Wtime();

      TEST_EXIT_DBG(meshLevel == 0)
	("Does not support for multilevel, check usage of localDofMap.\n");


      // === Create explicit matrix representation of the Schur primal system. ===

      if (!augmentedLagrange)
	createMatExplicitSchurPrimal();
      else
	createMatExplicitAugmentedSchurPrimal();


      // === Create KSP solver object and set appropriate solver options. ====

      KSPCreate(mpiCommGlobal, &ksp_schur_primal);
      KSPSetOperators(ksp_schur_primal, mat_schur_primal, mat_schur_primal,
		      SAME_NONZERO_PATTERN);
      KSPSetOptionsPrefix(ksp_schur_primal, "schur_primal_");
      KSPSetType(ksp_schur_primal, KSPPREONLY);
      PC pc_schur_primal;      
      KSPGetPC(ksp_schur_primal, &pc_schur_primal);
      PCSetType(pc_schur_primal, PCLU);
      PCFactorSetMatSolverPackage(pc_schur_primal, MATSOLVERMUMPS);
      KSPSetFromOptions(ksp_schur_primal);


      // === And finally print timings, if required. ===

      if (printTimings) {
	MPI::COMM_WORLD.Barrier();
	MatInfo minfo;
	MatGetInfo(mat_schur_primal, MAT_GLOBAL_SUM, &minfo);
	MSG("Schur primal matrix nnz = %f\n", minfo.nz_used);
	
	MSG("FETI-DP timing 06: %.5f seconds (creation of schur primal matrix)\n",
	    MPI::Wtime() - wtime);

	wtime = MPI::Wtime();
	KSPSetUp(ksp_schur_primal);
	KSPSetUpOnBlocks(ksp_schur_primal);
	MPI::COMM_WORLD.Barrier();
	MSG("FETI-DP timing 07: %.5f seconds (factorization of primal schur matrix).\n",
	    MPI::Wtime() - wtime);
      }
    }
  }


  void PetscSolverFeti::createMatExplicitSchurPrimal()
  {
    FUNCNAME("PetscSolverFeti::createMatExplicitSchurPrimal()");

    int creationMode = 0;
    Parameters::get("parallel->feti->schur primal creation mode", creationMode);
    if (creationMode == 0) {
      // matK = inv(A_BB) A_BPi
      Mat matK;
      petsc_helper::blockMatMatSolve(subdomain->getSolver(), 
				     subdomain->getMatInteriorCoarse(),
				     matK);
      
      
      // mat_schur_primal = A_PiPi - A_PiB inv(A_BB) A_BPi
      //                  = A_PiPi - A_PiB matK
      MatMatMult(subdomain->getMatCoarseInterior(), matK, MAT_INITIAL_MATRIX, 
		 PETSC_DEFAULT, &mat_schur_primal);
      MatAYPX(mat_schur_primal, -1.0, subdomain->getMatCoarse(), DIFFERENT_NONZERO_PATTERN);
      
      MatDestroy(&matK);
    } else {
      Mat tmp;
      
      schurPrimalData.subSolver = subdomain;
      
      localDofMap.createVec(schurPrimalData.tmp_vec_b, nGlobalOverallInterior);
      primalDofMap.createVec(schurPrimalData.tmp_vec_primal);
      
      MatCreateShell(mpiCommGlobal,
		     primalDofMap.getRankDofs(), 
		     primalDofMap.getRankDofs(), 
		     primalDofMap.getOverallDofs(), 
		     primalDofMap.getOverallDofs(),
		     &schurPrimalData, 
		     &tmp);
      MatShellSetOperation(tmp, MATOP_MULT, 
			   (void(*)(void))petscMultMatSchurPrimal);
      MatComputeExplicitOperator(tmp, &mat_schur_primal);

      MatDestroy(&tmp);
      schurPrimalData.subSolver = NULL;
      VecDestroy(&schurPrimalData.tmp_vec_b);
      VecDestroy(&schurPrimalData.tmp_vec_primal);
    }
  }
  
  
  void PetscSolverFeti::createMatExplicitAugmentedSchurPrimal()
  {
    FUNCNAME("PetscSolverFeti::createMatExplicitAugmentedSchurPrimal()");

    int creationMode = 1;
    Parameters::get("parallel->feti->schur primal creation mode", creationMode);
    if (creationMode == 0) {
      // qj = -Q J
      Mat qj;
      MatMatMult(mat_augmented_lagrange, mat_lagrange, MAT_INITIAL_MATRIX,
		 PETSC_DEFAULT, &qj);
      MatScale(qj, -1.0);
      
      
      // matTmp = inv(A_BB) A_BPi
      Mat matTmp;
      petsc_helper::blockMatMatSolve(subdomain->getSolver(), 
				     subdomain->getMatInteriorCoarse(),
				     matTmp);
      
      
      // mat00 = A_PiPi - A_PiB inv(A_BB) A_BPi
      //       = A_PiPi - A_PiB matTmp
      Mat mat00;
      MatMatMult(subdomain->getMatCoarseInterior(), matTmp, MAT_INITIAL_MATRIX, 
		 PETSC_DEFAULT, &mat00);
      MatAYPX(mat00, -1.0, subdomain->getMatCoarse(), DIFFERENT_NONZERO_PATTERN);
      
      
      // mat10 = -Q J inv(A_BB) A_BPi
      //       = qj matTmp
      Mat mat10;
      MatMatMult(qj, matTmp, MAT_INITIAL_MATRIX, PETSC_DEFAULT, &mat10); 
      
      
      // matTmp = inv(A_BB) trans(J) trans(Q)
      Mat qT, jTqT;
      MatTranspose(mat_augmented_lagrange, MAT_INITIAL_MATRIX, &qT);
      MatTransposeMatMult(mat_lagrange, qT, MAT_INITIAL_MATRIX, PETSC_DEFAULT, 
			  &jTqT);
      petsc_helper::blockMatMatSolve(subdomain->getSolver(), jTqT, matTmp);
      MatDestroy(&qT);
      MatDestroy(&jTqT);
      
      // mat01 = -A_PiB inv(A_BB) trans(J) trans(Q)
      //       = -A_PiB matTmp
      Mat mat01;
      MatMatMult(subdomain->getMatCoarseInterior(), matTmp, MAT_INITIAL_MATRIX, 
		 PETSC_DEFAULT, &mat01);
      MatScale(mat01, -1.0);
      
      // mat11 = -Q J inv(A_BB) trans(J) trans(Q)
      //       = qj matTmp
      Mat mat11;
      MatMatMult(qj, matTmp, MAT_INITIAL_MATRIX, PETSC_DEFAULT, &mat11);
      
      MatDestroy(&matTmp);
      MatDestroy(&qj);