//---------------------------------Spheral++----------------------------------//
// VoronoiRedistributeNodes
//
// This algorithm uses the Voronoi tessellation to decide how to domain
// decompose our points.  The idea is to relax a set of generator points into
// the SPH node distribution -- the generators are attracted to the SPH points
// repelled by one and other.  These generator points then become the seeds to
// draw the Voronoi tessellation about, each cell of which then represents a
// computational domain.
//
// Created by JMO, Fri Jan 15 09:56:56 PST 2010
//----------------------------------------------------------------------------//
#include "VoronoiRedistributeNodes.hh"
#include "Utilities/DomainNode.hh"
#include "Boundary/Boundary.hh"
#include "DataBase/DataBase.hh"
#include "Field/FieldList.hh"
#include "Field/Field.hh"
#include "FieldOperations/binFieldList2Lattice.hh"
#include "NodeList/NodeList.hh"
#include "Kernel/TableKernel.hh"
#include "Kernel/BSplineKernel.hh"
#include "Utilities/globalNodeIDs.hh"
#include "Utilities/RedistributionRegistrar.hh"
#include "Utilities/safeInv.hh"
#include "Utilities/boundPointWithinBox.hh"
#include "Utilities/testBoxIntersection.hh"
#include "Utilities/PairComparisons.hh"
#include "Utilities/allReduce.hh"
#include "Communicator.hh"

#include "Utilities/DBC.hh"

#include <algorithm>
#include <sstream>
#include <fstream>
#include <cstdlib>
#include <bitset>
using std::vector;
using std::string;
using std::pair;
using std::make_pair;
using std::cout;
using std::cerr;
using std::endl;
using std::min;
using std::max;
using std::abs;

#include <boost/assign.hpp>

namespace Spheral {

//------------------------------------------------------------------------------
// Helper method to find the nearest position in a vector of positions to the
// given one.
//------------------------------------------------------------------------------
template<typename Dimension>
size_t
findNearestGenerator(const typename Dimension::Vector& xi,
                     const vector<typename Dimension::Vector>& generators,
                     const vector<int>& generatorFlags) {

  // This N^2 thing shouldn't be too bad until we get to lots of processors.
  // Then we'll have to do something smarter.
  size_t result = 0;
  double minR2 = DBL_MAX;
  for (size_t igen = 0; igen != generators.size(); ++igen) {
    if (generatorFlags[igen] == 1) {
      const double dr2 = (xi - generators[igen]).magnitude2();
      // integrateThroughMeshAlongSegment<Dimension, double>(workBins, xmin, xmax, ncells, generators[igen], xi);
      if (dr2 < minR2) {
        minR2 = dr2;
        result = igen;
      }
    }
  }
  ENSURE(result < generators.size() or (generators.size() == 0 and result == 0));
  return result;
}

//------------------------------------------------------------------------------
// Compute the center cell position.
//------------------------------------------------------------------------------
template<typename Dimension>
inline
typename Dimension::Vector
computeCellPosition(const typename Dimension::Vector& xmin,
		    const typename Dimension::Vector& xmax,
		    const unsigned nxCells,
		    const unsigned index);

template<>
inline
Dim<1>::Vector
computeCellPosition<Dim<1> >(const Dim<1>::Vector& xmin,
			     const Dim<1>::Vector& xmax,
			     const unsigned nxCells,
			     const unsigned index) {
  REQUIRE(index < nxCells);
  return Dim<1>::Vector(xmin.x() + (xmax.x() - xmin.x())/nxCells * (index + 0.5));
}

template<>
inline
Dim<2>::Vector
computeCellPosition<Dim<2> >(const Dim<2>::Vector& xmin,
			     const Dim<2>::Vector& xmax,
			     const unsigned nxCells,
			     const unsigned index) {
  REQUIRE(index < nxCells*nxCells);
  const unsigned ix = index % nxCells;
  const unsigned iy = index / nxCells;
  const Dim<2>::Vector xstep = (xmax - xmin)/nxCells;
  return xmin + Dim<2>::Vector(xstep.x() * (ix + 0.5),
                               xstep.y() * (iy + 0.5));
}

template<>
inline
Dim<3>::Vector
computeCellPosition<Dim<3> >(const Dim<3>::Vector& xmin,
			     const Dim<3>::Vector& xmax,
			     const unsigned nxCells,
			     const unsigned index) {
  REQUIRE(index < nxCells*nxCells*nxCells);
  const unsigned iz = index / (nxCells*nxCells);
  const unsigned iy = (index - iz*nxCells*nxCells) / nxCells;
  const unsigned ix = index % nxCells;
  const Dim<3>::Vector xstep = (xmax - xmin)/nxCells;
  return xmin + Dim<3>::Vector(xstep.x() * (ix + 0.5),
                               xstep.y() * (iy + 0.5),
                               xstep.z() * (iz + 0.5));
}

//------------------------------------------------------------------------------
// Compute the cell boundaries.
//------------------------------------------------------------------------------
template<typename Dimension>
inline
void
computeCellBoundaries(const typename Dimension::Vector& xmin,
		      const typename Dimension::Vector& xmax,
		      const size_t nxCells,
		      const size_t index,
		      typename Dimension::Vector& xcellMin,
		      typename Dimension::Vector& xcellMax);

template<>
inline
void
computeCellBoundaries<Dim<1> >(const Dim<1>::Vector& xmin,
			       const Dim<1>::Vector& xmax,
			       const size_t nxCells,
			       const size_t index,
			       Dim<1>::Vector& xcellMin,
			       Dim<1>::Vector& xcellMax) {
  REQUIRE(index < nxCells);
  typedef Dim<1>::Vector Vector;
  const Vector xstep = (xmax - xmin)/nxCells;
  xcellMin = Vector(xstep.x()*index);
  xcellMax = Vector(xstep.x()*(index + 1));
}

template<>
inline
void
computeCellBoundaries<Dim<2> >(const Dim<2>::Vector& xmin,
			       const Dim<2>::Vector& xmax,
			       const size_t nxCells,
			       const size_t index,
			       Dim<2>::Vector& xcellMin,
			       Dim<2>::Vector& xcellMax) {
  REQUIRE(index < nxCells*nxCells);
  typedef Dim<2>::Vector Vector;
  const size_t ix = index % nxCells;
  const size_t iy = index / nxCells;
  const Vector xstep = (xmax - xmin)/nxCells;
  xcellMin = Vector(xstep.x()*ix,
		    xstep.y()*iy);
  xcellMin = Vector(xstep.x()*(ix + 1),
		    xstep.y()*(iy + 1));
}

template<>
inline
void
computeCellBoundaries<Dim<3> >(const Dim<3>::Vector& xmin,
			       const Dim<3>::Vector& xmax,
			       const size_t nxCells,
			       const size_t index,
			       Dim<3>::Vector& xcellMin,
			       Dim<3>::Vector& xcellMax) {
  REQUIRE(index < nxCells*nxCells*nxCells);
  typedef Dim<3>::Vector Vector;
  const size_t iz = 2*(index / (nxCells*nxCells));
  const size_t iy = 2*((index - iz/2) / nxCells);
  const size_t ix = 2*(index % nxCells);
  const Vector xstep = (xmax - xmin)/nxCells;
  xcellMin = Vector(xstep.x() * ix,
		    xstep.y() * iy,
		    xstep.z() * iz);
  xcellMax = Vector(xstep.x() * (ix + 1),
		    xstep.y() * (iy + 1),
		    xstep.z() * (iz + 1));
}

//------------------------------------------------------------------------------
// Compute the subcell positions.
//------------------------------------------------------------------------------
// 1-D
inline
vector<Dim<1>::Vector>
computeDaughterPositions(const Dim<1>::Vector& xmin,
                         const Dim<1>::Vector& xmax) {
  typedef Dim<1>::Vector Vector;
  vector<Vector> result;
  const Vector delta = xmax - xmin;
  result.push_back(xmin + 0.25*delta);
  result.push_back(xmin + 0.75*delta);
  ENSURE(result.size() == 2);
  return result;
}

// 2-D
inline
vector<Dim<2>::Vector>
computeDaughterPositions(const Dim<2>::Vector& xmin,
                         const Dim<2>::Vector& xmax) {
  typedef Dim<2>::Vector Vector;
  vector<Vector> result;
  const Vector delta = xmax - xmin;
  result.push_back(Vector(xmin.x() + 0.25*delta.x(), xmin.y() + 0.25*delta.y()));
  result.push_back(Vector(xmin.x() + 0.75*delta.x(), xmin.y() + 0.25*delta.y()));
  result.push_back(Vector(xmin.x() + 0.25*delta.x(), xmin.y() + 0.75*delta.y()));
  result.push_back(Vector(xmin.x() + 0.75*delta.x(), xmin.y() + 0.75*delta.y()));
  ENSURE(result.size() == 4);
  return result;
}

// 3-D
inline
vector<Dim<3>::Vector>
computeDaughterPositions(const Dim<3>::Vector& xmin,
                         const Dim<3>::Vector& xmax) {
  typedef Dim<3>::Vector Vector;
  vector<Vector> result;
  const Vector delta = xmax - xmin;
  result.push_back(Vector(xmin.x() + 0.25*delta.x(), xmin.y() + 0.25*delta.y(), xmin.z() + 0.25*delta.z()));
  result.push_back(Vector(xmin.x() + 0.75*delta.x(), xmin.y() + 0.25*delta.y(), xmin.z() + 0.25*delta.z()));
  result.push_back(Vector(xmin.x() + 0.25*delta.x(), xmin.y() + 0.75*delta.y(), xmin.z() + 0.25*delta.z()));
  result.push_back(Vector(xmin.x() + 0.75*delta.x(), xmin.y() + 0.75*delta.y(), xmin.z() + 0.25*delta.z()));
  result.push_back(Vector(xmin.x() + 0.25*delta.x(), xmin.y() + 0.25*delta.y(), xmin.z() + 0.75*delta.z()));
  result.push_back(Vector(xmin.x() + 0.75*delta.x(), xmin.y() + 0.25*delta.y(), xmin.z() + 0.75*delta.z()));
  result.push_back(Vector(xmin.x() + 0.25*delta.x(), xmin.y() + 0.75*delta.y(), xmin.z() + 0.75*delta.z()));
  result.push_back(Vector(xmin.x() + 0.75*delta.x(), xmin.y() + 0.75*delta.y(), xmin.z() + 0.75*delta.z()));
  ENSURE(result.size() == 8);
  return result;
}

//------------------------------------------------------------------------------
// Compute the node position closest to the cell center.
//------------------------------------------------------------------------------
template<typename Dimension>
inline
typename Dimension::Vector
computeClosestNodePosition(const typename Dimension::Vector& targetPosition,
                           const vector<DomainNode<Dimension> >& nodes,
                           const int numProcs,
                           MPI_Comm communicator) {
  typedef typename Dimension::Vector Vector;

  // First find the local node closest to the center.
  Vector localResult;
  double minr2 = DBL_MAX;
  const size_t n = nodes.size();
  for (typename vector<DomainNode<Dimension> >::const_iterator itr = nodes.begin();
       itr != nodes.end();
       ++itr) {
    const double r2 = (itr->position - targetPosition).magnitude2();
    if (r2 < minr2) {
      localResult = itr->position;
      minr2 = r2;
    }
  }
  CHECK(minr2 < DBL_MAX);

  // Find the global minimum.
  Vector result;
  minr2 = DBL_MAX;
  for (int sendProc = 0; sendProc != numProcs; ++sendProc) {
    vector<char> buffer;
    packElement(localResult, buffer);
    MPI_Bcast(&buffer.front(), buffer.size(), MPI_CHAR, sendProc, communicator);
    vector<char>::const_iterator itr = buffer.begin();
    Vector xi;
    unpackElement(xi, itr, buffer.end());
    CHECK(itr == buffer.end());
    const double r2 = (xi - targetPosition).magnitude2();
    if (r2 < minr2) {
      result = xi;
      minr2 = r2;
    }
  }
  CHECK(minr2 < DBL_MAX);

  // That's it.
  return result;
}

//------------------------------------------------------------------------------
// Constructor.
//------------------------------------------------------------------------------
template<typename Dimension>
VoronoiRedistributeNodes<Dimension>::
VoronoiRedistributeNodes(double dummy,
                         const bool workBalance,
                         const bool balanceGenerators,
                         const double tolerance,
                         const unsigned maxIterations):
  RedistributeNodes<Dimension>(),
  mWorkBalance(workBalance),
  mBalanceGenerators(balanceGenerators),
  mTolerance(tolerance),
  mMaxIterations(maxIterations) {
}

//------------------------------------------------------------------------------
// Destructor
//------------------------------------------------------------------------------
template<typename Dimension>
VoronoiRedistributeNodes<Dimension>::
~VoronoiRedistributeNodes() {
}

//------------------------------------------------------------------------------
// The main method of this class.  Call on the Voronoi library to describe an
// optimal partioning of the nodes, and then apply that partitioning.
//------------------------------------------------------------------------------
template<typename Dimension>
void
VoronoiRedistributeNodes<Dimension>::
redistributeNodes(DataBase<Dimension>& dataBase,
                  vector<Boundary<Dimension>*> boundaries) {

  // The usual parallel info.
  const int numProcs = this->numDomains();
  const int procID = this->domainID();

  // Get the global IDs.
  const FieldList<Dimension, int> globalIDs = globalNodeIDs(dataBase);
  const size_t numNodeLists = dataBase.numNodeLists();

  // Compute the work and number density per node.
  const TableKernel<Dimension> W(BSplineKernel<Dimension>(), 100);
  FieldList<Dimension, Scalar> workField = dataBase.newGlobalFieldList(1.0, "work");
  if (this->workBalance()) { // or mBalanceGenerators) {

    // Enforce boundary conditions for the work computation.
    for (typename DataBase<Dimension>::NodeListIterator nodeListItr = dataBase.nodeListBegin();
         nodeListItr != dataBase.nodeListEnd();
         ++nodeListItr) {
      (*nodeListItr)->numGhostNodes(0);
      (*nodeListItr)->neighbor().updateNodes();
    }
    for (typename vector<Boundary<Dimension>*>::iterator boundaryItr = boundaries.begin();
         boundaryItr != boundaries.end();
         ++boundaryItr) {
      (*boundaryItr)->setAllGhostNodes(dataBase);
      (*boundaryItr)->finalizeGhostBoundary();
      for (typename DataBase<Dimension>::FluidNodeListIterator nodeListItr = dataBase.fluidNodeListBegin();
           nodeListItr != dataBase.fluidNodeListEnd();
           ++nodeListItr) (*nodeListItr)->neighbor().updateNodes();
    }

    // Update the connectivity.
    dataBase.updateConnectivityMap(false, false);

    // Get the local description of the domain distribution, with the work per node filled in.
    if (this->workBalance()) workField = this->workPerNode(dataBase, 1.0);
  }

  // Print the beginning statistics.
  std::string stats0 = this->gatherDomainDistributionStatistics(workField);
  if (procID == 0) cout << "VoronoiRedistributeNodes: INITIAL node distribution statistics:" << endl
                        << stats0 << endl;

  // Now we can get the node distribution description.
  vector<DomainNode<Dimension> > nodeDistribution = this->currentDomainDecomposition(dataBase, globalIDs, workField);
  const size_t numNodes = nodeDistribution.size();
  const size_t numNodesGlobal = allReduce((uint64_t) numNodes, MPI_SUM, Communicator::communicator());
  const size_t avgNumNodes = numNodesGlobal/numProcs;
  CHECK(numNodes > 0);

  // Clear out any ghost nodes.
  // We won't bother to update the neighbor info at this point -- we don't need
  // it for this algorithm, so we just update it when we're done.
  for (typename DataBase<Dimension>::NodeListIterator nodeListItr = dataBase.nodeListBegin();
       nodeListItr != dataBase.nodeListEnd();
       ++nodeListItr) (*nodeListItr)->numGhostNodes(0);

  // Get the bounding boxes for the node set.
  Vector xmin, xmax;
  dataBase.boundingBox(xmin, xmax);

  // Define the the length scale we use to determine when the generator positions have converged.
  const double tol = (xmax - xmin).minElement() * mTolerance;
  if (procID == 0) cerr << "VoronoiRedistributeNodes: Found bounding box of " << xmin << " " << xmax << endl
                        << "                          yielding generator convergence tolerance of " << tol << endl;

  // Determine the average work per generator.
  const Scalar totWork = workField.sumElements();
  const Scalar avgWork = totWork/numProcs;

  // Try and distribute the seeds according to where the work is by an AMR like zooming
  // in of the work distribution in bins.
  vector<Vector> generators(numProcs, 0.5*(xmin + xmax));
  vector<pair<Vector, Vector> > generatorBounds(numProcs, make_pair(xmin, xmax));
  const size_t numDaughters = (1U << Dimension::nDim);
  vector<unsigned> ncells(Dimension::nDim, 2U);
  {
    // Everyone starts out in the same bin.
    vector<vector<size_t> > generatorsInParents(1);
    vector<pair<Vector, Vector> > parentCells;
    for (size_t igen = 0; igen != numProcs; ++igen) generatorsInParents[0].push_back(igen);
    parentCells.push_back(make_pair(xmin, xmax));

    // Descend until we either get each generator in an individual cell or hit the maximum number
    // of allowed levels
    size_t level = 0;
    size_t numRemainingGenerators = numProcs;
    while (generatorsInParents.size() > 0 and level < mMaxIterations) {
      CHECK(generatorsInParents.size() == parentCells.size());
      ++level;
      numRemainingGenerators = 0;
      vector<vector<size_t> > newGeneratorsInParents;
      vector<pair<Vector, Vector> > newParentCells;

      // Walk each parent cell.
      for (size_t k = 0; k != generatorsInParents.size(); ++k) {
        const vector<size_t>& gens = generatorsInParents[k];
        const size_t numParentGenerators = gens.size();
        const Vector& xminParent = parentCells[k].first;
        const Vector& xmaxParent = parentCells[k].second;

        // Break up the work in this parent cell into 2^ndim subcells.
        const vector<double> workBins = binFieldList2Lattice(workField, xminParent, xmaxParent, ncells);
        const double workParent = accumulate(workBins.begin(), workBins.end(), 0.0);
        CHECK(workBins.size() == numDaughters);
        CHECK(workParent > 0.0);

        // Sort the daughters by work.
        vector<pair<double, size_t> > sortedDaughters;
        for (size_t k = 0; k != numDaughters; ++k) sortedDaughters.push_back(make_pair(workBins[k], k));
        sort(sortedDaughters.begin(), sortedDaughters.end(), ComparePairsByFirstElementInDecreasingOrder<pair<double, size_t> >());

        // Figure out how many generators we're assigning to each daughter.
        vector<size_t> numGensForDaughter(numDaughters, 0);
        {
          size_t numAssigned = 0;
          size_t k = 0;
          while (numAssigned < numParentGenerators and k < numDaughters and sortedDaughters[k].first > 0.0) {
            const double work = sortedDaughters[k].first;
            const size_t kdaughter = sortedDaughters[k].second;
            numGensForDaughter[kdaughter] = min(numParentGenerators - numAssigned, size_t(work*safeInv(workParent, 1.0e-30)*numParentGenerators + 1));
            numAssigned += numGensForDaughter[kdaughter];
            ++k;
          }

          // Pick up any stragglers.
          VERIFY(numAssigned == numParentGenerators);
          if (numAssigned < numParentGenerators) {
            k = 0;
            while (numAssigned < numParentGenerators and k < numDaughters and sortedDaughters[k].first > 0.0) {
              const size_t kdaughter = sortedDaughters[k].second;
              ++numGensForDaughter[kdaughter];
              ++numAssigned;
              ++k;
            }
          }
          VERIFY(numAssigned == numParentGenerators);
        }

//         // Iterate until we have assigned each of the parent generators to a daughter.
//         vector<size_t> numGensForDaughter(numDaughters, 0);
//         int numRemaining = numParentGenerators;
//         int iteration = 0;
//         while (iteration < mMaxIterations and numRemaining > 0) {
//           ++iteration;
//           int numStillRemaining = numRemaining;
//           for (size_t kdaughter = 0; kdaughter != workBins.size(); ++kdaughter) {
//             const size_t delta = min(size_t(numStillRemaining), size_t(workBins[kdaughter]*numRemaining*safeInv(workParent, 1.0e-30) + 0.5 + 0.5*double(iteration)/double(max(1U, mMaxIterations - 1))));
//             numGensForDaughter[kdaughter] += delta;
//             numStillRemaining -= delta;
//             CHECK(numStillRemaining >= 0);
//           }
//           numRemaining = numStillRemaining;
//         }
//         VERIFY(numRemaining == 0);

        // Find the positions of the daughter cells.
        const vector<Vector> daughterPositions = computeDaughterPositions(xminParent, xmaxParent);
        CHECK(daughterPositions.size() == numDaughters);

        // Now assign the generator positions and the next generation of parents.
        vector<size_t>::const_iterator genItr = gens.begin();
        for (size_t kdaughter = 0; kdaughter != numDaughters; ++kdaughter) {
          CHECK(genItr + numGensForDaughter[kdaughter] <= gens.end());
          const Vector dcell = 0.25*(xmaxParent - xminParent);
          const Vector xminDaughter = daughterPositions[kdaughter] - dcell;
          const Vector xmaxDaughter = daughterPositions[kdaughter] + dcell;
          if (numGensForDaughter[kdaughter] == 1) {
            generators[*genItr] = computeClosestNodePosition<Dimension>(0.5*(xminDaughter + xmaxDaughter),
                                                                        nodeDistribution, numProcs, Communicator::communicator());
            generatorBounds[*genItr] = make_pair(xminDaughter, xmaxDaughter);
            CHECK(testPointInBox(generators[*genItr], xminDaughter, xmaxDaughter));
          } else if (numGensForDaughter[kdaughter] > 1) {
            for (vector<size_t>::const_iterator itr = genItr; itr != genItr + numGensForDaughter[kdaughter]; ++itr) {
              generators[*itr] = daughterPositions[kdaughter];
              generatorBounds[*itr] = make_pair(xminDaughter, xmaxDaughter);
            }
            newGeneratorsInParents.push_back(vector<size_t>(genItr, genItr + numGensForDaughter[kdaughter]));
            newParentCells.push_back(make_pair(xminDaughter, xmaxDaughter));
            numRemainingGenerators += numGensForDaughter[kdaughter];
          }
          genItr += numGensForDaughter[kdaughter];
        }
        CHECK(genItr == gens.end());
      }

      // Assign the next generation.
      CHECK(newGeneratorsInParents.size() == newParentCells.size());
      generatorsInParents = newGeneratorsInParents;
      parentCells = newParentCells;
      if (procID == 0) cerr << "   Generation " << level << " : "
                            << numRemainingGenerators << " generators remaining in "
                            << generatorsInParents.size() << " cells."
                            << endl;
    }
    VERIFY(numRemainingGenerators == 0);

//     // Are there still remaining degeneracies in the generator positions?
//     if (numRemainingGenerators > 0) {
//       if (procID == 0) cerr << "  --> Breaking up " << numRemainingGenerators
//                             << " degeneracies in intial generator positions."
//                             << endl;
//       for (vector<vector<size_t> >::const_iterator cellItr = generatorsInParents.begin();
//            cellItr != generatorsInParents.end();
//            ++cellItr) {
//         for (vector<size_t>::const_iterator genItr = cellItr->begin();
//              genItr != cellItr->end();
//              ++genItr) {
//           const size_t igen = *genItr;
//           if (procID == igen) generators[igen] = nodeDistribution[numNodes/2].position;
//           vector<char> buffer;
//           packElement(generators[igen], buffer);
//           MPI_Bcast(&buffer.front(), buffer.size(), MPI_CHAR, igen, Communicator::communicator());
//           vector<char>::const_iterator itr = buffer.begin();
//           unpackElement(generators[igen], itr, buffer.end());
//           CHECK(itr == buffer.end());
//         }
//       }
//     }

  }

  // Copy the initial generator distribution.
  const vector<Vector> startingGenerators(generators);

//   // Stage 1:  Lloyds algorithm iteration of the generator positions.
//   // Choose the initial positions of the generators randomly, one per domain.
//   vector<Vector> generators(numProcs);
//   for (size_t igen = 0; igen != numProcs; ++igen) {
//     if (procID == igen) generators[igen] = nodeDistribution[numNodes/2].position;
//     vector<char> buffer;
//     packElement(generators[igen], buffer);
//     MPI_Bcast(&buffer.front(), buffer.size(), MPI_CHAR, igen, Communicator::communicator());
//     Vector xgen;
//     vector<char>::const_iterator itr = buffer.begin();
//     unpackElement(xgen, itr, buffer.end());
//     CHECK(itr == buffer.end());
//     generators[igen] = xgen;
//   }

  // Set all nodes as unassigned.
  for (size_t i = 0; i != nodeDistribution.size(); ++i) nodeDistribution[i].domainID = -1;

  // Assign the Voronoi distribution based on the initial seeds.
  vector<double> generatorWork(generators.size(), 0.0);
  vector<int> generatorFlags(generators.size(), 1);
  double minWork, maxWork;
  unsigned minNodes, maxNodes;
  assignNodesToGenerators(generators,
                          generatorFlags,
                          generatorWork,
                          nodeDistribution,
                          minWork,
                          maxWork,
                          minNodes,
                          maxNodes);

  // Now iterate the generators until we either converge or hit the max iterations.
  size_t iteration = 0;
  double maxDeltaGenerator = 10.0*tol;
  double oldWorkRatio = maxWork*safeInv(minWork);
  double workRatio = 10.0*oldWorkRatio;
  while (iteration < mMaxIterations and
         maxDeltaGenerator > tol and
         abs(workRatio*safeInv(oldWorkRatio) - 1.0) > 0.001) {

    // Remember the starting generators.
    const vector<Vector> generators0(generators);

    // Iterate until we either have the desired work distribution for this pass or
    // all generators have been flagged.
    generatorFlags = vector<int>(generators.size(), 1);
    while (minWork/maxWork < 1.1 and accumulate(generatorFlags.begin(), generatorFlags.end(), 0) > 1) {

      // Cull out nodes from generators that have too much work.
      cullGeneratorNodesByWork(generators, generatorWork, avgWork, generatorFlags, nodeDistribution);

      // Reassign the loose nodes.
      assignNodesToGenerators(generators, generatorFlags, generatorWork, nodeDistribution,
                              minWork, maxWork, minNodes, maxNodes);

    }

    // Determine the new generators.
    computeCentroids(nodeDistribution, generators);

    // Reestablish the proper Voronoi distribution based on this iterations
    // generators.
    generatorWork = vector<double>(generators.size(), 0.0);
    generatorFlags = vector<int>(generators.size(), 1);
    for (size_t i = 0; i != nodeDistribution.size(); ++i) nodeDistribution[i].domainID = -1;
    assignNodesToGenerators(generators, generatorFlags, generatorWork, nodeDistribution,
                            minWork, maxWork, minNodes, maxNodes);

    // How much did the generators shift?
    maxDeltaGenerator = 0.0;
    for (size_t igen = 0; igen != generators.size(); ++igen) {
      maxDeltaGenerator = max(maxDeltaGenerator, (generators[igen] - generators0[igen]).magnitude2());
    }
    maxDeltaGenerator = sqrt(maxDeltaGenerator);

    // How much did the work distribution change?
    oldWorkRatio = workRatio;
    workRatio = maxWork*safeInv(minWork);

    // Report this iterations statistics.
    if (procID == 0) cerr << "VoronoiRedistributeNodes: Lloyds iteration " << iteration << endl
                          << "                          max change:  " << maxDeltaGenerator << endl
                          << "                          work ratio change:  " << workRatio << " " << oldWorkRatio << " " << abs(workRatio*safeInv(oldWorkRatio) - 1.0) << endl
                          << "                          [min, max, avg] work      [" << minWork << ", " << maxWork << ", " << avgWork << "]" << endl
                          << "                          [min, max, avg] num nodes [" << minNodes << ", " << maxNodes << ", " << avgNumNodes << "]" << endl;
    if (minWork == 0.0) {
      if (procID == 0) {
        cerr << "ERROR:  zero work associated with the following generators:" << endl;
        for (size_t k = 0; k != numProcs; ++k) {
          if (generatorWork[k] == 0.0) {
            cerr << "    ----->  " << generators[k] << endl;
            generators[k] = startingGenerators[k];
          }
        }
      }
    }
    //VERIFY(minWork > 0);
    ++iteration;
  }

  // Redistribute nodes between domains.
  CHECK(this->validDomainDecomposition(nodeDistribution, dataBase));
  this->enforceDomainDecomposition(nodeDistribution, dataBase);

  // Reinitialize neighbor info.
  for (typename DataBase<Dimension>::NodeListIterator nodeListItr = dataBase.nodeListBegin();
       nodeListItr != dataBase.nodeListEnd();
       ++nodeListItr) {
    (*nodeListItr)->neighbor().updateNodes();
  }

  // Notify everyone that the nodes have just been shuffled around.
  RedistributionRegistrar::instance().broadcastRedistributionNotifications();

  // Print the final statistics.
  std::string stats1 = this->gatherDomainDistributionStatistics(workField);
  if (Process::getRank() == 0) cout << "VoronoiRedistributeNodes: FINAL node distribution statistics:" << endl
                                    << stats1 << endl;
}

//------------------------------------------------------------------------------
// Split the given node distribution into ncells sets using a Voronoi Lloyds type
// algorithm.  The result is returned by setting the domainID attribute of each
// DomainNode to a value in the range [0, ncells[.
//------------------------------------------------------------------------------
template<typename Dimension>
void
VoronoiRedistributeNodes<Dimension>::
computeCentroids(const vector<DomainNode<Dimension> >& nodes,
                 vector<typename Dimension::Vector>& generators) const {

  const int numProcs = this->numDomains();
  const int procID = this->domainID();
  const size_t numGenerators = generators.size();
  REQUIRE(numGenerators == numProcs);

  // Initializations.
  const vector<Vector> generators0(generators);
  generators = vector<Vector>(numGenerators, Vector::zero);

  // Iterate over the nodes, assigning each to it's nearest generator.
  vector<double> normalization(numGenerators, 0.0);
  for (typename vector<DomainNode<Dimension> >::const_iterator itr = nodes.begin();
       itr != nodes.end();
       ++itr) {
    const DomainNode<Dimension>& node = *itr;
    const size_t igen = node.domainID;
    CHECK(igen < numGenerators);
    const double thpt = node.work;
    generators[igen] += thpt*node.position;
    normalization[igen] += thpt;
  }

  // Reduce the generator info across processors.
  vector<char> localBuffer;
  for (size_t igen = 0; igen != numGenerators; ++igen) {
    packElement(generators[igen], localBuffer);
    packElement(normalization[igen], localBuffer);
    generators[igen] = Vector::zero;
    normalization[igen] = 0.0;
  }
  for (size_t sendProc = 0; sendProc != numProcs; ++sendProc) {
    vector<char> buffer = localBuffer;
    MPI_Bcast(&buffer.front(), buffer.size(), MPI_CHAR, sendProc, Communicator::communicator());
    vector<char>::const_iterator itr = buffer.begin();
    for (size_t igen = 0; igen != numGenerators; ++igen) {
      Vector ri;
      double normi;
      unpackElement(ri, itr, buffer.end());
      unpackElement(normi, itr, buffer.end());
      generators[igen] += ri;
      normalization[igen] += normi;
    }
    CHECK(itr == buffer.end());
  }

  // Normalize the new generator positions and compute our final statistics.
  // We also force the generator to the position of the nearest node, ensuring
  // that all generators are actually in the the node distribution and have
  // at least some work.
  for (size_t igen = 0; igen != numGenerators; ++igen) {
    generators[igen] = 0.25*generators[igen]*safeInv(normalization[igen]) + 0.75*generators0[igen];
    generators[igen] = computeClosestNodePosition<Dimension>(generators[igen], nodes, numProcs, Communicator::communicator());
  }
}

//------------------------------------------------------------------------------
// Split the given node distribution into ncells sets using a Voronoi algorithm.
// The result is returned by setting the domainID attribute of each
// DomainNode to a value in the range [0, ncells).
//------------------------------------------------------------------------------
template<typename Dimension>
void
VoronoiRedistributeNodes<Dimension>::
assignNodesToGenerators(const vector<typename Dimension::Vector>& generators,
                        const vector<int>& generatorFlags,
                        vector<double>& generatorWork,
                        vector<DomainNode<Dimension> >& nodes,
                        double& minWork,
                        double& maxWork,
                        unsigned& minNodes,
                        unsigned& maxNodes) const {

  const int numProcs = this->numDomains();
  const int procID = this->domainID();

  // Initializations.
  minWork = DBL_MAX;
  maxWork = -DBL_MAX;
  minNodes = INT_MAX;
  maxNodes = 0;
  const size_t numGenerators = generators.size();
  generatorWork = vector<double>(numGenerators, 0.0);
  vector<unsigned> numNodesPerGenerator(numGenerators, 0);

  // Iterate over the nodes, assigning unassigned nodes to the nearest available generator.
  for (typename vector<DomainNode<Dimension> >::iterator itr = nodes.begin();
       itr != nodes.end();
       ++itr) {
    DomainNode<Dimension>& node = *itr;
    if (node.domainID == -1) {
      const size_t igen = findNearestGenerator<Dimension>(node.position, generators, generatorFlags);
      node.domainID = igen;
    }
    const size_t igen = node.domainID;
    CHECK(igen < numGenerators);
    generatorWork[igen] += node.work;
    ++numNodesPerGenerator[igen];
  }

  // Reduce the generator info across processors.
  vector<char> localBuffer;
  for (size_t igen = 0; igen != numGenerators; ++igen) {
    packElement(generatorWork[igen], localBuffer);
    packElement(numNodesPerGenerator[igen], localBuffer);
    generatorWork[igen] = 0.0;
    numNodesPerGenerator[igen] = 0;
  }
  for (size_t sendProc = 0; sendProc != numProcs; ++sendProc) {
    vector<char> buffer = localBuffer;
    MPI_Bcast(&buffer.front(), buffer.size(), MPI_CHAR, sendProc, Communicator::communicator());
    vector<char>::const_iterator itr = buffer.begin();
    for (size_t igen = 0; igen != numGenerators; ++igen) {
      double worki;
      unsigned ni;
      unpackElement(worki, itr, buffer.end());
      unpackElement(ni, itr, buffer.end());
      generatorWork[igen] += worki;
      numNodesPerGenerator[igen] += ni;
    }
    CHECK(itr == buffer.end());
  }

  // Compute our final statistics.
  for (size_t igen = 0; igen != numGenerators; ++igen) {
    minWork = min(minWork, generatorWork[igen]);
    maxWork = max(maxWork, generatorWork[igen]);
    minNodes = min(minNodes, numNodesPerGenerator[igen]);
    maxNodes = max(maxNodes, numNodesPerGenerator[igen]);
  }
}

//------------------------------------------------------------------------------
// Cull or unassign nodes from generators with too much work.
//------------------------------------------------------------------------------
template<typename Dimension>
void
VoronoiRedistributeNodes<Dimension>::
cullGeneratorNodesByWork(const vector<typename Dimension::Vector>& generators,
                         const vector<double>& generatorWork,
                         const double targetWork,
                         vector<int>& generatorFlags,
                         vector<DomainNode<Dimension> >& nodes) const {

  const int numProcs = this->numDomains();
  const int procID = this->domainID();

  // Pre-conditions.
  const size_t numGenerators = generators.size();
  REQUIRE(targetWork > 0.0);
  REQUIRE(generatorWork.size() == numGenerators);

  // How many generators are we starting with?
  const int numOpenGenerators = accumulate(generatorFlags.begin(), generatorFlags.end(), 0);

  // Walk the generators.
  for (size_t igen = 0; igen != numGenerators; ++igen) {

    // Do we need to examine this generator?
    if (generatorFlags[igen] == 1 and
        generatorWork[igen] >= targetWork) {
      generatorFlags[igen] = 0;

      // Try to cull out nodes from this generator so we don't overshoot the target work.
      // Find the nodes associated with this generator on this processor.
      // Sort them in increasing distance from the generator.
      typedef pair<size_t, double> PairType;
      vector<PairType> distances;
      for (size_t i = 0; i != nodes.size(); ++i) {
        if (nodes[i].domainID == igen) distances.push_back(make_pair(i, (nodes[i].position - generators[igen]).magnitude2()));
      }
      sort(distances.begin(), distances.end(), ComparePairsBySecondElement<PairType>());

      // Find the global range of distances from the generator.
      double rmin = allReduce((distances.size() > 0 ? distances.front().second : DBL_MAX), MPI_MIN, Communicator::communicator());
      double rmax = allReduce((distances.size() > 0 ? distances.back().second  : 0.0),     MPI_MAX, Communicator::communicator());

      // Bisect for the appropriate radius to reject nodes.
      const double worktol = max(1.0e-10, 0.01*targetWork);
      const double rtol = 1.0e-10*max(1.0, rmax - rmin);
      double rreject = rmax;
      double currentWork = generatorWork[igen];
      while ((abs(currentWork - targetWork) >  worktol) and ((rmax - rmin) > rtol)) {
        rreject = 0.5*(rmin + rmax);
        double localWork = 0.0;
        vector<PairType>::const_iterator itr = distances.begin();
        while (itr != distances.end() and itr->second < rreject) {
          localWork += nodes[itr->first].work;
          ++itr;
        }
        currentWork = allReduce(localWork, MPI_SUM, Communicator::communicator());
        if (currentWork < targetWork) {
          rmin = rreject;
        } else {
          rmax = rreject;
        }
      }

      // Now go through and unassign any nodes from this generator that are outside the
      // rejection threshold.
      typename vector<PairType>::iterator lowerItr = lower_bound(distances.begin(),
                                                                 distances.end(),
                                                                 rreject,
                                                                 ComparePairsBySecondElement<PairType>());
      for (typename vector<PairType>::iterator itr = lowerItr;
           itr != distances.end();
           ++itr) nodes[itr->first].domainID = -1;
    }
  }

  // Did we actually assign any generators this pass?  If not, we're stuck!
  // Flag all generators as done.
  if (accumulate(generatorFlags.begin(), generatorFlags.end(), 0) == numOpenGenerators) {
    generatorFlags = vector<int>(numGenerators, 0);
  }
}

//------------------------------------------------------------------------------
// Find the Voronoi cells adjacent to the specified one.
// If the given cell is one of the vertices, reject it.
//------------------------------------------------------------------------------
template<typename Dimension>
vector<size_t>
VoronoiRedistributeNodes<Dimension>::
findNeighborGenerators(const size_t igen,
                       const vector<Vector>& generators) const {

  // Are there enough generators to construct a meaningful volume?
  vector<size_t> result;
  if (generators.size() < (Dimension::nDim + 1)) {
    for (size_t i = 0; i != generators.size(); ++i) {
      if (i != igen) result.push_back(i);
    }
    return result;
  }

  // Construct the convex hull of the inverse distance to the other generators.
  vector<Vector> inverseDistance;
  for (size_t jgen = 0; jgen != generators.size(); ++jgen) {
    if (jgen != igen) {
      const Vector delta = generators[jgen] - generators[igen];
      const Scalar rmag2 = delta.magnitude2();
      CHECK(rmag2 > 0.0);
      inverseDistance.push_back(delta * safeInv(rmag2, 1.0e-4));
    } else {
      inverseDistance.push_back(Vector::zero);
    }
  }
  typedef typename Dimension::ConvexHull ConvexHull;
  ConvexHull invHull(inverseDistance);
  const vector<Vector> hullVertices = invHull.vertices();

  // Select the generators that correspond to the vertices of the inverse hull.
  for (size_t jgen = 0; jgen != generators.size(); ++jgen) {
    if (jgen != igen) {
      for (typename vector<Vector>::const_iterator itr = hullVertices.begin();
           itr != hullVertices.end();
           ++itr) {
        if (fuzzyEqual((inverseDistance[jgen] - *itr).magnitude2(), 0.0, 1.0e-10)) result.push_back(jgen);
      }
    }
  }

  return result;
}

//------------------------------------------------------------------------------
// Flag for whether we should compute the work per node or strictly balance by
// node count.
//------------------------------------------------------------------------------
template<typename Dimension>
bool
VoronoiRedistributeNodes<Dimension>::
workBalance() const {
  return mWorkBalance;
}

template<typename Dimension>
void
VoronoiRedistributeNodes<Dimension>::
workBalance(bool val) {
  mWorkBalance = val;
}

//------------------------------------------------------------------------------
// Flag to determine if we try to work balance the generators as part of the
// iteration.
//------------------------------------------------------------------------------
template<typename Dimension>
bool
VoronoiRedistributeNodes<Dimension>::
balanceGenerators() const {
  return mBalanceGenerators;
}

template<typename Dimension>
void
VoronoiRedistributeNodes<Dimension>::
balanceGenerators(bool val) {
  mBalanceGenerators = val;
}

//------------------------------------------------------------------------------
// Tolerance used to determine convergence of the generators.
//------------------------------------------------------------------------------
template<typename Dimension>
double
VoronoiRedistributeNodes<Dimension>::
tolerance() const {
  return mTolerance;
}

template<typename Dimension>
void
VoronoiRedistributeNodes<Dimension>::
tolerance(double val) {
  mTolerance = val;
}

//------------------------------------------------------------------------------
// Maximum allowed iterations to try and converge the generator positions.
//------------------------------------------------------------------------------
template<typename Dimension>
unsigned
VoronoiRedistributeNodes<Dimension>::
maxIterations() const {
  return mMaxIterations;
}

template<typename Dimension>
void
VoronoiRedistributeNodes<Dimension>::
maxIterations(unsigned val) {
  mMaxIterations = val;
}

}


//=======================================================================
// Copyright 2002 Indiana University.
// Authors: Andrew Lumsdaine, Lie-Quan Lee, Jeremy G. Siek
//
// This file is part of the Boost Graph Library
//
// You should have received a copy of the License Agreement for the
// Boost Graph Library along with the software; see the file LICENSE.
//
// Permission to modify the code and to distribute modified code is
// granted, provided the text of this NOTICE is retained, a notice that
// the code was modified is included with the above COPYRIGHT NOTICE and
// with the COPYRIGHT NOTICE in the LICENSE file, and that the LICENSE
// file is distributed with the modified code.
//
// LICENSOR MAKES NO REPRESENTATIONS OR WARRANTIES, EXPRESS OR IMPLIED.
// By way of example, but not limitation, Licensor MAKES NO
// REPRESENTATIONS OR WARRANTIES OF MERCHANTABILITY OR FITNESS FOR ANY
// PARTICULAR PURPOSE OR THAT THE USE OF THE LICENSED SOFTWARE COMPONENTS
// OR DOCUMENTATION WILL NOT INFRINGE ANY PATENTS, COPYRIGHTS, TRADEMARKS
// OR OTHER RIGHTS.
//=======================================================================

#ifndef BOOST_GRAPH_DAG_SHORTEST_PATHS_HPP
#define BOOST_GRAPH_DAG_SHORTEST_PATHS_HPP

#include <boost/graph/topological_sort.hpp>
#include <boost/graph/dijkstra_shortest_paths.hpp>

// single-source shortest paths for a Directed Acyclic Graph (DAG)

namespace boost {

  // Initalize distances and call depth first search
  template <class VertexListGraph, class DijkstraVisitor,
            class DistanceMap, class WeightMap, class ColorMap,
            class PredecessorMap,
            class Compare, class Combine,
            class DistInf, class DistZero>
  inline void
  dag_shortest_paths
    (const VertexListGraph& g,
     typename graph_traits<VertexListGraph>::vertex_descriptor s,
     DistanceMap distance, WeightMap weight, ColorMap color,
     PredecessorMap pred,
     DijkstraVisitor vis, Compare compare, Combine combine,
     DistInf inf, DistZero zero)
  {
    typedef typename graph_traits<VertexListGraph>::vertex_descriptor Vertex;
    std::vector<Vertex> rev_topo_order;
    rev_topo_order.reserve(num_vertices(g));
    topological_sort(g, std::back_inserter(rev_topo_order));

    typename graph_traits<VertexListGraph>::vertex_iterator ui, ui_end;
    for (tie(ui, ui_end) = vertices(g); ui != ui_end; ++ui) {
      put(distance, *ui, inf);
      put(pred, *ui, *ui);
    }

    put(distance, s, zero);
    vis.discover_vertex(s, g);
    typename std::vector<Vertex>::reverse_iterator i;
    for (i = rev_topo_order.rbegin(); i != rev_topo_order.rend(); ++i) {
      Vertex u = *i;
      vis.examine_vertex(u, g);
      typename graph_traits<VertexListGraph>::out_edge_iterator e, e_end;
      for (tie(e, e_end) = out_edges(u, g); e != e_end; ++e) {
        vis.discover_vertex(target(*e, g), g);
        bool decreased = relax(*e, g, weight, pred, distance,
                               combine, compare);
        if (decreased)
          vis.edge_relaxed(*e, g);
        else
          vis.edge_not_relaxed(*e, g);
      }
      vis.finish_vertex(u, g);
    }
  }

  namespace detail {

    // Defaults are the same as Dijkstra's algorithm

    // Handle Distance Compare, Combine, Inf and Zero defaults
    template <class VertexListGraph, class DijkstraVisitor,
      class DistanceMap, class WeightMap, class ColorMap,
      class IndexMap, class Params>
    inline void
    dag_sp_dispatch2
      (const VertexListGraph& g,
       typename graph_traits<VertexListGraph>::vertex_descriptor s,
       DistanceMap distance, WeightMap weight, ColorMap color, IndexMap id,
       DijkstraVisitor vis, const Params& params)
    {
      typedef typename property_traits<DistanceMap>::value_type D;
      dummy_property_map p_map;
      dag_shortest_paths
        (g, s, distance, weight, color,
         choose_param(get_param(params, vertex_predecessor), p_map),
         vis,
         choose_param(get_param(params, distance_compare_t()), std::less<D>()),
         choose_param(get_param(params, distance_combine_t()), closed_plus<D>()),
         choose_param(get_param(params, distance_inf_t()),
                      (std::numeric_limits<D>::max)()),
         choose_param(get_param(params, distance_zero_t()),
                      D()));
    }

    // Handle DistanceMap and ColorMap defaults
    template <class VertexListGraph, class DijkstraVisitor,
              class DistanceMap, class WeightMap, class ColorMap,
              class IndexMap, class Params>
    inline void
    dag_sp_dispatch1
      (const VertexListGraph& g,
       typename graph_traits<VertexListGraph>::vertex_descriptor s,
       DistanceMap distance, WeightMap weight, ColorMap color, IndexMap id,
       DijkstraVisitor vis, const Params& params)
    {
      typedef typename property_traits<WeightMap>::value_type T;
      typename std::vector<T>::size_type n;
      n = is_default_param(distance) ? num_vertices(g) : 1;
      std::vector<T> distance_map(n);
      n = is_default_param(color) ? num_vertices(g) : 1;
      std::vector<default_color_type> color_map(n);

      dag_sp_dispatch2
        (g, s,
         choose_param(distance,
                      make_iterator_property_map(distance_map.begin(), id,
                                                 distance_map[0])),
         weight,
         choose_param(color,
                      make_iterator_property_map(color_map.begin(), id,
                                                 color_map[0])),
         id, vis, params);
    }

  } // namespace detail

  template <class VertexListGraph, class Param, class Tag, class Rest>
  inline void
  dag_shortest_paths
    (const VertexListGraph& g,
     typename graph_traits<VertexListGraph>::vertex_descriptor s,
     const bgl_named_params<Param,Tag,Rest>& params)
  {
    // assert that the graph is directed...
    null_visitor null_vis;
    detail::dag_sp_dispatch1
      (g, s,
       get_param(params, vertex_distance),
       choose_const_pmap(get_param(params, edge_weight), g, edge_weight),
       get_param(params, vertex_color),
       choose_const_pmap(get_param(params, vertex_index), g, vertex_index),
       choose_param(get_param(params, graph_visitor),
                    make_dijkstra_visitor(null_vis)),
       params);
  }

} // namespace boost

#endif // BOOST_GRAPH_DAG_SHORTEST_PATHS_HPP


/*
 This program is free software; you can redistribute it and/or modify it under
 the terms of the European Union Public Licence - EUPL v.1.1 as published by
 the European Commission.

 This program is distributed in the hope that it will be useful, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
 FOR A PARTICULAR PURPOSE. See the European Union Public Licence - EUPL v.1.1
 for more details.

 You should have received a copy of the European Union Public Licence - EUPL v.1.1
 along with this program.

 Further information about the European Union Public Licence - EUPL v.1.1 can
 also be found on the world wide web at http://ec.europa.eu/idabc/eupl

*/

/*
 ------ Copyright (C) 2010 STA Steering Board (space.trajectory.analysis AT gmail.com) ----
*/

/*
------------------ Author: Valentino Zuccarelli  -------------------------------------------------
 ------------------ E-mail: (Valentino.Zuccarelli@gmail.com) ----------------------------
 */

#include <Eigen/Core>
#include "Optimization/GlobalOptimizers.h"
#include <QDebug>
using namespace Eigen;

extern int numObj, numIterations, initMODE;
void GlobalOptimizers::InitializeOptimizer()
{
        int i;

        //Random numbers generator initialization:
        // Next line patched by Guillermo to avoid complaints of MingW 4.4
        //int seed=time(0);						//set initial seed for random number generator based on current clock
        int seed=0;
        srand(seed);							//initialize random numbers generator
        //NOTE: seed can be ANY integer number

        //Problem parameters definition:
        NVAR=4;								//define number of optimization variables
        NOBJ=numObj;									//define number of optimization objectives
        qDebug()<<"numObj"<<NOBJ;
        NCONS=1;								//define number of optimization constraints
        NsolPareto=150;							//define number of archive solutions
        //NOTE: the parameter NsolPareto is NOT used within PSO-1D (only one solution is found for mono-objective optimization),
        //for NSGA-II it defines the population size (and the archive coincides with the population) and for DG-MOPSO it defines
        //the archive size, while the swarm size is fixed to 100 (it can be changed within the code in file dgmopso.cpp).
        MAXITER=numIterations;							//define number of iterations to be performed by the algorithm
        InitMode=initMODE;								//define initial set of solutions initialization mode
        // InitMode=0: standard random initialization, to use the first time an optimization is run;
        // InitMode=1: initialize from previous iteration results: optimization variables for N ARCHIVE solutions must have previously
        //				been saved in gsl_matrix *OptimalSolutions (of dimensions NxNVAR: 1 solution for each of the N rows,
        //				1 variable for each of the NVAR variables) and optimization objectives and constraints must have
        //				previously been saved in gsl_matrix *ParetoFront (of dimensions Nx(NOBJ+NCONS): 1 solution for each of
        //				the N rows, with the objectives and then the constraints on the columns).
        //		In case of NSGA-II archive and population coincide, while for PSO-1D and DG-MOPSO it is also necessary to provide
        //		the results in terms of previous swarm positions, velocities and personal bests --> also the matrixes *OldSwarm,
        //		*OldPbest,*OldVel (which have 100 rows for the 100 particles of the swarm and NVAR columns for each variable
        //		value) must be initialized by the user.
        // InitMode=2: initialize from previous run results: only optimization variables of a the optimal solutions found in
        //				a previous run must have been saved in *OptimalSolutions (same structure as above).

        //GSL vectors and matrixes memory allocation
        LOWERBOUND=VectorXd(NVAR); //=gsl_vector_alloc(NVAR);
        UPPERBOUND=VectorXd(NVAR); //=gsl_vector_alloc(NVAR);
        OptimalSolutions=MatrixXd(NsolPareto,NVAR); //=gsl_matrix_alloc(NsolPareto,NVAR);
        ParetoFront=MatrixXd(NsolPareto,NOBJ+NCONS); //=gsl_matrix_alloc(NsolPareto,NOBJ+NCONS);

        //the 1st number in brackets should be the same of swarmsize in pso1D or DG-MOPSO
        OldSwarm=MatrixXd(500,NVAR); //=gsl_matrix_alloc(100,NVAR);		//This matrix must be allocated only if PSO-1D or DG-MOPSO are selected
        OldPbest=MatrixXd(500,NVAR); //=gsl_matrix_alloc(100,NVAR);		//This matrix must be allocated only if PSO-1D or DG-MOPSO are selected
        OldVel=MatrixXd(500,NVAR); //=gsl_matrix_alloc(100,NVAR);			//This matrix must be allocated only if PSO-1D or DG-MOPSO are selected
        OldJpbest=MatrixXd(500,NVAR); //=gsl_matrix_alloc(100,NVAR);		//This matrix must be allocated only if DG-MOPSO is selected
        OldJswarm=MatrixXd(500,NVAR); //=gsl_matrix_alloc(100,NVAR);		//This matrix must be allocated only if DG-MOPSO is selected

        //Variables optimization boundaries definition:
        for(i=0;i<NVAR;i++)
                {
                    LOWERBOUND(i)=0;
                    UPPERBOUND(i)=1;
                }
        LOWERBOUND(0)=-0.001; UPPERBOUND(0)=0.001;//for deviation of Halo orbit

        char *file1="OutputPareto.stam";		//File name for Pareto front objectives and constraints history writing
        char *file2="OutputSolutions.stam";		//File name for Pareto front variables writing at the end of run
        if ((fpParetoFront=fopen(file1,"w"))==NULL) { printf("\nError: could not open file %s for writing.\n",file1); exit(1); }
        if ((fpSolutions=fopen(file2,"w"))==NULL) { printf("\nError: could not open file %s for writing.\n",file2); exit(1); }
}


/*=============================================================================
Copyright 2018 Pranam Lashkari <plashkari628@gmail.com>

Distributed under the Boost Software License, Version 1.0. (See accompanying
file License.txt or copy at https://www.boost.org/LICENSE_1_0.txt)
=============================================================================*/

#ifndef BOOST_ASTRONOMY_IO_IMAGE_HPP
#define BOOST_ASTRONOMY_IO_IMAGE_HPP

#include <cstddef>
#include <algorithm>
#include <iterator>
#include <cstdint>
#include <string>
#include <cmath>
#include <numeric>
#include <valarray>

#include <boost/endian/conversion.hpp>
#include <boost/cstdfloat.hpp>
#include <boost/variant.hpp>

#include <boost/astronomy/io/bitpix.hpp>

namespace boost { namespace astronomy { namespace io {

template <typename PixelType>
struct image_buffer
{
protected:
    std::valarray<PixelType> data_; //! stores the image
    std::size_t width_; //! width of image
    std::size_t height_; //! height of image
    //std::fstream image_file; //! image file

    /**
     * @brief Only used for Type Punning ( Accessing bytes individually )
    */
    union pixel_data
    {
        PixelType pixel;
        std::uint8_t byte[sizeof(PixelType)];
        //char byte[sizeof(PixelType)];
    };

public:
    /**
     * @brief       Constructs an standalone object of image_buffer
    */
    image_buffer() {}

    /**
     * @brief       Constructs an image_buffer object by allocating width*height space for buffer
     * @param[in]   width Width of Image
     * @param[in]   height  Height of Image
    */
    image_buffer(std::size_t width, std::size_t height) : width_(width), height_(height)
    {
        this->data_.resize(width*height);
    }

    /**
     * @brief       virtual destructor allowing image_buffer to be a polymorphic base for derived classes
    */
    virtual ~image_buffer() {}

    /**
     * @brief   Gets the maximum value of all the pixels in the image
     * @return  PixelType containing the maximum pixel value
    */
    PixelType max() const
    {
        return this->data_.max();
    }

    /**
     * @brief   Gets the minimum value of all the pixels in the image
     * @return  PixelType containing the minimum pixel value
    */
    PixelType min() const
    {
        return this->data_.min();
    }

    /**
     * @brief   Gets the mean value of all the pixels in the image
     * @return  mean value of pixels in the image
    */
    double mean() const
    {
        if (this->data_.size() == 0)
        {
            return 0;
        }

        return (std::accumulate(std::begin(this->data_),
            std::end(this->data_), 0.0) / this->data_.size());
    }

    /**
     * @brief   Gets the median of all the pixel values in the image
     * @note    This method uses additional space of order O(n) where n is the number of total pixels
     * @return  median of all the pixel values in the image
    */
    PixelType median() const
    {
        std::valarray<PixelType> sorted_array = this->data_;
        std::nth_element(std::begin(sorted_array),
            std::begin(sorted_array) + sorted_array.size() / 2, std::end(sorted_array));

        return sorted_array[sorted_array.size() / 2];
    }

   /**
     * @brief   Gets the standard deviation of all the pixel values in the image
     * @note    This method uses additional space of order O(n) where n is the number of total pixels
     * @return  standard deviation of all the pixel values in the image
    */
    double std_dev() const
    {
        if (this->data_.size() == 0)
        {
            return 0;
        }

        double avg = this->mean();

        std::valarray<double> diff(this->data_.size());
        for (size_t i = 0; i < diff.size(); i++)
        {
            diff[i] = this->data_[i] - avg;
        }

        diff *= diff;
        return std::sqrt(diff.sum() / (diff.size() - 1));
    }

    /**
     * @brief       Gets the pixel value at specified position
     * @param[in]   x x position of pixel
     * @param[in]   y y position of pixel
    */
    PixelType operator() (std::size_t x, std::size_t y)
    {
        return this->data_[(x*this->width_) + y];
    }

    /**
     * @brief       Returns the size of image
    */
    std::size_t size() { return data_.size(); }
};

/**
 * @brief Visitor used for reading image from data buffer for image variants
*/
struct read_image_visitor :public boost::static_visitor<> {

    const std::string& data_buffer;

    read_image_visitor(const std::string& data_buf) :data_buffer(data_buf) {}

    template<typename Image_Type>
    void operator()(Image_Type& type) { type.read_image(data_buffer); }
};

/**
 * @brief Visitor used for writing image data from the image variants onto a buffer
*/

struct write_image_visitor : public boost::static_visitor<std::string> {

    template<typename Image_Type>
    std::string operator()(Image_Type& type) { return type.write_image(); }

};

/**
 * @brief   Stores image data associated with the perticular HDU
 * @tparam  args Specifies the number of bits that represents a data value in image.
 * @author  Pranam Lashkari, Gopi Krishna Menon
 * @see     image_buffer
*/
template<bitpix bitpix_val,typename Converter>
struct image:public image_buffer<typename bitpix_type<bitpix_val>::underlying_type> {

public:
    /**
     * @brief Reads an image from the data buffer
     * @param[in] data_buffer Data associated with image HDU or primary HDU
    */
    void read_image(const std::string& data_buffer) {

        if (data_buffer.empty()) { return; }

        auto element_size = get_element_size_from_bitpix(bitpix_val);
        std::string::const_iterator raw_data_iter=data_buffer.begin();

        this->data_.resize(data_buffer.size() / element_size);

        for (auto& element : this->data_) {
            std::string data(raw_data_iter, raw_data_iter + element_size);
            element = Converter::template deserialize_to<typename std::remove_reference<decltype(element)>::type>(data,0);
            raw_data_iter += element_size;
        }
    }

    /**
     * @brief Creates a temporary buffer and writes all image data to it
    */
    std::string write_image() {

        if (this->data_.size() != 0) {

            std::string temp_buffer;

            for (auto& element : this->data_) {
                temp_buffer += Converter::template serialize(element);
            }
            return temp_buffer;
        }

        return "";
    }

};
}}} //namespace boost::astronomy::io

#endif // !BOOST_ASTRONOMY_IO_IMAGE_HPP


// This file is part of the dune-gdt project:
//   http://users.dune-project.org/projects/dune-gdt
// Copyright holders: Felix Schindler
// License: BSD 2-Clause License (http://opensource.org/licenses/BSD-2-Clause)

#ifndef DUNE_GDT_LOCALEVALUATION_SIPDG_HH
#define DUNE_GDT_LOCALEVALUATION_SIPDG_HH

#include <tuple>

#include <boost/numeric/conversion/cast.hpp>

#include <dune/common/dynmatrix.hh>

#include <dune/stuff/common/color.hh>
#ifndef NDEBUG
# ifndef DUNE_GDT_LOCALEVALUATION_SIPDG_DISABLE_WARNINGS
#   include <dune/stuff/common/timedlogging.hh>
# endif
#endif
#include <dune/stuff/common/type_utils.hh>
#include <dune/stuff/functions/interfaces.hh>

#include "interface.hh"

namespace Dune {
namespace GDT {
namespace LocalEvaluation {

/**
 *  \brief      Contains local evaluations for the symmetric interior penalty discontinuous Galerkin (SIPDG)
 *              discretization.
 *
 *              For the choice of penalization and the role of the user input see Epshteyn, Riviere (2007):
 *              "Estimation of penalty parameters for symmetric interior penalty Galerkin methods"
 */
namespace SIPDG {

// forwards
template< class LocalizableFunctionImp >
class Inner;

template< class LocalizableFunctionImp >
class BoundaryLHS;

template< class LocalizableDiffusionFunctionImp, class LocalizableDirichletFunctionImp >
class BoundaryRHS;

namespace internal {

template< class LocalizableFunctionImp >
class InnerTraits
{
  static_assert(Stuff::is_localizable_function< LocalizableFunctionImp >::value,
                "LocalizableFunctionImp has to be a localizable function!");
public:
  typedef Inner< LocalizableFunctionImp >                     derived_type;
  typedef LocalizableFunctionImp                              LocalizableFunctionType;
  typedef typename LocalizableFunctionType::EntityType        EntityType;
  typedef typename LocalizableFunctionType::DomainFieldType   DomainFieldType;
  typedef typename LocalizableFunctionType::LocalfunctionType LocalfunctionType;
  typedef std::tuple< std::shared_ptr< LocalfunctionType > >  LocalfunctionTupleType;
  static const size_t                                         dimDomain = LocalizableFunctionType::dimDomain;
}; // class InnerTraits

template< class LocalizableFunctionImp >
class BoundaryLHSTraits
{
  static_assert(Stuff::is_localizable_function< LocalizableFunctionImp >::value,
                "LocalizableFunctionImp has to be a localizable function!");
public:
  typedef LocalizableFunctionImp                              LocalizableFunctionType;
  typedef BoundaryLHS< LocalizableFunctionImp >               derived_type;
  typedef typename LocalizableFunctionType::EntityType        EntityType;
  typedef typename LocalizableFunctionType::DomainFieldType   DomainFieldType;
  typedef typename LocalizableFunctionType::LocalfunctionType LocalfunctionType;
  typedef std::tuple< std::shared_ptr< LocalfunctionType > >  LocalfunctionTupleType;
  static const size_t                                         dimDomain = LocalizableFunctionType::dimDomain;
}; // class BoundaryLHSTraits

template< class LocalizableDiffusionFunctionImp, class LocalizableDirichletFunctionImp >
class BoundaryRHSTraits
{
  static_assert(Stuff::is_localizable_function< LocalizableDiffusionFunctionImp >::value,
                "LocalizableDiffusionFunctionImp has to be a localizable function!");
  static_assert(Stuff::is_localizable_function< LocalizableDirichletFunctionImp >::value,
                "LocalizableDirichletFunctionImp has to be a localizable function!");
  static_assert(std::is_same< typename LocalizableDiffusionFunctionImp::EntityType,
                              typename LocalizableDirichletFunctionImp::EntityType >::value,
                "EntityTypes have to agree!");
  static_assert(std::is_same< typename LocalizableDiffusionFunctionImp::DomainFieldType,
                              typename LocalizableDirichletFunctionImp::DomainFieldType >::value,
                "DomainFieldTypes have to agree!");
  static_assert(LocalizableDiffusionFunctionImp::dimDomain == LocalizableDirichletFunctionImp::dimDomain,
                "Dimensions have to agree");
public:
  typedef BoundaryRHS< LocalizableDiffusionFunctionImp, LocalizableDirichletFunctionImp >   derived_type;
  typedef LocalizableDiffusionFunctionImp                              LocalizableDiffusionFunctionType;
  typedef LocalizableDirichletFunctionImp                              LocalizableDirichletFunctionType;
  typedef typename LocalizableDiffusionFunctionType::LocalfunctionType LocalDiffusionFunctionType;
  typedef typename LocalizableDirichletFunctionType::LocalfunctionType LocalDirichletFunctionType;
  typedef std::tuple< std::shared_ptr< LocalDiffusionFunctionType >,
                      std::shared_ptr< LocalDirichletFunctionType > >  LocalfunctionTupleType;
  typedef typename LocalizableDiffusionFunctionType::EntityType        EntityType;
  typedef typename LocalizableDiffusionFunctionType::DomainFieldType   DomainFieldType;
  static const size_t dimDomain = LocalizableDiffusionFunctionType::dimDomain;
}; // class BoundaryRHSTraits

/**
 * \note see Epshteyn, Riviere, 2007
 */
static inline double default_beta(const size_t dimDomain)
{
  return 1.0/(dimDomain - 1.0);
}

/**
 * \note see Epshteyn, Riviere, 2007
 */
static inline double inner_sigma(const size_t pol_order)
{
  double sigma = 1.0;
  if (pol_order <= 1)
    sigma *= 8.0;
  else if (pol_order <= 2)
    sigma *= 20.0;
  else if (pol_order <= 3)
    sigma *= 38.0;
  else {
#ifndef NDEBUG
# ifndef DUNE_GDT_LOCALEVALUATION_SIPDG_DISABLE_WARNINGS
    DSC::TimedLogger().get("gdt.localevaluation.sipdg.inner").warn()
        << "a polynomial order of " << pol_order << " is untested!\n"
        << "  #define DUNE_GDT_LOCALEVALUATION_SIPDG_DISABLE_WARNINGS to statically disable this warning\n"
        << "  or dynamically disable warnings of the TimedLogger() instance!" << std::endl;
# endif
#endif
    sigma *= 50.0;
  }
  return sigma;
} // ... inner_sigma(...)

/**
 * \note see Epshteyn, Riviere, 2007
 */
static inline double boundary_sigma(const size_t pol_order)
{
  double sigma = 1.0;
  if (pol_order <= 1)
    sigma *= 14.0;
  else if (pol_order <= 2)
    sigma *= 38.0;
  else if (pol_order <= 3)
    sigma *= 74.0;
  else {
#ifndef NDEBUG
# ifndef DUNE_GDT_LOCALEVALUATION_SIPDG_DISABLE_WARNINGS
    DSC::TimedLogger().get("gdt.localevaluation.sipdg.inner").warn()
        << "a polynomial order of " << pol_order << " is untested!\n"
        << "  #define DUNE_GDT_LOCALEVALUATION_SIPDG_DISABLE_WARNINGS to statically disable this warning\n"
        << "  or dynamically disable warnings of the TimedLogger() instance!" << std::endl;
# endif
#endif
    sigma *= 100.0;
  }
  return sigma;
} // ... boundary_sigma(...)

} // namespace internal

/**
 * see Epshteyn, Riviere, 2007 for the meaning of beta
 */
template< class LocalizableFunctionImp >
class Inner
  : public LocalEvaluation::Codim1Interface< internal::InnerTraits< LocalizableFunctionImp >, 4 >
{
public:
  typedef internal::InnerTraits< LocalizableFunctionImp > Traits;
  typedef typename Traits::LocalizableFunctionType        LocalizableFunctionType;
  typedef typename Traits::LocalfunctionTupleType         LocalfunctionTupleType;
  typedef typename Traits::EntityType                     EntityType;
  typedef typename Traits::DomainFieldType                DomainFieldType;
  static const size_t                                     dimDomain = Traits::dimDomain;

  Inner(const LocalizableFunctionType& inducingFunction, const double beta = internal::default_beta(dimDomain))
    : inducingFunction_(inducingFunction)
    , beta_(beta)
  {}

  /// \name Required by LocalEvaluation::Codim1Interface< ..., 4 >
  /// \{

  LocalfunctionTupleType localFunctions(const EntityType& entity) const
  {
    return std::make_tuple(inducingFunction_.local_function(entity));
  }

  /**
   * \brief extracts the local functions and calls the correct order() method
   */
  template< class R, size_t rT, size_t rCT, size_t rA, size_t rCA >
  size_t order(const LocalfunctionTupleType& localFunctionsEntity,
               const LocalfunctionTupleType& localFunctionsNeighbor,
               const Stuff::LocalfunctionSetInterface
                   < EntityType, DomainFieldType, dimDomain, R, rT, rCT >& testBaseEntity,
               const Stuff::LocalfunctionSetInterface
                   < EntityType, DomainFieldType, dimDomain, R, rA, rCA >& ansatzBaseEntity,
               const Stuff::LocalfunctionSetInterface
                   < EntityType, DomainFieldType, dimDomain, R, rT, rCT >& testBaseNeighbor,
               const Stuff::LocalfunctionSetInterface
                   < EntityType, DomainFieldType, dimDomain, R, rA, rCA >& ansatzBaseNeighbor) const
  {
    const auto localFunctionEntity = std::get< 0 >(localFunctionsEntity);
    const auto localFunctionNeighbor = std::get< 0 >(localFunctionsNeighbor);
    return order(*localFunctionEntity, *localFunctionNeighbor,
                 testBaseEntity, ansatzBaseEntity,
                 testBaseNeighbor, ansatzBaseNeighbor);
  }

  /**
   * \brief extracts the local functions and calls the correct evaluate() method
   */
  template< class IntersectionType, class R, size_t rT, size_t rCT, size_t rA, size_t rCA >
  void evaluate(const LocalfunctionTupleType& localFunctionsEntity,
                const LocalfunctionTupleType& localFunctionsNeighbor,
                const Stuff::LocalfunctionSetInterface
                    < EntityType, DomainFieldType, dimDomain, R, rT, rCT >& testBaseEntity,
                const Stuff::LocalfunctionSetInterface
                    < EntityType, DomainFieldType, dimDomain, R, rA, rCA >& ansatzBaseEntity,
                const Stuff::LocalfunctionSetInterface
                    < EntityType, DomainFieldType, dimDomain, R, rT, rCT >& testBaseNeighbor,
                const Stuff::LocalfunctionSetInterface
                    < EntityType, DomainFieldType, dimDomain, R, rA, rCA >& ansatzBaseNeighbor,
                const IntersectionType& intersection,
                const Dune::FieldVector< DomainFieldType, dimDomain - 1 >& localPoint,
                Dune::DynamicMatrix< R >& entityEntityRet,
                Dune::DynamicMatrix< R >& neighborNeighborRet,
                Dune::DynamicMatrix< R >& entityNeighborRet,
                Dune::DynamicMatrix< R >& neighborEntityRet) const
  {
    const auto localFunctionEntity = std::get< 0 >(localFunctionsEntity);
    const auto localFunctionNeighbor = std::get< 0 >(localFunctionsNeighbor);
    evaluate(*localFunctionEntity, *localFunctionNeighbor,
             testBaseEntity, ansatzBaseEntity,
             testBaseNeighbor, ansatzBaseNeighbor,
             intersection, localPoint,
             entityEntityRet,
             neighborNeighborRet,
             entityNeighborRet,
             neighborEntityRet);
  }

  /// \}
  /// \name Actual implementation of order
  /// \{

  template< class R, size_t rL, size_t rCL, size_t rT, size_t rCT, size_t rA, size_t rCA >
  size_t order(const Stuff::LocalfunctionInterface
                   < EntityType, DomainFieldType, dimDomain, R, rL, rCL >& localFunctionEntity,
               const Stuff::LocalfunctionInterface
                   < EntityType, DomainFieldType, dimDomain, R, rL, rCL >& localFunctionNeighbor,
               const Stuff::LocalfunctionSetInterface
                   < EntityType, DomainFieldType, dimDomain, R, rT, rCT >& testBaseEntity,
               const Stuff::LocalfunctionSetInterface
                   < EntityType, DomainFieldType, dimDomain, R, rA, rCA >& ansatzBaseEntity,
               const Stuff::LocalfunctionSetInterface
                   < EntityType, DomainFieldType, dimDomain, R, rT, rCT >& testBaseNeighbor,
               const Stuff::LocalfunctionSetInterface
                   < EntityType, DomainFieldType, dimDomain, R, rA, rCA >& ansatzBaseNeighbor) const
  {
    return std::max(localFunctionEntity.order(), localFunctionNeighbor.order())
         + std::max(testBaseEntity.order(), testBaseNeighbor.order())
         + std::max(ansatzBaseEntity.order(), ansatzBaseNeighbor.order());
  }

  /// \}
  /// \name Actual implementation of evaluate
  /// \{

  /**
   *  \brief  Computes the ipdg fluxes in a primal setting.
   *  \tparam IntersectionType Type of the codim 1 Intersection
   *  \tparam R         RangeFieldType
   */
  template< class IntersectionType, class R >
  void evaluate(const Stuff::LocalfunctionInterface< EntityType, DomainFieldType, 2, R, 1, 1 >& localFunctionEntity,
                const Stuff::LocalfunctionInterface< EntityType, DomainFieldType, 2, R, 1, 1 >& localFunctionNeighbor,
                const Stuff::LocalfunctionSetInterface< EntityType, DomainFieldType, 2, R, 1, 1 >& testBaseEntity,
                const Stuff::LocalfunctionSetInterface< EntityType, DomainFieldType, 2, R, 1, 1 >& ansatzBaseEntity,
                const Stuff::LocalfunctionSetInterface< EntityType, DomainFieldType, 2, R, 1, 1 >& testBaseNeighbor,
                const Stuff::LocalfunctionSetInterface< EntityType, DomainFieldType, 2, R, 1, 1 >& ansatzBaseNeighbor,
                const IntersectionType& intersection,
                const Dune::FieldVector< DomainFieldType, 1 >& localPoint,
                Dune::DynamicMatrix< R >& entityEntityRet,
                Dune::DynamicMatrix< R >& neighborNeighborRet,
                Dune::DynamicMatrix< R >& entityNeighborRet,
                Dune::DynamicMatrix< R >& neighborEntityRet) const
  {
    // clear ret
    entityEntityRet *= 0.0;
    neighborNeighborRet *= 0.0;
    entityNeighborRet *= 0.0;
    neighborEntityRet *= 0.0;
    // convert local point (which is in intersection coordinates) to entity/neighbor coordinates
    const auto localPointEn = intersection.geometryInInside().global(localPoint);
    const auto localPointNe = intersection.geometryInOutside().global(localPoint);
    const auto unitOuterNormal = intersection.unitOuterNormal(localPoint);
    // evaluate local function
    const auto functionValueEn = localFunctionEntity.evaluate(localPointEn);
    const auto functionValueNe = localFunctionNeighbor.evaluate(localPointNe);
    // compute penalty (see Epshteyn, Riviere, 2007)
    const auto max_polorder = std::max(testBaseEntity.order(),
                                       std::max(ansatzBaseEntity.order(),
                                                std::max(testBaseNeighbor.order(),
                                                         ansatzBaseNeighbor.order())));
    const R sigma = internal::inner_sigma(max_polorder);
    const R penalty = sigma / std::pow(intersection.geometry().volume(), beta_);
    // evaluate bases
    // * entity
    //   * test
    const auto rowsEn = testBaseEntity.size();
    const auto testValuesEn = testBaseEntity.evaluate(localPointEn);
    const auto testGradientsEn = testBaseEntity.jacobian(localPointEn);
    //   * ansatz
    const auto colsEn = ansatzBaseEntity.size();
    const auto ansatzValuesEn = ansatzBaseEntity.evaluate(localPointEn);
    const auto ansatzGradientsEn = ansatzBaseEntity.jacobian(localPointEn);
    // * neighbor
    //   * test
    const auto rowsNe = testBaseNeighbor.size();
    const auto testValuesNe = testBaseNeighbor.evaluate(localPointNe);
    const auto testGradientsNe = testBaseNeighbor.jacobian(localPointNe);
    //   * ansatz
    const auto colsNe = ansatzBaseNeighbor.size();
    const auto ansatzValuesNe = ansatzBaseNeighbor.evaluate(localPointNe);
    const auto ansatzGradientsNe = ansatzBaseNeighbor.jacobian(localPointNe);
    // compute the evaluations
    assert(entityEntityRet.rows() >= rowsEn);
    assert(entityEntityRet.cols() >= colsEn);
    assert(entityNeighborRet.rows() >= rowsEn);
    assert(entityNeighborRet.cols() >= colsNe);
    assert(neighborEntityRet.rows() >= rowsNe);
    assert(neighborEntityRet.cols() >= colsEn);
    assert(neighborNeighborRet.rows() >= rowsNe);
    assert(neighborNeighborRet.cols() >= colsNe);
    // loop over all entity test basis functions
    for (size_t ii = 0; ii < rowsEn; ++ii) {
      auto& entityEntityRetRow = entityEntityRet[ii];
      auto& entityNeighborRetRow = entityNeighborRet[ii];
      // loop over all entity ansatz basis functions
      for (size_t jj = 0; jj < colsEn; ++jj) {
        // consistency term
        entityEntityRetRow[jj]
            += -0.5 * functionValueEn * (ansatzGradientsEn[jj][0] * unitOuterNormal) * testValuesEn[ii];
        // symmetry term
        entityEntityRetRow[jj]
            += -0.5 * ansatzValuesEn[jj] * functionValueEn * (testGradientsEn[ii][0] * unitOuterNormal);
        // penalty term
        entityEntityRetRow[jj] += penalty * ansatzValuesEn[jj] * testValuesEn[ii];
      } // loop over all entity ansatz basis functions
      // loop over all neighbor ansatz basis functions
      for (size_t jj = 0; jj < colsNe; ++jj) {
        // consistency term
        entityNeighborRetRow[jj]
            += -0.5 * functionValueNe * (ansatzGradientsNe[jj][0] * unitOuterNormal) * testValuesEn[ii];
        // symmetry term
        entityNeighborRetRow[jj]
            += 0.5 * ansatzValuesNe[jj] * functionValueEn * (testGradientsEn[ii][0] * unitOuterNormal);
        // penalty term
        entityNeighborRetRow[jj] += -1.0 * penalty * ansatzValuesNe[jj] * testValuesEn[ii];
      } // loop over all neighbor ansatz basis functions
    } // loop over all entity test basis functions
    // loop over all neighbor test basis functions
    for (size_t ii = 0; ii < rowsNe; ++ii) {
      auto& neighborEntityRetRow = neighborEntityRet[ii];
      auto& neighborNeighborRetRow = neighborNeighborRet[ii];
      // loop over all entity ansatz basis functions
      for (size_t jj = 0; jj < colsEn; ++jj) {
        // consistency term
        neighborEntityRetRow[jj]
            += 0.5 * functionValueEn * (ansatzGradientsEn[jj][0] * unitOuterNormal) * testValuesNe[ii];
        // symmetry term
        neighborEntityRetRow[jj]
            += -0.5 * ansatzValuesEn[jj] * functionValueNe * (testGradientsNe[ii][0] * unitOuterNormal);
        // penalty term
        neighborEntityRetRow[jj] += -1.0 * penalty * ansatzValuesEn[jj] * testValuesNe[ii];
      } // loop over all entity ansatz basis functions
      // loop over all neighbor ansatz basis functions
      for (size_t jj = 0; jj < colsNe; ++jj) {
        // consistency term
        neighborNeighborRetRow[jj]
            += 0.5 * functionValueNe * (ansatzGradientsNe[jj][0] * unitOuterNormal) * testValuesNe[ii];
        // symmetry term
        neighborNeighborRetRow[jj]
            += 0.5 * ansatzValuesNe[jj] * functionValueNe * (testGradientsNe[ii][0] * unitOuterNormal);
        // penalty term
        neighborNeighborRetRow[jj] += penalty * ansatzValuesNe[jj] * testValuesNe[ii];
      } // loop over all neighbor ansatz basis functions
    } // loop over all neighbor test basis functions
  } // ... evaluate(...)

  /// \}

private:
  const LocalizableFunctionType& inducingFunction_;
  const double beta_;
}; // class Inner

/**
 * see Epshteyn, Riviere, 2007 for the meaning of beta
 */
template< class LocalizableFunctionImp >
class BoundaryLHS
    : public LocalEvaluation::Codim1Interface< internal::BoundaryLHSTraits< LocalizableFunctionImp >, 2 >
{
public:
  typedef internal::BoundaryLHSTraits< LocalizableFunctionImp > Traits;
  typedef typename Traits::LocalizableFunctionType              LocalizableFunctionType;
  typedef typename Traits::LocalfunctionTupleType               LocalfunctionTupleType;
  typedef typename Traits::EntityType                           EntityType;
  typedef typename Traits::DomainFieldType                      DomainFieldType;
  static const size_t                                           dimDomain = Traits::dimDomain;

  BoundaryLHS(const LocalizableFunctionType& inducingFunction, const double beta = internal::default_beta(dimDomain))
    : inducingFunction_(inducingFunction)
    , beta_(beta)
  {}

  /// \name Required by LocalEvaluation::Codim1Interface< ..., 2>
  /// \{

  LocalfunctionTupleType localFunctions(const EntityType& entity) const
  {
    return std::make_tuple(inducingFunction_.local_function(entity));
  }

  /**
   * \brief extracts the local functions and calls the correct order() method
   */
  template< class R, size_t rT, size_t rCT, size_t rA, size_t rCA >
  size_t order(const LocalfunctionTupleType localFuncs,
               const Stuff::LocalfunctionSetInterface< EntityType, DomainFieldType, dimDomain, R, rT, rCT >& testBase,
               const Stuff::LocalfunctionSetInterface< EntityType, DomainFieldType, dimDomain, R, rA, rCA >& ansatzBase)
  const
  {
    return order(*std::get< 0 >(localFuncs), testBase, ansatzBase);
  }

  /**
   * \brief extracts the local functions and calls the correct evaluate() method
   */
  template< class IntersectionType, class R, size_t rT, size_t rCT, size_t rA, size_t rCA >
  void evaluate(const LocalfunctionTupleType localFuncs,
                const Stuff::LocalfunctionSetInterface
                    < EntityType, DomainFieldType, dimDomain, R, rT, rCT >& testBase,
                const Stuff::LocalfunctionSetInterface
                    < EntityType, DomainFieldType, dimDomain, R, rA, rCA >& ansatzBase,
                const IntersectionType& intersection,
                const Dune::FieldVector< DomainFieldType, dimDomain - 1 >& localPoint,
                Dune::DynamicMatrix< R >& ret) const
  {
    evaluate(*std::get< 0 >(localFuncs), testBase, ansatzBase, intersection, localPoint, ret);
  }

  /// \}
  /// \name Actual implementation of order
  /// \{

  /**
   * \return localFunction.order() + testBase.order() + ansatzBase.order();
   */
  template< class R, size_t rL, size_t rCL, size_t rT, size_t rCT, size_t rA, size_t rCA >
  size_t order(const Stuff::LocalfunctionInterface< EntityType, DomainFieldType, dimDomain, R, rL, rCL >& localFunction,
               const Stuff::LocalfunctionSetInterface< EntityType, DomainFieldType, dimDomain, R, rT, rCT >& testBase,
               const Stuff::LocalfunctionSetInterface< EntityType, DomainFieldType, dimDomain, R, rA, rCA >& ansatzBase)
  const
  {
    return localFunction.order() + testBase.order() + ansatzBase.order();
  }

  /// \}
  /// \name Actual implementation of evaluate
  /// \{

  template< class IntersectionType, class R >
  void evaluate(const Stuff::LocalfunctionInterface< EntityType, DomainFieldType, 2, R, 1, 1 >& localFunction,
                const Stuff::LocalfunctionSetInterface< EntityType, DomainFieldType, 2, R, 1, 1 >& testBase,
                const Stuff::LocalfunctionSetInterface< EntityType, DomainFieldType, 2, R, 1, 1 >& ansatzBase,
                const IntersectionType& intersection,
                const Dune::FieldVector< DomainFieldType, 1 >& localPoint,
                Dune::DynamicMatrix< R >& ret) const
  {
    // clear ret
    ret *= 0.0;
    // get local point (which is in intersection coordinates) in entity coordinates
    const auto localPointEntity = intersection.geometryInInside().global(localPoint);
    const auto unitOuterNormal = intersection.unitOuterNormal(localPoint);
    // evaluate local function
    const auto functionValue = localFunction.evaluate(localPointEntity);
    // compute penalty (see Epshteyn, Riviere, 2007)
    const auto max_polorder = std::max(testBase.order(), ansatzBase.order());
    const R sigma = internal::boundary_sigma(max_polorder);
    const R penalty = sigma / std::pow(intersection.geometry().volume(), beta_);
    // evaluate bases
    // * test
    const auto rows = testBase.size();
    const auto testValues = testBase.evaluate(localPointEntity);
    const auto testGradients = testBase.jacobian(localPointEntity);
    // * ansatz
    const auto cols = ansatzBase.size();
    const auto ansatzValues = ansatzBase.evaluate(localPointEntity);
    const auto ansatzGradients = ansatzBase.jacobian(localPointEntity);
    // compute products
    assert(ret.rows() >= rows);
    assert(ret.cols() >= cols);
    // loop over all test basis functions
    for (size_t ii = 0; ii < rows; ++ii) {
      auto& retRow = ret[ii];
      // loop over all ansatz basis functions
      for (size_t jj = 0; jj < cols; ++jj) {
        // consistency term
        retRow[jj] += -1.0 * functionValue * (ansatzGradients[jj][0] * unitOuterNormal) * testValues[ii];
        // symmetry term
        retRow[jj] += -1.0 * ansatzValues[jj] * functionValue * (testGradients[ii][0] * unitOuterNormal);
        // penalty term
        retRow[jj] += penalty * ansatzValues[jj] * testValues[ii];
      } // loop over all ansatz basis functions
    } // loop over all test basis functions
  } // ... evaluate(...)

  /// \}
private:
  const LocalizableFunctionType& inducingFunction_;
  const double beta_;
}; // class BoundaryLHS

/**
 * see Epshteyn, Riviere, 2007 for the meaning of beta
 */
template< class LocalizableDiffusionFunctionImp, class LocalizableDirichletFunctionImp >
class BoundaryRHS
  : public LocalEvaluation::Codim1Interface< internal::BoundaryRHSTraits< LocalizableDiffusionFunctionImp,
                                                                LocalizableDirichletFunctionImp >, 1 >
{
public:
  typedef internal::BoundaryRHSTraits< LocalizableDiffusionFunctionImp, LocalizableDirichletFunctionImp > Traits;
  typedef typename Traits::LocalizableDiffusionFunctionType LocalizableDiffusionFunctionType;
  typedef typename Traits::LocalizableDirichletFunctionType LocalizableDirichletFunctionType;
  typedef typename Traits::LocalfunctionTupleType           LocalfunctionTupleType;
  typedef typename Traits::EntityType                       EntityType;
  typedef typename Traits::DomainFieldType                  DomainFieldType;
  static const size_t                                       dimDomain = Traits::dimDomain;

  BoundaryRHS(const LocalizableDiffusionFunctionType& diffusion,
              const LocalizableDirichletFunctionType& dirichlet,
              const double beta = internal::default_beta(dimDomain))
    : diffusion_(diffusion)
    , dirichlet_(dirichlet)
    , beta_(beta)
  {}

  /// \name Required by LocalEvaluation::Codim1Interface< ...., 1 >
  /// \{

  LocalfunctionTupleType localFunctions(const EntityType& entity) const
  {
    return std::make_tuple(diffusion_.local_function(entity), dirichlet_.local_function(entity));
  }

  /**
   * \brief extracts the local functions and calls the correct order() method
   */
  template< class R, size_t r, size_t rC >
  size_t order(const LocalfunctionTupleType localFuncs,
               const Stuff::LocalfunctionSetInterface< EntityType, DomainFieldType, dimDomain, R, r, rC >& testBase)
  const
  {
    const auto localDiffusion = std::get< 0 >(localFuncs);
    const auto localDirichlet = std::get< 1 >(localFuncs);
    return order(*localDiffusion, *localDirichlet, testBase);
  }

  /**
   * \brief extracts the local functions and calls the correct evaluate() method
   */
  template< class IntersectionType, class R, size_t r, size_t rC >
  void evaluate(const LocalfunctionTupleType localFuncs,
                const Stuff::LocalfunctionSetInterface< EntityType, DomainFieldType, dimDomain, R, r, rC >& testBase,
                const IntersectionType& intersection,
                const Dune::FieldVector< DomainFieldType, dimDomain - 1 >& localPoint,
                Dune::DynamicVector< R >& ret) const
  {
    const auto localDiffusion = std::get< 0 >(localFuncs);
    const auto localDirichlet = std::get< 1 >(localFuncs);
    evaluate(*localDiffusion, *localDirichlet, testBase, intersection, localPoint, ret);
  }

  /// \}
  /// \name Actual implementation of order
  /// \{

  /**
   *  \return std::max(testOrder + dirichletOrder, diffusionOrder + testGradientOrder + dirichletOrder);
   */
  template< class R, size_t rLF, size_t rCLF, size_t rLR, size_t rCLR, size_t rT, size_t rCT >
  size_t order(const Stuff::LocalfunctionInterface
                            < EntityType, DomainFieldType, dimDomain, R, rLF, rCLF >& localDiffusion,
                        const Stuff::LocalfunctionInterface
                            < EntityType, DomainFieldType, dimDomain, R, rLR, rCLR >& localDirichlet,
                        const Stuff::LocalfunctionSetInterface
                            < EntityType, DomainFieldType, dimDomain, R, rT, rCT >& testBase) const
  {
    const size_t testOrder = testBase.order();
    const size_t testGradientOrder = boost::numeric_cast< size_t >(std::max(ssize_t(testOrder) - 1, ssize_t(0)));
    const size_t diffusionOrder = localDiffusion.order();
    const size_t dirichletOrder = localDirichlet.order();
    return std::max(testOrder + dirichletOrder, diffusionOrder + testGradientOrder + dirichletOrder);
  } // ... order(...)

  /// \}
  /// \name Actual implementation of evaluate
  /// \{

  template< class IntersectionType, class R >
  void evaluate(const Stuff::LocalfunctionInterface< EntityType, DomainFieldType, dimDomain, R, 1, 1 >& localDiffusion,
                const Stuff::LocalfunctionInterface< EntityType, DomainFieldType, dimDomain, R, 1, 1 >& localDirichlet,
                const Stuff::LocalfunctionSetInterface< EntityType, DomainFieldType, dimDomain, R, 1, 1 >& testBase,
                const IntersectionType& intersection,
                const Dune::FieldVector< DomainFieldType, dimDomain - 1 >& localPoint,
                Dune::DynamicVector< R >& ret) const
  {
    // clear ret
    ret *= 0.0;
    // get local point (which is in intersection coordinates) in entity coordinates
    const auto localPointEntity = intersection.geometryInInside().global(localPoint);
    const auto unitOuterNormal = intersection.unitOuterNormal(localPoint);
    // evaluate local functions
    const auto diffusionValue = localDiffusion.evaluate(localPointEntity);
    const auto dirichletValue = localDirichlet.evaluate(localPointEntity);
    // compute penalty (see Epshteyn, Riviere, 2007)
    const auto polorder = testBase.order();
    const R sigma = internal::boundary_sigma(polorder);
    const R penalty = sigma / std::pow(intersection.geometry().volume(), beta_);
    // evaluate basis
    const auto size = testBase.size();
    const auto testValues = testBase.evaluate(localPointEntity);
    const auto testGradients = testBase.jacobian(localPointEntity);
    // compute
    assert(ret.size() >= size);
    // loop over all test basis functions
    for (size_t ii = 0; ii < size; ++ii) {
      // symmetry term
      ret[ii] += -1.0 * dirichletValue * diffusionValue * (testGradients[ii][0] * unitOuterNormal);
      // penalty term
      ret[ii] += penalty * dirichletValue * testValues[ii];
    } // loop over all test basis functions
  } // ... evaluate(...)

  /// \{

private:
  const LocalizableDiffusionFunctionType& diffusion_;
  const LocalizableDirichletFunctionType& dirichlet_;
  const double beta_;
}; // class BoundaryRHS

} // namespace SIPDG
} // namespace LocalEvaluation
} // namespace GDT
} // namespace Dune

#endif // DUNE_GDT_LOCALEVALUATION_SIPDG_HH