#include <queue>
#include "ParMetisPartitioner.h"
#include "Mesh.h"
#include "Traverse.h"
#include "ElInfo.h"
#include "Element.h"
#include "FixVec.h"
#include "PartitionElementData.h"
#include "DOFVector.h"
#include "mpi.h"

namespace AMDiS {

  ParMetisMesh::ParMetisMesh(Mesh *mesh, MPI::Intracomm *comm)
    : dim(mesh->getDim()),
      nElements(0),
      mpiComm(comm)
  {
    FUNCNAME("ParMetisMesh::ParMetisMesh()");

    int mpiSize = mpiComm->Get_size();
    int nodeCounter = 0;
    int elementCounter = 0;
    int dow = Global::getGeo(WORLD);

    TraverseStack stack;
    ElInfo *elInfo = stack.traverseFirst(mesh, -1, Mesh::CALL_EVERY_EL_PREORDER);
    while (elInfo) {
      // get partition data
      PartitionElementData *partitionData = dynamic_cast<PartitionElementData*>
	(elInfo->getElement()->getElementData(PARTITION_ED));

      if (partitionData && 
	  partitionData->getPartitionStatus() == IN &&
	  partitionData->getLevel() == 0)
	elementCounter++;      

      elInfo = stack.traverseNext(elInfo);
    }

    nElements = elementCounter;

    TEST_EXIT(nElements > 0)("no elements in ParMETIS mesh\n");

    // allocate memory
    eptr = new int[nElements + 1];
    eind = new int[nElements * (dim + 1)];
    elmdist = new int[mpiSize + 1];
    elem_p2a = new int[nElements];

    if (dim == dow)
      xyz = new float[nElements * dim];
    else
      xyz = NULL;    

    eptr[0] = 0;

    int *ptr_eptr = eptr + 1;
    int *ptr_eind = eind;
    float *ptr_xyz = xyz;
    
    // gather element numbers and create elmdist
    mpiComm->Allgather(&nElements, 1, MPI_INT, elmdist + 1, 1, MPI_INT);

    elmdist[0] = 0;
    for (int i = 2; i < mpiSize + 1; i++)
      elmdist[i] += elmdist[i - 1];

    // traverse mesh and fill distributed ParMETIS data
    DimVec<double> bary(dim, DEFAULT_VALUE, 1.0 / (dim + 1));
    WorldVector<double> world;

    elementCounter = 0;

    elInfo = stack.traverseFirst(mesh, -1, 
				 Mesh::CALL_EVERY_EL_PREORDER | Mesh::FILL_COORDS);
    while (elInfo) {
      Element *element = elInfo->getElement();
      int index = element->getIndex();

      // get partition data
      PartitionElementData *partitionData = dynamic_cast<PartitionElementData*>
	(element->getElementData(PARTITION_ED));

      // if element in partition
      if (partitionData && 
	  partitionData->getPartitionStatus() == IN &&
	  partitionData->getLevel() == 0) {
	// remember index
	setParMetisIndex(index, elementCounter);
	setAMDiSIndex(elementCounter, index);

	// write eptr entry
	nodeCounter += dim + 1;
	*ptr_eptr = nodeCounter;
	ptr_eptr++;

	// write eind entries (element nodes)
	for (int i = 0; i < dim + 1; i++) {
	  *ptr_eind = element->getDOF(i, 0);
	  ptr_eind++;
	}

	// write xyz element coordinates
	if (ptr_xyz) {
	  elInfo->coordToWorld(bary, world);
	  for (int i = 0; i < dim; i++) {
	    *ptr_xyz = static_cast<float>(world[i]); 
	    ptr_xyz++;
	  }
	}

	elementCounter++;
      }
      elInfo = stack.traverseNext(elInfo);
    }
  }


  ParMetisMesh::~ParMetisMesh()
  {
    if (eptr)
      delete [] eptr;
    
    if (eind)     
      delete [] eind;
    
    if (elmdist)
      delete [] elmdist;
    
    if (xyz)
      delete [] xyz;
    
    if (elem_p2a) 
      delete [] elem_p2a;
  }


  ParMetisGraph::ParMetisGraph(ParMetisMesh *parMesh,
			       MPI::Intracomm *comm,
			       int ncommonnodes)
    : parMetisMesh(parMesh)
  {
    int numflag = 0;

    if (ncommonnodes == -1) 
      ncommonnodes = parMetisMesh->getDim();

    MPI_Comm tmpComm = MPI_Comm(*comm);

    ParMETIS_V3_Mesh2Dual(parMetisMesh->getElementDist(),
			  parMetisMesh->getElementPtr(),
			  parMetisMesh->getElementInd(),
			  &numflag,
			  &ncommonnodes,
			  &xadj,
			  &adjncy,
			  &tmpComm);
  }


  ParMetisGraph::~ParMetisGraph()
  {
    free(xadj);
    free(adjncy);
  }


  void ParMetisPartitioner::deletePartitionData() 
  {
    TraverseStack stack;
    ElInfo *elInfo = stack.traverseFirst(mesh, -1, Mesh::CALL_EVERY_EL_PREORDER);
    while (elInfo) {
      Element *element = elInfo->getElement();
      element->deleteElementData(PARTITION_ED);
      elInfo = stack.traverseNext(elInfo);
    }
  }


  void ParMetisPartitioner::createPartitionData() 
  {
    FUNCNAME("ParMetrisPartitioner::createPartitionData()");

    int mpiRank = mpiComm->Get_rank();
    int mpiSize = mpiComm->Get_size();
    int nLeaves = mesh->getNumberOfLeaves();
    int elPerRank = nLeaves / mpiSize;

    // === Create initial partitioning of the AMDiS mesh. ===

    TraverseStack stack;
    ElInfo *elInfo = stack.traverseFirst(mesh, -1, Mesh::CALL_LEAF_EL);
    while (elInfo) {
      Element *element = elInfo->getElement();

      TEST_EXIT(element->getElementData(PARTITION_ED) == NULL)
	("mesh already partitioned\n");

      PartitionElementData *elData = 
	new PartitionElementData(element->getElementData());
      element->setElementData(elData);

      if ((element->getIndex() >= mpiRank * elPerRank &&
	   element->getIndex() < (mpiRank + 1) * elPerRank) ||
	  (element->getIndex() >= mpiSize * elPerRank &&
	   mpiRank == mpiSize - 1))
	elData->setPartitionStatus(IN);
      else
	elData->setPartitionStatus(UNDEFINED);
      
      elInfo = stack.traverseNext(elInfo);
    }
  }


  void ParMetisPartitioner::partition(std::map<int, double> *elemWeights,
				      PartitionMode mode,
				      float itr) 
  {
    FUNCNAME("ParMetisPartitioner::partition()");

    int mpiSize = mpiComm->Get_size();

    // === create parmetis mesh ===
    if (parMetisMesh) 
      delete parMetisMesh;

    parMetisMesh = new ParMetisMesh(mesh, mpiComm);

    int nElements = parMetisMesh->getNumElements();

    // === create weight array ===
    int *wgts = elemWeights ? new int[nElements] : NULL;
    float *floatWgts = elemWeights ? new float[nElements] : NULL;
    float maxWgt = 0.0;
    float *ptr_floatWgts = floatWgts;

    TraverseStack stack;
    ElInfo *elInfo = stack.traverseFirst(mesh, -1, Mesh::CALL_EVERY_EL_PREORDER);
    while (elInfo) {
      Element *element = elInfo->getElement();

      // get partition data
      PartitionElementData *partitionData = dynamic_cast<PartitionElementData*>
	(element->getElementData(PARTITION_ED));

      if (partitionData &&
	  partitionData->getPartitionStatus() == IN &&
	  partitionData->getLevel() == 0) {

	int index = element->getIndex();

	// get weight 
	float wgt = static_cast<float>((*elemWeights)[index]);
	maxWgt = max(wgt, maxWgt);

	// write float weight
	*ptr_floatWgts = wgt;
	ptr_floatWgts++;
      }
      elInfo = stack.traverseNext(elInfo);
    }

    float tmp;
    mpiComm->Allreduce(&maxWgt, &tmp, 1, MPI_FLOAT, MPI_MAX);
    maxWgt = tmp;

    // === create dual graph ===
    ParMetisGraph parMetisGraph(parMetisMesh, mpiComm);

    // === partitioning of dual graph ===
    int wgtflag = elemWeights ? 2 : 0; // weights at vertices only!
    int numflag = 0; // c numbering style!
    int ncon = elemWeights ? 1 : 0; // one weight at each vertex!
    int nparts = mpiSize; // number of partitions

    float *tpwgts = elemWeights ? new float[mpiSize] : NULL;
    float ubvec = 1.05;
    int options[3] = {0, 0, 0}; // default options
    int edgecut = -1;
    int *part = new int[nElements];
    
    if (elemWeights) {
      // set tpwgts
      for (int i = 0; i < mpiSize; i++)
	tpwgts[i] = 1.0 / nparts;

      float scale = 10000.0 / maxWgt;
      // scale wgts
      for (int i = 0; i < nElements; i++)
	wgts[i] = static_cast<int>(floatWgts[i] * scale);
    }

    MPI_Comm tmpComm = MPI_Comm(*mpiComm);

    switch(mode) {
    case INITIAL:
      ParMETIS_V3_PartKway(parMetisMesh->getElementDist(),
			   parMetisGraph.getXAdj(),
			   parMetisGraph.getAdjncy(),
			   wgts,
			   NULL,
			   &wgtflag,
			   &numflag,
			   &ncon,
			   &nparts,
			   tpwgts,
			   &ubvec,
			   options,
			   &edgecut,
			   part,
			   &tmpComm);
      break;
    case ADAPTIVE_REPART:
      {
	int *vsize = new int[nElements];
	for (int i = 0; i < nElements; i++)
	  vsize[i] = 1;
	ParMETIS_V3_AdaptiveRepart(parMetisMesh->getElementDist(),
				   parMetisGraph.getXAdj(),
				   parMetisGraph.getAdjncy(),
				   wgts,
				   NULL,
				   vsize,
				   &wgtflag,
				   &numflag,
				   &ncon,
				   &nparts,
				   tpwgts,
				   &ubvec,
				   &itr,
				   options,
				   &edgecut,
				   part,
				   &tmpComm);
	delete [] vsize;
      }
      break;
    case REFINE_PART:
      ParMETIS_V3_RefineKway(parMetisMesh->getElementDist(),
			     parMetisGraph.getXAdj(),
			     parMetisGraph.getAdjncy(),
			     wgts,
			     NULL,
			     &wgtflag,
			     &numflag,
			     &ncon,
			     &nparts,
			     tpwgts,
			     &ubvec,
			     options,
			     &edgecut,
			     part,
			     &tmpComm);
      break;
    default: 
      ERROR_EXIT("unknown partitioning mode\n");
    }

    // === distribute new partition data ===
    distributePartitioning(part);

    if (floatWgts) 
      delete [] floatWgts;
    if (wgts) 
      delete [] wgts;
    if (tpwgts) 
      delete [] tpwgts;

    delete [] part;
  }


  void ParMetisPartitioner::fillCoarsePartitionVec(std::map<int, int> *partitionVec)
  {
    TEST_EXIT(partitionVec)("no partition vector\n");

    partitionVec->clear();

    // update ParMETIS mesh to new partitioning
    if (!parMetisMesh)
      parMetisMesh = new ParMetisMesh(mesh, mpiComm);    

    int mpiRank = mpiComm->Get_rank();
    int mpiSize = mpiComm->Get_size();
    int *nPartitionElements = new int[mpiSize];
    int *elmdist = parMetisMesh->getElementDist();

    for (int i = 0;  i < mpiSize; i++)
      nPartitionElements[i] = elmdist[i + 1] - elmdist[i];

    // === count number of elements ===
    int nElements = 0;
    int localElements = parMetisMesh->getNumElements();
    mpiComm->Allreduce(&localElements, &nElements, 1, MPI_INT, MPI_SUM);

    int *partitionElements = new int[nElements];

    // distribute partition elements
    mpiComm->Allgatherv(parMetisMesh->getAMDiSIndices(),
			nPartitionElements[mpiRank], 
			MPI_INT, 
			partitionElements, 
			nPartitionElements, 
			elmdist, 
			MPI_INT);

    // fill partitionVec
    for (int i = 0; i < mpiSize; i++)
      for (int j = 0; j < nPartitionElements[i]; j++)
	(*partitionVec)[partitionElements[elmdist[i] + j]] = i;

    delete [] partitionElements;
    delete [] nPartitionElements;
  }


  void ParMetisPartitioner::distributePartitioning(int *part) 
  {
    FUNCNAME("ParMetisPartitioner::distributePartitioning()");

    int mpiSize = mpiComm->Get_size();
    int mpiRank = mpiComm->Get_rank();
    int nElements = parMetisMesh->getNumElements();

    // nPartitionElements[i] is the number of elements for the i-th partition
    int *nPartitionElements = new int[mpiSize];
    for (int i = 0; i < mpiSize; i++) 
      nPartitionElements[i] = 0;
    for (int i = 0; i < nElements; i++)
      nPartitionElements[part[i]]++;

    // collect number of partition elements from all ranks for this rank
    int *nRankElements = new int[mpiSize];
    mpiComm->Alltoall(nPartitionElements, 1, MPI_INT, nRankElements, 1, MPI_INT);

    // sum up partition elements over all ranks
    int *sumPartitionElements = new int[mpiSize];
    mpiComm->Allreduce(nPartitionElements, sumPartitionElements, mpiSize,
		       MPI_INT, MPI_SUM);
    
    // prepare distribution (fill partitionElements with AMDiS indices)
    int *bufferOffset = new int[mpiSize];
    bufferOffset[0] = 0;
    for (int i = 1; i < mpiSize; i++)
      bufferOffset[i] = bufferOffset[i - 1] + nPartitionElements[i - 1];

    int *partitionElements = new int[nElements];
    int **partitionPtr = new int*[mpiSize];

    for (int i = 0; i < mpiSize; i++)
      partitionPtr[i] = partitionElements + bufferOffset[i];

    for (int i = 0; i < nElements; i++) {
      int partition = part[i];
      int amdisIndex = parMetisMesh->getAMDiSIndex(i);
      *(partitionPtr[partition]) = amdisIndex;
      ++(partitionPtr[partition]);
    }

    // all to all: partition elements to rank elements
    int *rankElements = new int[sumPartitionElements[mpiRank]];
    int *recvBufferOffset = new int[mpiSize];
    recvBufferOffset[0] = 0;
    for (int i = 1; i < mpiSize; i++)
      recvBufferOffset[i] = recvBufferOffset[i - 1] + nRankElements[i - 1];

    mpiComm->Alltoallv(partitionElements, 
		       nPartitionElements,
		       bufferOffset,
		       MPI_INT,
		       rankElements,
		       nRankElements,
		       recvBufferOffset,
		       MPI_INT);
    

    // Create map which stores for each element index on ther partitioning level
    // if the element is in the partition of this rank.
    std::map<int, bool> elementInPartition;
    for (int i = 0; i < mpiSize; i++) {
      int *rankStart = rankElements + recvBufferOffset[i];
      int *rankEnd = rankStart + nRankElements[i];
      for (int *rankPtr = rankStart; rankPtr < rankEnd; ++rankPtr)
	elementInPartition[*rankPtr] = true;
    }

    TraverseStack stack;
    ElInfo *elInfo = stack.traverseFirst(mesh, -1, Mesh::CALL_EVERY_EL_PREORDER);
    while (elInfo) {
      Element *element = elInfo->getElement();

      // get partition data
      PartitionElementData *partitionData = dynamic_cast<PartitionElementData*>
	(element->getElementData(PARTITION_ED));

      if (partitionData && partitionData->getLevel() == 0) {
	int amdisIndex = element->getIndex();
	if (elementInPartition[amdisIndex])
	  partitionData->setPartitionStatus(IN);
	else
	  partitionData->setPartitionStatus(OUT);
	
	descendPartitionData(element);
      }

      elInfo = stack.traverseNext(elInfo);
    }

    delete parMetisMesh;
    parMetisMesh = NULL;

    delete [] rankElements;
    delete [] nPartitionElements;
    delete [] nRankElements;
    delete [] sumPartitionElements;
    delete [] partitionElements;
    delete [] partitionPtr;
    delete [] bufferOffset;
    delete [] recvBufferOffset;
  }


  void ParMetisPartitioner::descendPartitionData(Element *element) 
  {
    FUNCNAME("ParMetisPartitioner::descendPartitionData()");

    if (!element->isLeaf()) {
      Element *child0 = element->getChild(0);
      Element *child1 = element->getChild(1);

      // get partition data
      PartitionElementData *parentData = dynamic_cast<PartitionElementData*>
	(element->getElementData(PARTITION_ED));
      PartitionElementData *child0Data = dynamic_cast<PartitionElementData*>
	(child0->getElementData(PARTITION_ED));
      PartitionElementData *child1Data = dynamic_cast<PartitionElementData*>
	(child1->getElementData(PARTITION_ED));
      
      TEST_EXIT(parentData && child0Data && child1Data)("no partition data\n");

      child0Data->setPartitionStatus(parentData->getPartitionStatus());
      child1Data->setPartitionStatus(parentData->getPartitionStatus());

      descendPartitionData(child0);
      descendPartitionData(child1);
    }
  }


  void ParMetisPartitioner::fillLeafPartitionVec(std::map<int, int> *coarseVec,
						 std::map<int, int> *fineVec)
  {
    int partition = -1;
    TraverseStack stack;
    ElInfo *elInfo = stack.traverseFirst(mesh, -1, Mesh::CALL_EVERY_EL_PREORDER);
    while (elInfo) {
      Element *element = elInfo->getElement();
      PartitionElementData *partitionData = dynamic_cast<PartitionElementData*>
	(element->getElementData(PARTITION_ED));
      if (partitionData) {
	if (partitionData->getLevel() == 0)
	  partition = (*(coarseVec))[element->getIndex()];
	if (element->isLeaf())
	  (*(fineVec))[element->getIndex()] = partition;
      }
      elInfo = stack.traverseNext(elInfo);
    }
  }
}