//
// 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 "parallel/PetscSolverGlobalMatrix.h"
#include "parallel/StdMpi.h"
#include "parallel/MpiHelper.h"

namespace AMDiS {

  void PetscSolverGlobalMatrix::fillPetscMatrix(Matrix<DOFMatrix*> *mat)
  {
    FUNCNAME("PetscSolverGlobalMatrix::fillPetscMatrix()");

    if (coarseSpaceMap != NULL) {
      updateSubdomainData();
      fillPetscMatrixWithCoarseSpace(mat);
      return;
    }

    TEST_EXIT_DBG(meshDistributor)("No mesh distributor object defined!\n");
    TEST_EXIT_DBG(interiorMap)("No parallel mapping object defined!\n");
    TEST_EXIT_DBG(mat)("No DOF matrix defined!\n");
    
    double wtime = MPI::Wtime();

    
    // === Check if mesh was changed and, in this case, recompute matrix ===
    // === nnz structure and matrix indices.                             ===

    int recvAllValues = 0;
    int sendValue = 
      static_cast<int>(meshDistributor->getLastMeshChangeIndex() != lastMeshNnz);
    mpiCommGlobal.Allreduce(&sendValue, &recvAllValues, 1, MPI_INT, MPI_SUM);

    if (!d_nnz || recvAllValues != 0 || alwaysCreateNnzStructure) {
      vector<const FiniteElemSpace*> feSpaces = getFeSpaces(mat);
      interiorMap->setComputeMatIndex(true, true);
      interiorMap->update(feSpaces);

      if (d_nnz) {
	delete [] d_nnz;
	d_nnz = NULL;
	delete [] o_nnz;
	o_nnz = NULL;
      }

      updateSubdomainData();
      createPetscNnzStructure(mat);
      lastMeshNnz = meshDistributor->getLastMeshChangeIndex();
    }


    // === Create PETSc vector (solution and a temporary vector). ===

    int nRankRows = interiorMap->getRankDofs();
    int nOverallRows = interiorMap->getOverallDofs();

    VecCreateMPI(mpiCommGlobal, nRankRows, nOverallRows, &petscSolVec);


    // === Create PETSc matrix with the computed nnz data structure. ===

    MatCreateAIJ(mpiCommGlobal, nRankRows, nRankRows, 
 		    nOverallRows, nOverallRows,
		    0, d_nnz, 0, o_nnz, &matIntInt);

#if (DEBUG != 0)
    MSG("Fill petsc matrix 1 needed %.5f seconds\n", MPI::Wtime() - wtime);
#endif

#if (DEBUG != 0)
    int a, b;
    MatGetOwnershipRange(matIntInt, &a, &b);
    TEST_EXIT(a == interiorMap->getStartDofs())("Wrong matrix ownership range!\n");
    TEST_EXIT(b == interiorMap->getStartDofs() + nRankRows)
      ("Wrong matrix ownership range!\n");
#endif


    // === Transfer values from DOF matrices to the PETSc matrix. === 

    int nComponents = mat->getNumRows();
    for (int i = 0; i < nComponents; i++)
      for (int j = 0; j < nComponents; j++)
	if ((*mat)[i][j])
	  setDofMatrix((*mat)[i][j], i, j);

#if (DEBUG != 0)
    MSG("Fill petsc matrix 2 needed %.5f seconds\n", MPI::Wtime() - wtime);
#endif

    MatAssemblyBegin(matIntInt, MAT_FINAL_ASSEMBLY);
    MatAssemblyEnd(matIntInt, MAT_FINAL_ASSEMBLY);


    // === Remove Dirichlet BC DOFs. ===

    //    removeDirichletBcDofs(mat);
    

    // === Init PETSc solver. ===

    KSPCreate(mpiCommGlobal, &kspInterior);
    KSPGetPC(kspInterior, &pcInterior);
    KSPSetOperators(kspInterior, matIntInt, matIntInt, SAME_NONZERO_PATTERN); 
    KSPSetTolerances(kspInterior, 0.0, 1e-8, PETSC_DEFAULT, PETSC_DEFAULT);
    KSPSetType(kspInterior, KSPBCGS);
    KSPSetOptionsPrefix(kspInterior, kspPrefix.c_str());
    KSPSetFromOptions(kspInterior);
    PCSetFromOptions(pcInterior);

    createFieldSplit(pcInterior);

    // Do not delete the solution vector, use it for the initial guess.
    if (!zeroStartVector)
      KSPSetInitialGuessNonzero(kspInterior, PETSC_TRUE);

    MSG("Fill petsc matrix needed %.5f seconds\n", MPI::Wtime() - wtime);
  }


  void PetscSolverGlobalMatrix::fillPetscMatrixWithCoarseSpace(Matrix<DOFMatrix*> *mat)
  {
    FUNCNAME("PetscSolverGlobalMatrix::fillPetscMatrixWithCoarseSpace()");
    
    vector<const FiniteElemSpace*> feSpaces = getFeSpaces(mat);

    int nRowsRankInterior = interiorMap->getRankDofs();
    int nRowsOverallInterior = interiorMap->getOverallDofs();

    if (subdomainLevel == 0) {
      MatCreateSeqAIJ(mpiCommLocal, nRowsRankInterior, nRowsRankInterior,
		      60, PETSC_NULL, &matIntInt);
    } else {
      MatCreateAIJ(mpiCommLocal, 
		   nRowsRankInterior, nRowsRankInterior,
		   nRowsOverallInterior, nRowsOverallInterior,
		   60, PETSC_NULL, 60, PETSC_NULL, &matIntInt);
    }

    if (coarseSpaceMap) {
      int nRowsRankCoarse = coarseSpaceMap->getRankDofs();
      int nRowsOverallCoarse = coarseSpaceMap->getOverallDofs();

      MatCreateAIJ(mpiCommGlobal,
		   nRowsRankCoarse, nRowsRankCoarse,
		   nRowsOverallCoarse, nRowsOverallCoarse,
		   60, PETSC_NULL, 60, PETSC_NULL, &matCoarseCoarse);
      
      MatCreateAIJ(mpiCommGlobal,
		   nRowsRankCoarse, nRowsRankInterior,
		   nRowsOverallCoarse, nGlobalOverallInterior,
		   60, PETSC_NULL, 60, PETSC_NULL, &matCoarseInt);
      
      MatCreateAIJ(mpiCommGlobal,
		   nRowsRankInterior, nRowsRankCoarse,
		   nGlobalOverallInterior, nRowsOverallCoarse,
		   60, PETSC_NULL, 60, PETSC_NULL, &matIntCoarse);
    }

    // === Prepare traverse of sequentially created matrices. ===

    using mtl::tag::row; using mtl::tag::nz; using mtl::begin; using mtl::end;
    namespace traits = mtl::traits;
    typedef DOFMatrix::base_matrix_type Matrix;

    typedef traits::range_generator<row, Matrix>::type cursor_type;
    typedef traits::range_generator<nz, cursor_type>::type icursor_type;

    vector<int> cols, colsOther;
    vector<double> values, valuesOther;
    cols.reserve(300);
    colsOther.reserve(300);
    values.reserve(300);
    valuesOther.reserve(300);

    // === Traverse all sequentially created matrices and add the values to ===
    // === the global PETSc matrices.                                       ===

    int nComponents = mat->getSize();
    for (int i = 0; i < nComponents; i++) {
      for (int j = 0; j < nComponents; j++) {
	if (!(*mat)[i][j])
	  continue;

	traits::col<Matrix>::type col((*mat)[i][j]->getBaseMatrix());
	traits::const_value<Matrix>::type value((*mat)[i][j]->getBaseMatrix());
	
	// Traverse all rows.
	for (cursor_type cursor = begin<row>((*mat)[i][j]->getBaseMatrix()), 
	       cend = end<row>((*mat)[i][j]->getBaseMatrix()); cursor != cend; ++cursor) {

	  bool rowPrimal = isCoarseSpace(feSpaces[i], *cursor);
  
	  cols.clear();
	  colsOther.clear();
	  values.clear();	  
	  valuesOther.clear();

	  // Traverse all columns.
	  for (icursor_type icursor = begin<nz>(cursor), icend = end<nz>(cursor); 
	       icursor != icend; ++icursor) {

	    bool colPrimal = isCoarseSpace(feSpaces[j], col(*icursor));

	    if (colPrimal) {
	      if (rowPrimal) {
		cols.push_back(col(*icursor));
		values.push_back(value(*icursor));
	      } else {
		colsOther.push_back(col(*icursor));
		valuesOther.push_back(value(*icursor));
	      }
	    } else {
	      if (rowPrimal) {
		colsOther.push_back(col(*icursor));
		valuesOther.push_back(value(*icursor));
	      } else {
		cols.push_back(col(*icursor));
		values.push_back(value(*icursor));
	      }
	    }
	  }  // for each nnz in row


	  // === Set matrix values. ===

	  if (rowPrimal) {
	    int rowIndex = coarseSpaceMap->getMatIndex(i, *cursor);
	    for (unsigned int k = 0; k < cols.size(); k++)
	      cols[k] = coarseSpaceMap->getMatIndex(j, cols[k]);

	    MatSetValues(matCoarseCoarse, 1, &rowIndex, cols.size(),
			 &(cols[0]), &(values[0]), ADD_VALUES);

	    if (colsOther.size()) {
	      if (subdomainLevel == 0) {
		for (unsigned int k = 0; k < colsOther.size(); k++)
		  colsOther[k] = interiorMap->getMatIndex(j, colsOther[k]);
	      } else {
		for (unsigned int k = 0; k < colsOther.size(); k++)
		  colsOther[k] = 
		    interiorMap->getMatIndex(j, colsOther[k]) + rStartInterior;
	      }
 	      
	      MatSetValues(matCoarseInt, 1, &rowIndex, colsOther.size(),
 			   &(colsOther[0]), &(valuesOther[0]), ADD_VALUES);
	    }
	  } else {
	    int localRowIndex = 
	      (subdomainLevel == 0 ? interiorMap->getLocalMatIndex(i, *cursor) :
	       interiorMap->getMatIndex(i, *cursor));

	    for (unsigned int k = 0; k < cols.size(); k++) {
	      if (subdomainLevel == 0)
		cols[k] = interiorMap->getLocalMatIndex(j, cols[k]);
	      else
		cols[k] = interiorMap->getMatIndex(j, cols[k]);
	    }
	    
  	    MatSetValues(matIntInt, 1, &localRowIndex, cols.size(),
  			 &(cols[0]), &(values[0]), ADD_VALUES);

	    if (colsOther.size()) {
	      int globalRowIndex = interiorMap->getMatIndex(i, *cursor);

	      if (subdomainLevel != 0)
		globalRowIndex += rStartInterior;

	      for (unsigned int k = 0; k < colsOther.size(); k++)
		colsOther[k] = coarseSpaceMap->getMatIndex(j, colsOther[k]);

  	      MatSetValues(matIntCoarse, 1, &globalRowIndex, colsOther.size(),
  			   &(colsOther[0]), &(valuesOther[0]), ADD_VALUES);
	    }
	  }
	} 
      }
    }

    // === Start global assembly procedure. ===

    MatAssemblyBegin(matIntInt, MAT_FINAL_ASSEMBLY);
    MatAssemblyEnd(matIntInt, MAT_FINAL_ASSEMBLY);

    if (coarseSpaceMap) {
      MatAssemblyBegin(matCoarseCoarse, MAT_FINAL_ASSEMBLY);
      MatAssemblyEnd(matCoarseCoarse, MAT_FINAL_ASSEMBLY);
      
      MatAssemblyBegin(matIntCoarse, MAT_FINAL_ASSEMBLY);
      MatAssemblyEnd(matIntCoarse, MAT_FINAL_ASSEMBLY);
      
      MatAssemblyBegin(matCoarseInt, MAT_FINAL_ASSEMBLY);
      MatAssemblyEnd(matCoarseInt, MAT_FINAL_ASSEMBLY);
    }


    // === Remove Dirichlet BC DOFs. ===

    //    removeDirichletBcDofs(mat);

    // === Create solver for the non primal (thus local) variables. ===

    KSPCreate(mpiCommLocal, &kspInterior);
    KSPSetOperators(kspInterior, matIntInt, matIntInt, SAME_NONZERO_PATTERN);
    KSPSetOptionsPrefix(kspInterior, "interior_");
    KSPSetType(kspInterior, KSPPREONLY);
    PC pcInterior;
    KSPGetPC(kspInterior, &pcInterior);
    PCSetType(pcInterior, PCLU);
    if (subdomainLevel == 0)
      PCFactorSetMatSolverPackage(pcInterior, MATSOLVERUMFPACK);
    else
      PCFactorSetMatSolverPackage(pcInterior, MATSOLVERMUMPS);
    KSPSetFromOptions(kspInterior);  
  }


  void PetscSolverGlobalMatrix::fillPetscRhs(SystemVector *vec)
  {
    FUNCNAME("PetscSolverGlobalMatrix::fillPetscRhs()");

    VecCreateMPI(mpiCommGlobal, 
		 interiorMap->getRankDofs(), 
		 nGlobalOverallInterior,
		 &rhsInterior);

    if (coarseSpaceMap) 
      VecCreateMPI(mpiCommGlobal, 
		   coarseSpaceMap->getRankDofs(), 
		   coarseSpaceMap->getOverallDofs(),
		   &rhsCoarseSpace);
    
    TEST_EXIT_DBG(vec)("No DOF vector defined!\n");
    TEST_EXIT_DBG(interiorMap)("No parallel DOF map defined!\n");
    

    // === Transfer values from DOF vector to the PETSc vector. === 
    if (coarseSpaceMap) {
      for (int i = 0; i < vec->getSize(); i++)
	setDofVector(rhsInterior, rhsCoarseSpace, vec->getDOFVector(i), i);
    } else {
      for (int i = 0; i < vec->getSize(); i++)
	setDofVector(rhsInterior, vec->getDOFVector(i), i);
    }

    VecAssemblyBegin(rhsInterior);
    VecAssemblyEnd(rhsInterior);

    if (coarseSpaceMap) {
      VecAssemblyBegin(rhsCoarseSpace);
      VecAssemblyEnd(rhsCoarseSpace);
    }


    // === Remove Dirichlet BC DOFs. ===
    //    removeDirichletBcDofs(vec);
 
  }


  void PetscSolverGlobalMatrix::solvePetscMatrix(SystemVector &vec, 
						 AdaptInfo *adaptInfo)
  {
    FUNCNAME("PetscSolverGlobalMatrix::solvePetscMatrix()");

    int nComponents = vec.getSize();

    // === Set old solution to be initiual guess for PETSc solver. ===
    if (!zeroStartVector) {
      VecSet(petscSolVec, 0.0);
      
      for (int i = 0; i < nComponents; i++)
	setDofVector(petscSolVec, vec.getDOFVector(i), i, true);
      
      VecAssemblyBegin(petscSolVec);
      VecAssemblyEnd(petscSolVec);
    }


    MatNullSpace matNullspace;
    Vec nullspaceBasis;

    if (nullspace.size() > 0 || hasConstantNullspace) {
      TEST_EXIT_DBG(nullspace.size() <= 1)("Not yet implemented!\n");

      if (nullspace.size() > 0) {
	VecDuplicate(petscSolVec, &nullspaceBasis);
	for (int i = 0; i < nComponents; i++)
	  setDofVector(nullspaceBasis, nullspace[0]->getDOFVector(i), i, true);
	
	VecAssemblyBegin(nullspaceBasis);
	VecAssemblyEnd(nullspaceBasis);
	
	MatNullSpaceCreate(mpiCommGlobal, (hasConstantNullspace ? PETSC_TRUE : PETSC_FALSE), 
			   1, &nullspaceBasis, &matNullspace);

	MatMult(matIntInt, nullspaceBasis, petscSolVec);
	PetscReal n;
	VecNorm(petscSolVec, NORM_2, &n);
	MSG("NORM IS: %e\n", n);
      } else {
	MatNullSpaceCreate(mpiCommGlobal, PETSC_TRUE, 0, PETSC_NULL, &matNullspace);
      }

      KSPSetNullSpace(kspInterior, matNullspace);

      // === Remove null space, if requested. ===

      if (removeRhsNullspace) {
	TEST_EXIT_DBG(coarseSpaceMap == NULL)("Not supported!\n");
	
	MatNullSpaceRemove(matNullspace, rhsInterior, PETSC_NULL);
      }
    } else {
      TEST_EXIT(removeRhsNullspace == false)
	("No nullspace provided that should be removed from rhs!\n");
    }


    // PETSc.
    solve(rhsInterior, petscSolVec);


    if (nullspace.size() > 0) {
      MatNullSpaceDestroy(&matNullspace);
      VecDestroy(&nullspaceBasis);
    }


    // === Transfere values from PETSc's solution vectors to the DOF vectors. ===
    PetscScalar *vecPointer;
    VecGetArray(petscSolVec, &vecPointer);    

    int c = 0;
    for (int i = 0; i < nComponents; i++) {
      DOFVector<double> &dv = *(vec.getDOFVector(i));

      DofMap& d = (*interiorMap)[dv.getFeSpace()].getMap();
      for (DofMap::iterator it = d.begin(); it != d.end(); ++it)
	if (it->second.local != -1)
	  dv[it->first] = vecPointer[c++];
    }

    VecRestoreArray(petscSolVec, &vecPointer);


    // === Synchronize DOFs at common DOFs, i.e., DOFs that correspond to ===
    // === more than one partition.                                       ===
    meshDistributor->synchVector(vec);
  }


  void PetscSolverGlobalMatrix::solveGlobal(Vec &rhs, Vec &sol)
  {
    FUNCNAME("PetscSolverGlobalMatrix::solveGlobal()");

    double wtime = MPI::Wtime();
    double t0 = 0.0, t1 = 0.0;

    Vec tmp;
    if (mpiCommLocal.Get_size() == 1)
      VecCreateSeq(mpiCommLocal, interiorMap->getRankDofs(), &tmp);
    else
      VecCreateMPI(mpiCommLocal,
		   interiorMap->getRankDofs(),
		   interiorMap->getOverallDofs(),
		   &tmp);

    PetscScalar *tmpValues, *rhsValues;
    VecGetArray(tmp, &tmpValues);
    VecGetArray(rhs, &rhsValues);

    for (int i = 0; i < interiorMap->getRankDofs(); i++)
      tmpValues[i] = rhsValues[i];

    VecRestoreArray(rhs, &rhsValues);
    VecRestoreArray(tmp, &tmpValues);

    t0 = MPI::Wtime() - wtime;

    wtime = MPI::Wtime();
    KSPSolve(kspInterior, tmp, tmp);
    t1 = MPI::Wtime() - wtime;

    wtime = MPI::Wtime();

    VecGetArray(tmp, &tmpValues);
    VecGetArray(sol, &rhsValues);

    for (int i = 0; i < interiorMap->getRankDofs(); i++) 
      rhsValues[i] = tmpValues[i];

    VecRestoreArray(sol, &rhsValues);
    VecRestoreArray(tmp, &tmpValues);

    VecDestroy(&tmp);
    t0 += MPI::Wtime() - wtime;

    //    MSG("TIMEING: %.5f %.5f\n", t0, t1);
  }


  void PetscSolverGlobalMatrix::destroyMatrixData()
  {
    FUNCNAME("PetscSolverGlobalMatrix::destroyMatrixData()");

    MatDestroy(&matIntInt);
    KSPDestroy(&kspInterior);

    if (coarseSpaceMap) {
      MatDestroy(&matCoarseCoarse);
      MatDestroy(&matCoarseInt);
      MatDestroy(&matIntCoarse);
    }

    if (petscSolVec != PETSC_NULL) {
      VecDestroy(&petscSolVec);
      petscSolVec = PETSC_NULL;
    }
  }


  void PetscSolverGlobalMatrix::destroyVectorData()
  {
    FUNCNAME("PetscSolverGlobalMatrix::destroyVectorData()");

    VecDestroy(&rhsInterior);

    if (coarseSpaceMap)
      VecDestroy(&rhsCoarseSpace);
  }


  void PetscSolverGlobalMatrix::createFieldSplit(PC pc)
  {
    FUNCNAME("PetscSolverGlobalMatrix::createFieldSplit()");

    vector<string> isNames;
    Parameters::get("parallel->solver->is blocks", isNames);

    int nBlocks = isNames.size();
    if (nBlocks == 0)
      return;

    for (int i = 0; i < nBlocks; i++) {
      MSG("Create for block %s\n", isNames[i].c_str());

      vector<int> blockComponents;
      Parameters::get("parallel->solver->is block " + lexical_cast<string>(i),
		      blockComponents);
      int nComponents = static_cast<int>(blockComponents.size());

      TEST_EXIT(nComponents > 0)("No is block for block %d defined!\n", i);

      // Check if blocks are continous
      for (int j = 0; j < nComponents; j++) {
	TEST_EXIT(blockComponents[j] == blockComponents[0] + j)
	  ("Does not yet support not continous IS blocks! Block %s\n", 
	   isNames[i].c_str());
      }

      IS is;
      interiorMap->createIndexSet(is, blockComponents[0], nComponents);
      PCFieldSplitSetIS(pc, isNames[i].c_str(), is);
      ISDestroy(&is);
    }
  }


  void PetscSolverGlobalMatrix::setDofMatrix(DOFMatrix* mat,
					     int nRowMat, int nColMat)
  {
    FUNCNAME("PetscSolverGlobalMatrix::setDofMatrix()");

    TEST_EXIT(mat)("No DOFMatrix!\n");

    using mtl::tag::row; using mtl::tag::nz; using mtl::begin; using mtl::end;
    namespace traits = mtl::traits;
    typedef DOFMatrix::base_matrix_type Matrix;

    traits::col<Matrix>::type col(mat->getBaseMatrix());
    traits::const_value<Matrix>::type value(mat->getBaseMatrix());

    typedef traits::range_generator<row, Matrix>::type cursor_type;
    typedef traits::range_generator<nz, cursor_type>::type icursor_type;

    vector<int> cols;
    vector<double> values;
    cols.reserve(300);
    values.reserve(300);
    
    vector<int> globalCols;

    // Get periodic mapping object
    PeriodicMap &perMap = meshDistributor->getPeriodicMap();

    // === Traverse all rows of the dof matrix and insert row wise the values ===
    // === to the PETSc matrix.                                               ===

    for (cursor_type cursor = begin<row>(mat->getBaseMatrix()), 
	   cend = end<row>(mat->getBaseMatrix()); cursor != cend; ++cursor) {

      const FiniteElemSpace *rowFe = mat->getRowFeSpace();
      const FiniteElemSpace *colFe = mat->getColFeSpace();

      // Global index of the current row DOF.
      int globalRowDof = (*interiorMap)[rowFe][*cursor].global;

      // Test if the current row DOF is a periodic DOF.
      bool periodicRow = perMap.isPeriodic(rowFe, globalRowDof);

      if (!periodicRow) {
	// === Row DOF index is not periodic. ===

	// Get PETSc's mat row index.
	int rowIndex = interiorMap->getMatIndex(nRowMat, globalRowDof);

	cols.clear();
	values.clear();

	for (icursor_type icursor = begin<nz>(cursor), icend = end<nz>(cursor); 
	     icursor != icend; ++icursor) {

	  // Global index of the current column index.
	  int globalColDof = (*interiorMap)[colFe][col(*icursor)].global;
	  // Test if the current col dof is a periodic dof.
	  bool periodicCol = perMap.isPeriodic(colFe, globalColDof);
	  // Get PETSc's mat col index.
	  int colIndex = interiorMap->getMatIndex(nColMat, globalColDof);

	  // Ignore all zero entries, expect it is a diagonal entry.
 	  if (value(*icursor) == 0.0 && rowIndex != colIndex)
 	    continue;

	  if (!periodicCol) {
	    // Calculate the exact position of the column index in the PETSc matrix.
	    cols.push_back(colIndex);
	    values.push_back(value(*icursor));
	  } else {
	    // === Row index is not periodic, but column index is. ===

	    // Create set of all periodic associations of the column index.
	    std::set<int> perAsc;
	    std::set<int>& perColAsc = perMap.getAssociations(colFe, globalColDof);
	    for (std::set<int>::iterator it = perColAsc.begin(); 
		 it != perColAsc.end(); ++it)
	      if (meshDistributor->getElementObjectDb().isValidPeriodicType(*it))
		perAsc.insert(*it);
    
	    // Scale value to the number of periodic associations of the column index.
	    double scaledValue = 
	      value(*icursor) * pow(0.5, static_cast<double>(perAsc.size()));

	    
	    // === Create set of all matrix column indices due to the periodic ===
	    // === associations of the column DOF index.                       ===

	    vector<int> newCols;

	    // First, add the original matrix index.
	    newCols.push_back(globalColDof);

	    // And add all periodic matrix indices.
	    for (std::set<int>::iterator it = perAsc.begin(); 
		 it != perAsc.end(); ++it) {
	      int nCols = static_cast<int>(newCols.size());

	      for (int i = 0; i < nCols; i++) {
 		TEST_EXIT_DBG(perMap.isPeriodic(colFe, *it, newCols[i]))
 		  ("Wrong periodic DOF associations at boundary %d with DOF %d!\n",
		   *it, newCols[i]);

		newCols.push_back(perMap.map(colFe, *it, newCols[i]));
	      }
	    }

	    for (unsigned int i = 0; i < newCols.size(); i++) {
	      cols.push_back(interiorMap->getMatIndex(nColMat, newCols[i]));
	      values.push_back(scaledValue);	      
	    }
	  }
	}

	MatSetValues(matIntInt, 1, &rowIndex, cols.size(), 
		     &(cols[0]), &(values[0]), ADD_VALUES);	
      } else {
	// === Row DOF index is periodic. ===

	// Because this row is periodic, we will have to add the entries of this 
	// matrix row to multiple rows. The following maps store to each row an
	// array of column indices and values of the entries that must be added to
	// the PETSc matrix.
	map<int, vector<int> > colsMap;
	map<int, vector<double> > valsMap;

	// Traverse all column entries.
	for (icursor_type icursor = begin<nz>(cursor), icend = end<nz>(cursor); 
	     icursor != icend; ++icursor) {
	  // Global index of the current column index.
	  int globalColDof = (*interiorMap)[colFe][col(*icursor)].global;

	  // Ignore all zero entries, expect it is a diagonal entry.
 	  if (value(*icursor) == 0.0 && globalRowDof != globalColDof)
 	    continue;

	  // === Add all periodic associations of both, the row and the column ===
	  // === indices to the set perAsc.                                    ===

	  std::set<int> perAsc;

	  if (perMap.isPeriodic(colFe, globalColDof)) {
	    std::set<int>& perColAsc = perMap.getAssociations(colFe, globalColDof);
	    for (std::set<int>::iterator it = perColAsc.begin(); 
		 it != perColAsc.end(); ++it)
	      if (meshDistributor->getElementObjectDb().isValidPeriodicType(*it))
		perAsc.insert(*it);
	  }

	  std::set<int>& perRowAsc = perMap.getAssociations(rowFe, globalRowDof);
	  for (std::set<int>::iterator it = perRowAsc.begin(); 
	       it != perRowAsc.end(); ++it)
	    if (meshDistributor->getElementObjectDb().isValidPeriodicType(*it))
	      perAsc.insert(*it);

	  // Scale the value with respect to the number of periodic associations.
	  double scaledValue = 
	    value(*icursor) * pow(0.5, static_cast<double>(perAsc.size()));


	  // === Create all matrix entries with respect to the periodic  ===
	  // === associations of the row and column indices.             ===

	  vector<pair<int, int> > entry;
	  
	  // First, add the original entry.
	  entry.push_back(make_pair(globalRowDof, globalColDof));

	  // Then, traverse the periodic associations of the row and column
	  // indices and create the corresponding entries.
	  for (std::set<int>::iterator it = perAsc.begin(); it != perAsc.end(); ++it) {
	    int nEntry = static_cast<int>(entry.size());
	    for (int i = 0; i < nEntry; i++) {
	      int perRowDof = 0;

	      if (perMap.isPeriodic(rowFe, *it, entry[i].first))
		perRowDof = perMap.map(rowFe, *it, entry[i].first);
	      else
		perRowDof = entry[i].first;

	      int perColDof;
	      if (perMap.isPeriodic(colFe, *it, entry[i].second))
		perColDof = perMap.map(colFe, *it, entry[i].second);
	      else
		perColDof = entry[i].second;	      	      
	      

	      entry.push_back(make_pair(perRowDof, perColDof));
	    }
	  }


	  // === Translate the matrix entries to PETSc's matrix.

	  for (unsigned int i = 0; i < entry.size(); i++) {
	    int rowIdx = interiorMap->getMatIndex(nRowMat, entry[i].first);
	    int colIdx = interiorMap->getMatIndex(nColMat, entry[i].second);

	    colsMap[rowIdx].push_back(colIdx);
	    valsMap[rowIdx].push_back(scaledValue);
	  }
	}


	// === Finally, add all periodic rows to the PETSc matrix. ===

	for (map<int, vector<int> >::iterator rowIt = colsMap.begin();
	     rowIt != colsMap.end(); ++rowIt) {
	  TEST_EXIT_DBG(rowIt->second.size() == valsMap[rowIt->first].size())
	    ("Should not happen!\n");

	  int rowIndex = rowIt->first;

	  MatSetValues(matIntInt, 1, &rowIndex, rowIt->second.size(),
		       &(rowIt->second[0]), &(valsMap[rowIt->first][0]), ADD_VALUES);
	}
      }
    }
  }


  void PetscSolverGlobalMatrix::setDofVector(Vec vecInterior, 
					     Vec vecCoarse,
					     DOFVector<double>* vec, 
					     int nRowVec, 
					     bool rankOnly)
  {
    FUNCNAME("PetscSolverGlobalMatrix::setDofVector()");

    const FiniteElemSpace *feSpace = vec->getFeSpace();
    PeriodicMap &perMap = meshDistributor->getPeriodicMap();

    // Traverse all used DOFs in the dof vector.
    DOFVector<double>::Iterator dofIt(vec, USED_DOFS);
    for (dofIt.reset(); !dofIt.end(); ++dofIt) {
      if (rankOnly && !(*interiorMap)[feSpace].isRankDof(dofIt.getDOFIndex()))
	continue;

      if (isCoarseSpace(feSpace, dofIt.getDOFIndex())) {
	TEST_EXIT_DBG(vecCoarse != PETSC_NULL)("Should not happen!\n");

	int index = coarseSpaceMap->getMatIndex(nRowVec, dofIt.getDOFIndex());
	VecSetValue(vecCoarse, index, *dofIt, ADD_VALUES);
      } else {
	// Calculate global row index of the DOF.
	DegreeOfFreedom globalRowDof = 
	  (*interiorMap)[feSpace][dofIt.getDOFIndex()].global;
	
	// Get PETSc's mat index of the row DOF.
	int index = 0;
	if (interiorMap->isMatIndexFromGlobal())
	  index = 
	    interiorMap->getMatIndex(nRowVec, globalRowDof) + rStartInterior;
	else
	  index =
	    interiorMap->getMatIndex(nRowVec, dofIt.getDOFIndex()) + rStartInterior;
	
	if (perMap.isPeriodic(feSpace, globalRowDof)) {
	  std::set<int>& perAsc = perMap.getAssociations(feSpace, globalRowDof);
	  double value = *dofIt / (perAsc.size() + 1.0);
	  VecSetValue(vecInterior, index, value, ADD_VALUES);

	  for (std::set<int>::iterator perIt = perAsc.begin(); 
	       perIt != perAsc.end(); ++perIt) {
	    int mappedDof = perMap.map(feSpace, *perIt, globalRowDof);
	    int mappedIndex = interiorMap->getMatIndex(nRowVec, mappedDof);
	    VecSetValue(vecInterior, mappedIndex, value, ADD_VALUES);
	  }	  
	} else {	  
	  // The DOF index is not periodic.
	  
	  VecSetValue(vecInterior, index, *dofIt, ADD_VALUES);
	}
      }
    }
  }


  void PetscSolverGlobalMatrix::removeDirichletBcDofs(Matrix<DOFMatrix*> *mat)
  {
    FUNCNAME("PetscSolverGlobalMatrix::removeDirichletBcDofs()");

#if 0
    vector<int> dofsInterior, dofsCoarse;

    int nComponents = mat->getNumRows();
    for (int i = 0; i < nComponents; i++) {
      for (int j = 0; j < nComponents; j++) {
	if ((*mat)[i][j]) {
	  const FiniteElemSpace *feSpace = (*mat)[i][j]->getRowFeSpace();
	    
	  std::set<DegreeOfFreedom> &dirichletDofs = *((*mat)[i][j]->getApplyDBCs());

	  MSG("DIRICHLET DOFS: %d %d -> %d\n", i, j, dirichletDofs.size());
	  
	  for (std::set<DegreeOfFreedom>::iterator it = dirichletDofs.begin();
	       it != dirichletDofs.end(); ++it) {
	    if (isCoarseSpace(feSpace, *it)) {
	      if ((*coarseSpaceMap)[feSpace].isRankDof(*it)) {
		int globalDof = (*coarseSpaceMap)[feSpace][*it].global;
		dofsCoarse.push_back(coarseSpaceMap->getMatIndex(i, globalDof));
	      }
	    } else {
	      if ((*interiorMap)[feSpace].isRankDof(*it)) {
		int globalDof = (*interiorMap)[feSpace][*it].global;
		dofsInterior.push_back(interiorMap->getMatIndex(i, globalDof));
	      }
	    }
	  }
	} else {
	  MSG("NO MAT DIAG in %d\n", i);
	}
      }
    }

    MatZeroRows(matIntInt, dofsInterior.size(), &(dofsInterior[0]), 1.0, 
		PETSC_NULL, PETSC_NULL);

    if (coarseSpaceMap != NULL)
      MatZeroRows(matCoarseCoarse, dofsCoarse.size(), &(dofsCoarse[0]), 1.0, 
		  PETSC_NULL, PETSC_NULL);
#endif
  }


  void PetscSolverGlobalMatrix::removeDirichletBcDofs(SystemVector *vec)
  {
    FUNCNAME("PetscSolverGlobalMatrix::removeDirichletBcDofs()");

    int nComponents = vec->getSize();
    for (int i = 0; i < nComponents; i++) {
      const FiniteElemSpace *feSpace = vec->getDOFVector(i)->getFeSpace();

      map<DegreeOfFreedom, double> &dirichletValues = 
	vec->getDOFVector(i)->getDirichletValues();

      for (map<DegreeOfFreedom, double>::iterator it = dirichletValues.begin();
	   it != dirichletValues.end(); ++it) {
	if (isCoarseSpace(feSpace, it->first)) {
	  if ((*coarseSpaceMap)[feSpace].isRankDof(it->first)) {
	    int globalDof = (*coarseSpaceMap)[feSpace][it->first].global;
	    VecSetValue(rhsCoarseSpace, coarseSpaceMap->getMatIndex(i, globalDof), 
			it->second, INSERT_VALUES);
	  }
	} else {
	  if ((*interiorMap)[feSpace].isRankDof(it->first)) {
	    int globalDof = (*interiorMap)[feSpace][it->first].global;
	    VecSetValue(rhsInterior, interiorMap->getMatIndex(i, globalDof),
			it->second, INSERT_VALUES);	    
	  }
	}
      }
    }

    VecAssemblyBegin(rhsInterior);
    VecAssemblyEnd(rhsInterior);

    if (coarseSpaceMap) {
      VecAssemblyBegin(rhsCoarseSpace);
      VecAssemblyEnd(rhsCoarseSpace);
    }
  }


  void PetscSolverGlobalMatrix::createPetscNnzStructure(Matrix<DOFMatrix*> *mat)
  {
    FUNCNAME("PetscSolverGlobalMatrix::createPetscNnzStructure()");

    TEST_EXIT_DBG(!d_nnz)("There is something wrong!\n");
    TEST_EXIT_DBG(!o_nnz)("There is something wrong!\n");

    vector<const FiniteElemSpace*> feSpaces = getFeSpaces(mat);
    int nRankRows = interiorMap->getRankDofs();
    int rankStartIndex = interiorMap->getStartDofs();

    d_nnz = new int[nRankRows];
    o_nnz = new int[nRankRows];
    for (int i = 0; i < nRankRows; i++) {
      d_nnz[i] = 0;
      o_nnz[i] = 0;
    }

    using mtl::tag::row; using mtl::tag::nz; using mtl::begin; using mtl::end;
    namespace traits = mtl::traits;
    typedef DOFMatrix::base_matrix_type Matrix;
    typedef vector<pair<int, int> > MatrixNnzEntry;
    typedef map<int, DofContainer> RankToDofContainer;

    // Stores to each rank a list of nnz entries (i.e. pairs of row and column
    // index) that this rank will send to. These nnz entries will be assembled
    // on this rank, but because the row DOFs are not DOFs of this rank they 
    // will be send to the owner of the row DOFs.
    map<int, MatrixNnzEntry> sendMatrixEntry;

    // Maps to each DOF that must be send to another rank the rank number of the
    // receiving rank.
    map<pair<DegreeOfFreedom, int>, int> sendDofToRank;


    // First, create for all ranks, to which we send data to, MatrixNnzEntry 
    // object with 0 entries.
    for (unsigned int i = 0; i < feSpaces.size(); i++) {
      for (DofComm::Iterator it(meshDistributor->getDofComm().getRecvDofs(), feSpaces[i]);
	   !it.end(); it.nextRank()) {
	sendMatrixEntry[it.getRank()].resize(0);
	
	for (; !it.endDofIter(); it.nextDof())
	  sendDofToRank[make_pair(it.getDofIndex(), i)] = it.getRank();
      }
    }

    // Create list of ranks from which we receive data from.
    std::set<int> recvFromRank;
    for (unsigned int i = 0; i < feSpaces.size(); i++) 
      for (DofComm::Iterator it(meshDistributor->getDofComm().getSendDofs(), feSpaces[i]);
	   !it.end(); it.nextRank())
	recvFromRank.insert(it.getRank());


    // === Traverse matrices to create nnz data. ===

    int nComponents = mat->getNumRows();
    for (int i = 0; i < nComponents; i++) {
      for (int j = 0; j < nComponents; j++) {
 	if (!(*mat)[i][j])
	  continue;

	TEST_EXIT_DBG((*mat)[i][j]->getRowFeSpace() == feSpaces[i])
	  ("Should not happen!\n");
	TEST_EXIT_DBG((*mat)[i][j]->getColFeSpace() == feSpaces[j])
	  ("Should not happen!\n");

	Matrix bmat = (*mat)[i][j]->getBaseMatrix();

	traits::col<Matrix>::type col(bmat);
	traits::const_value<Matrix>::type value(bmat);
	  
	typedef traits::range_generator<row, Matrix>::type cursor_type;
	typedef traits::range_generator<nz, cursor_type>::type icursor_type;
	
	for (cursor_type cursor = begin<row>(bmat), 
	       cend = end<row>(bmat); cursor != cend; ++cursor) {
	  int globalRowDof = (*interiorMap)[feSpaces[i]][*cursor].global;

	  // The corresponding global matrix row index of the current row DOF.
	  int petscRowIdx = interiorMap->getMatIndex(i, globalRowDof);
	  if ((*interiorMap)[feSpaces[i]].isRankDof(*cursor)) {
    	    
	    // === The current row DOF is a rank DOF, so create the       ===
	    // === corresponding nnz values directly on rank's nnz data.  ===
	    
	    // This is the local row index of the local PETSc matrix.
	    int localPetscRowIdx = petscRowIdx - rankStartIndex;
	    
	    TEST_EXIT_DBG(localPetscRowIdx >= 0 && localPetscRowIdx < nRankRows)
	      ("Should not happen! \n Debug info: DOF = %d   globalRowIndx = %d   petscRowIdx = %d   localPetscRowIdx = %d   rStart = %d   compontens = %d from %d   nRankRows = %d\n",
	       *cursor,
	       (*interiorMap)[feSpaces[i]][*cursor].global,
	       petscRowIdx, 
	       localPetscRowIdx, 
	       rankStartIndex,
	       i,
	       nComponents, 
	       nRankRows);
	    
	    
	    // Traverse all non zero entries in this row.
	    for (icursor_type icursor = begin<nz>(cursor), 
		   icend = end<nz>(cursor); icursor != icend; ++icursor) {
	      int globalColDof = (*interiorMap)[feSpaces[j]][col(*icursor)].global;
	      int petscColIdx = interiorMap->getMatIndex(j, globalColDof);
	      
	      if (value(*icursor) != 0.0 || petscRowIdx == petscColIdx) {
		// The row DOF is a rank DOF, if also the column is a rank DOF, 
		// increment the d_nnz values for this row, otherwise the 
		// o_nnz value.
		if (petscColIdx >= rankStartIndex && 
		    petscColIdx < rankStartIndex + nRankRows)
		  d_nnz[localPetscRowIdx]++;
		else
		  o_nnz[localPetscRowIdx]++;
	      }    
	    }
	  } else {
	    // === The current row DOF is not a rank DOF, i.e., its values   ===
	    // === are also created on this rank, but afterthere they will   ===
	    // === be send to another rank. So we need to send also the      ===
	    // === corresponding nnz structure of this row to the corres-    ===
	    // === ponding rank.                                             ===
	    
	    // Send all non zero entries to the member of the row DOF.
	    int sendToRank = sendDofToRank[make_pair(*cursor, i)];
	    
	    for (icursor_type icursor = begin<nz>(cursor), 
		   icend = end<nz>(cursor); icursor != icend; ++icursor) {
	      if (value(*icursor) != 0.0) {
		int globalColDof = 
		  (*interiorMap)[feSpaces[j]][col(*icursor)].global;
		int petscColIdx = interiorMap->getMatIndex(j, globalColDof);
		
		sendMatrixEntry[sendToRank].
		  push_back(make_pair(petscRowIdx, petscColIdx));
	      }
	    }
	    
	  } // if (isRankDof[*cursor]) ... else ...
	} // for each row in mat[i][j]
      } 
    }

    // === Send and recv the nnz row structure to/from other ranks. ===

    StdMpi<MatrixNnzEntry> stdMpi(mpiCommGlobal, true);
    stdMpi.send(sendMatrixEntry);
    for (std::set<int>::iterator it = recvFromRank.begin(); 
	 it != recvFromRank.end(); ++it)
      stdMpi.recv(*it);
    stdMpi.startCommunication();


    // === Evaluate the nnz structure this rank got from other ranks and add ===
    // === it to the PETSc nnz data structure.                               ===

    for (map<int, MatrixNnzEntry>::iterator it = stdMpi.getRecvData().begin();
	 it != stdMpi.getRecvData().end(); ++it) {
      if (it->second.size() > 0) {
	for (unsigned int i = 0; i < it->second.size(); i++) {
	  int r = it->second[i].first;
	  int c = it->second[i].second;

	  int localRowIdx = r - rankStartIndex;

	  TEST_EXIT_DBG(localRowIdx >= 0 && localRowIdx < nRankRows)
	    ("Got row index %d/%d (nRankRows = %d) from rank %d. Should not happen!\n",
	     r, localRowIdx, nRankRows, it->first);
	  
	  if (c < rankStartIndex || c >= rankStartIndex + nRankRows)
	    o_nnz[localRowIdx]++;
	  else
	    d_nnz[localRowIdx]++;
	}
      }
    }

    // The above algorithm for calculating the number of nnz per row over-
    // approximates the value, i.e., the number is always equal or larger to 
    // the real number of nnz values in the global parallel matrix. For small
    // matrices, the problem may arise, that the result is larger than the
    // number of elements in a row. This is fixed in the following.

    if (nRankRows < 100) 
      for (int i = 0; i < nRankRows; i++)
	d_nnz[i] = std::min(d_nnz[i], nRankRows);
  }

}