#include "GlobalMatrixSolver.h"
#include "DOFVector.h"
#include "Debug.h"
#include "SystemVector.h"
#include "parallel/StdMpi.h"

#include "petscksp.h"

namespace AMDiS {

  PetscErrorCode myKSPMonitor(KSP ksp, PetscInt iter, PetscReal rnorm, void *)
  {    
    if (iter % 100 == 0 && MPI::COMM_WORLD.Get_rank() == 0)
      std::cout << "[0]  Petsc-Iteration " << iter << ": " << rnorm << std::endl;

    return 0;
  }
 
  void GlobalMatrixSolver::solve()
  {
    FUNCNAME("GlobalMatrixSolver::solve()");

#ifdef _OPENMP
    double wtime = omp_get_wtime();
#endif
    clock_t first = clock();

    fillPetscMatrix(probStat->getSystemMatrix(), probStat->getRhs());
    solvePetscMatrix(*(probStat->getSolution()));

#ifdef _OPENMP
    INFO(info, 8)("solution of discrete system needed %.5f seconds system time / %.5f seconds wallclock time\n",
		   TIME_USED(first, clock()),
		   omp_get_wtime() - wtime);
#else
    INFO(info, 8)("solution of discrete system needed %.5f seconds\n",
		   TIME_USED(first, clock()));
#endif    
  }


  void GlobalMatrixSolver::setDofMatrix(DOFMatrix* mat, int dispMult, 
					int dispAddRow, int dispAddCol)
  {
    FUNCNAME("GlobalMatrixSolver::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;

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

    // === 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) {

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

      // Global index of the current row dof.
      int globalRowDof = mapLocalGlobalDofs[*cursor];
      // Test if the current row dof is a periodic dof.
      bool periodicRow = (periodicDof.count(globalRowDof) > 0);


      // === Traverse all non zero entries of the row and produce vector cols ===
      // === with the column indices of all row entries and vector values     ===
      // === with the corresponding values.                                   ===

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

	// Set only non null values.
	if (value(*icursor) != 0.0) {
	  // Global index of the current column index.
	  int globalColDof = mapLocalGlobalDofs[col(*icursor)];
	  // Calculate the exact position of the column index in the petsc matrix.
	  int colIndex = globalColDof * dispMult + dispAddCol;

	  // If the current row is not periodic, but the current dof index is periodic,
	  // we have to duplicate the value to the other corresponding periodic columns.
 	  if (periodicRow == false && periodicDof.count(globalColDof) > 0) {
	    // The value is assign to n matrix entries, therefore, every entry 
	    // has only 1/n value of the original entry.
	    double scalFactor = 1.0 / (periodicDof[globalColDof].size() + 1.0);

	    // Insert original entry.
 	    cols.push_back(colIndex);
 	    values.push_back(value(*icursor) * scalFactor);

	    // Insert the periodic entries.
 	    for (std::set<DegreeOfFreedom>::iterator it = 
		   periodicDof[globalColDof].begin();
 		 it != periodicDof[globalColDof].end(); ++it) {
 	      cols.push_back(*it * dispMult + dispAddCol);
 	      values.push_back(value(*icursor) * scalFactor);
	    }
 	  } else {
	    // Neigher row nor column dof index is periodic, simple add entry.
	    cols.push_back(colIndex);
	    values.push_back(value(*icursor));
	  }
	}
      }


      // === Up to now we have assembled on row. Now, the row must be send to the ===
      // === corresponding rows to the petsc matrix.                              ===

      // Calculate petsc row index.
      int rowIndex = globalRowDof * dispMult + dispAddRow;
      
      if (periodicRow) {
	// The row dof is periodic, so send dof to all the corresponding rows.

	double scalFactor = 1.0 / (periodicDof[globalRowDof].size() + 1.0);
	
	int diagIndex = -1;
	for (int i = 0; i < static_cast<int>(values.size()); i++) {
	  // Change only the non diagonal values in the col. For the diagonal test
	  // we compare the global dof indices of the dof matrix (not of the petsc
	  // matrix!).
	  if ((cols[i] - dispAddCol) / dispMult != globalRowDof)
	    values[i] *= scalFactor;
	  else
	    diagIndex = i;
	}
	
	// Send the main row to the petsc matrix.
	MatSetValues(petscMatrix, 1, &rowIndex, cols.size(), 
		     &(cols[0]), &(values[0]), ADD_VALUES);	
 
	// Set diagonal element to zero, i.e., the diagonal element of the current
	// row is not send to the periodic row indices.
	if (diagIndex != -1)
	  values[diagIndex] = 0.0;

	// Send the row to all periodic row indices.
	for (std::set<DegreeOfFreedom>::iterator it = periodicDof[globalRowDof].begin();
	     it != periodicDof[globalRowDof].end(); ++it) {
	  int perRowIndex = *it * dispMult + dispAddRow;
	  MatSetValues(petscMatrix, 1, &perRowIndex, cols.size(), 
		       &(cols[0]), &(values[0]), ADD_VALUES);
	}

      } else {
	// The row dof is not periodic, simply send the row to the petsc matrix.
	MatSetValues(petscMatrix, 1, &rowIndex, cols.size(), 
		     &(cols[0]), &(values[0]), ADD_VALUES);
      }    
    }
  }


  void GlobalMatrixSolver::setDofVector(Vec& petscVec, DOFVector<double>* vec, 
					int dispMult, int dispAdd)
  {
    // Traverse all used dofs in the dof vector.
    DOFVector<double>::Iterator dofIt(vec, USED_DOFS);
    for (dofIt.reset(); !dofIt.end(); ++dofIt) {
      // Calculate global row index of the dof.
      int globalRow = mapLocalGlobalDofs[dofIt.getDOFIndex()];
      // Calculate petsc index of the row dof.
      int index = globalRow * dispMult + dispAdd;

      if (periodicDof.count(globalRow) > 0) {
	// The dof index is periodic, so devide the value to all dof entries.

	double value = *dofIt / (periodicDof[globalRow].size() + 1.0);
	VecSetValues(petscVec, 1, &index, &value, ADD_VALUES);

	for (std::set<DegreeOfFreedom>::iterator it = periodicDof[globalRow].begin();
	     it != periodicDof[globalRow].end(); ++it) {
	  index = *it * dispMult + dispAdd;
	  VecSetValues(petscVec, 1, &index, &value, ADD_VALUES);
	}

      } else {
	// The dof index is not periodic.
	double value = *dofIt;
	VecSetValues(petscVec, 1, &index, &value, ADD_VALUES);
      }
    }    
  }


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

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

    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 std::vector<std::pair<int, int> > MatrixNnzEntry;

    // Stores to each rank a list of nnz entries (i.e. pairs of row and column index)
    // that this rank will send to. This 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.
    std::map<int, MatrixNnzEntry> sendMatrixEntry;

    for (int i = 0; i < nComponents; i++) {
      for (int j = 0; j < nComponents; j++) {
 	if ((*mat)[i][j]) {
	  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) {

	    // Map the local row number to the global DOF index and create from it
	    // the global PETSc row index of this DOF.
	    int petscRowIdx = mapLocalGlobalDofs[*cursor] * nComponents + i;

	    if (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 - rstart * nComponents;

#if (DEBUG != 0)    
	      if (localPetscRowIdx < 0 || localPetscRowIdx >= nRankRows) {
		std::cout << "ERROR in rank: " << mpiRank << std::endl;
		std::cout << "  Wrong r = " << localPetscRowIdx << " " << *cursor 
			  << " " << mapLocalGlobalDofs[*cursor] << " " 
			  << nRankRows << std::endl;
		ERROR_EXIT("Should not happen!\n");
	      }
#endif
	      
	      // Traverse all non zero entries in this row.
	      for (icursor_type icursor = begin<nz>(cursor), 
		     icend = end<nz>(cursor); icursor != icend; ++icursor) {
		if (value(*icursor) != 0.0) {
		  int petscColIdx = mapLocalGlobalDofs[col(*icursor)] * nComponents + j;

		  // 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 >= rstart * nComponents && 
		      petscColIdx < rstart * nComponents + nRankRows)
		    d_nnz[localPetscRowIdx]++;
		  else
		    o_nnz[localPetscRowIdx]++;
		}    
	      }
	    } else {
	      
	      // === The current row DOF is not a rank dof, i.e., it will be created ===
	      // === on this rank, but after this it will be send to another rank    ===
	      // === matrix. So we need to send also the corresponding nnz structure ===
	      // === of this row to the corresponding rank.                          ===

	      // Find out who is the member of this DOF.
	      int sendToRank = -1;
	      for (RankToDofContainer::iterator it = recvDofs.begin();
		   it != recvDofs.end(); ++it) {
		for (DofContainer::iterator dofIt = it->second.begin();
		     dofIt != it->second.end(); ++dofIt) {
		  if (**dofIt == *cursor) {

		    if (petscRowIdx == 6717) {
		      debug::writeDofMesh(mpiRank, *cursor, feSpace);
		      MSG("SEND DOF TO: %d/%d\n", it->first, *cursor);
		    }

		    sendToRank = it->first;
		    break;
		  }
		}

		if (sendToRank != -1)
		  break;
	      }

	      TEST_EXIT_DBG(sendToRank != -1)("Should not happen!\n");

	      // Send all non zero entries to the member of the row DOF.
	      for (icursor_type icursor = begin<nz>(cursor), 
		     icend = end<nz>(cursor); icursor != icend; ++icursor) {
		if (value(*icursor) != 0.0) {
		  int petscColIdx = mapLocalGlobalDofs[col(*icursor)] * nComponents + j;
		  
		  sendMatrixEntry[sendToRank].
		    push_back(std::make_pair(petscRowIdx, petscColIdx));
		}
	      }

	    } // if (isRankDof[*cursor]) ... else ...
	  } // for each row in mat[i][j]
	} // if mat[i][j]
      } 
    }

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

    StdMpi<MatrixNnzEntry> stdMpi(mpiComm, true);
    stdMpi.send(sendMatrixEntry);
    stdMpi.recv(sendDofs);
    stdMpi.startCommunication<int>(MPI_INT);

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

    for (std::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 - rstart * nComponents;

	  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 < rstart * nComponents || c >= rstart * nComponents + nRankRows)
	    o_nnz[localRowIdx]++;
	  else
	    d_nnz[localRowIdx]++;
	}
      }
    }  
  }


  void GlobalMatrixSolver::fillPetscMatrix(Matrix<DOFMatrix*> *mat, SystemVector *vec)
  {
    FUNCNAME("GlobalMatrixSolver::fillPetscMatrix()");

    clock_t first = clock();

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

    VecCreate(PETSC_COMM_WORLD, &petscRhsVec);
    VecSetSizes(petscRhsVec, nRankRows, nOverallRows);
    VecSetType(petscRhsVec, VECMPI);

    VecCreate(PETSC_COMM_WORLD, &petscSolVec);
    VecSetSizes(petscSolVec, nRankRows, nOverallRows);
    VecSetType(petscSolVec, VECMPI);

    VecCreate(PETSC_COMM_WORLD, &petscTmpVec);
    VecSetSizes(petscTmpVec, nRankRows, nOverallRows);
    VecSetType(petscTmpVec, VECMPI);

    if (!d_nnz || lastMeshChangeIndex != lastMeshNnz) {
      if (d_nnz) {
	delete [] d_nnz;
	delete [] o_nnz;
      }

      createPetscNnzStructure(mat);
      lastMeshNnz = lastMeshChangeIndex;
    }

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

    MatCreateMPIAIJ(PETSC_COMM_WORLD, nRankRows, nRankRows, nOverallRows, nOverallRows,
		    0, d_nnz, 0, o_nnz, &petscMatrix);

    INFO(info, 8)("Fill petsc matrix 1 needed %.5f seconds\n", TIME_USED(first, clock()));

#if (DEBUG != 0)
    int a, b;
    MatGetOwnershipRange(petscMatrix, &a, &b);
    TEST_EXIT(a == rstart * nComponents)("Wrong matrix ownership range!\n");
    TEST_EXIT(b == rstart * nComponents + nRankRows)("Wrong matrix ownership range!\n");
#endif

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

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

    INFO(info, 8)("Fill petsc matrix 2 needed %.5f seconds\n", TIME_USED(first, clock()));

    MatAssemblyBegin(petscMatrix, MAT_FINAL_ASSEMBLY);
    MatAssemblyEnd(petscMatrix, MAT_FINAL_ASSEMBLY);

    // === Transfer values from DOF vector to the PETSc vector. === 

    for (int i = 0; i < nComponents; i++)
      setDofVector(petscRhsVec, vec->getDOFVector(i), nComponents, i);

    VecAssemblyBegin(petscRhsVec);
    VecAssemblyEnd(petscRhsVec);

    INFO(info, 8)("Fill petsc matrix needed %.5f seconds\n", TIME_USED(first, clock()));
  }


  void GlobalMatrixSolver::solvePetscMatrix(SystemVector &vec)
  {
    FUNCNAME("GlobalMatrixSolver::solvePetscMatrix()");

#if 0
    // Set old solution to be initiual guess for petsc solver.
    for (int i = 0; i < nComponents; i++)
      setDofVector(petscSolVec, vec->getDOFVector(i), nComponents, i);

    VecAssemblyBegin(petscSolVec);
    VecAssemblyEnd(petscSolVec);
#endif

    // === Init Petsc solver. ===

    KSP solver;
    KSPCreate(PETSC_COMM_WORLD, &solver);
    KSPSetOperators(solver, petscMatrix, petscMatrix, SAME_NONZERO_PATTERN); 
    KSPSetTolerances(solver, 0.0, 1e-8, PETSC_DEFAULT, PETSC_DEFAULT);
    KSPSetType(solver, KSPBCGS);
    KSPMonitorSet(solver, myKSPMonitor, PETSC_NULL, 0);
    KSPSetFromOptions(solver);
    // Do not delete the solution vector, use it for the initial guess.
    //    KSPSetInitialGuessNonzero(solver, PETSC_TRUE);


    // === Run Petsc. ===

    KSPSolve(solver, petscRhsVec, petscSolVec);

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

    for (int i = 0; i < nComponents; i++) {
      DOFVector<double> *dofvec = vec.getDOFVector(i);
      for (int j = 0; j < nRankDofs; j++)
	(*dofvec)[mapLocalToDofIndex[j]] = vecPointer[j * nComponents + i];      
    }

    VecRestoreArray(petscSolVec, &vecPointer);


    // === Synchronize dofs at common dofs, i.e., dofs that correspond to more ===
    // === than one partition.                                                 ===
    synchVector(vec);


    // === Print information about solution process. ===

    int iterations = 0;
    KSPGetIterationNumber(solver, &iterations);
    MSG("  Number of iterations: %d\n", iterations);
    
    double norm = 0.0;
    MatMult(petscMatrix, petscSolVec, petscTmpVec);
    VecAXPY(petscTmpVec, -1.0, petscRhsVec);
    VecNorm(petscTmpVec, NORM_2, &norm);
    MSG("  Residual norm: %e\n", norm);


    // === Destroy Petsc's variables. ===

    MatDestroy(petscMatrix);
    VecDestroy(petscRhsVec);
    VecDestroy(petscSolVec);
    VecDestroy(petscTmpVec);
    KSPDestroy(solver);
  }

}