#define PY_ARRAY_UNIQUE_SYMBOL PyArrayHandleCore

#ifndef OPENGM_PYTHON_INTERFACE
#define OPENGM_PYTHON_INTERFACE 1
#endif
#include <boost/python.hpp>
#include <boost/python/suite/indexing/vector_indexing_suite.hpp>
#include <numpy/noprefix.h>
#ifdef Bool
#undef Bool
#endif

#include <stdexcept>
#include <string>
#include <sstream>
#include <ostream>
#include <stddef.h>
#include <boost/python.hpp>
#include <boost/python/suite/indexing/vector_indexing_suite.hpp>
#include <opengm/graphicalmodel/graphicalmodel.hxx>
#include <opengm/inference/inference.hxx>
#include "opengm_helpers.hxx"
#include "copyhelper.hxx"
#include "nifty_iterator.hxx"
#include "export_typedes.hxx"
#include "../converter.hxx"
#include "numpyview.hxx"

using namespace boost::python;

namespace pygm {

      //constructor from numpy array
      template<class GM,class VALUE_TYPE>
      GM *  gmConstructorPythonNumpy( NumpyView<VALUE_TYPE,1>  numberOfLabels) {
         return new GM(typename GM::SpaceType(numberOfLabels.begin1d(), numberOfLabels.end1d()));
      }
      template<class GM,class VALUE_TYPE>
      GM *  gmConstructorPythonList(const boost::python::list & numberOfLabelsList) {
         typedef PythonIntListAccessor<VALUE_TYPE,true> Accessor;
         typedef opengm::AccessorIterator<Accessor,true> Iterator;
         Accessor accessor(numberOfLabelsList);
         Iterator begin(accessor,0);
         Iterator end(accessor,accessor.size());
         return new GM(typename GM::SpaceType(begin, end));
      }

      template<class GM,class VALUE_TYPE>
      void assignPythonList( GM & gm ,const boost::python::list & numberOfLabelsList) {
         typedef PythonIntListAccessor<VALUE_TYPE,true> Accessor;
         typedef opengm::AccessorIterator<Accessor,true> Iterator;
         Accessor accessor(numberOfLabelsList);
         Iterator begin(accessor,0);
         Iterator end(accessor,accessor.size());
         typename GM::SpaceType space(begin, end);
         gm.assign(space);
      }

      template<class GM,class VALUE_TYPE>
      void assignPythonNumpy( GM & gm ,NumpyView<VALUE_TYPE,1>  numberOfLabels) {
         typename GM::SpaceType space(numberOfLabels.begin1d(), numberOfLabels.end1d());
         gm.assign(space);
      }
      template<class GM>
      typename GM::IndexType addFactorPyNumpy
      (
         GM & gm,const typename GM::FunctionIdentifier & fid, NumpyView<typename  GM::IndexType,1>   vis
      ) {
         return gm.addFactor(fid, vis.begin1d(), vis.end1d());
      }

      template<class GM,class VALUE_TYPE>
      typename GM::IndexType addFactorPyList
      (
         GM & gm,const typename GM::FunctionIdentifier & fid, const boost::python::list &  vis
      ) {
         typedef PythonIntListAccessor<VALUE_TYPE,true> Accessor;
         typedef opengm::AccessorIterator<Accessor,true> Iterator;
         Accessor accessor(vis);
         Iterator begin(accessor,0);
         Iterator end(accessor,accessor.size());
         return gm.addFactor(fid, begin, end);
      }
      template<class GM,class VALUE_TYPE>
      typename GM::IndexType addFactorPyTuple
      (
         GM & gm,const typename GM::FunctionIdentifier & fid, const boost::python::tuple & vis
      ) {
         typedef PythonIntTupleAccessor<VALUE_TYPE,true> Accessor;
         typedef opengm::AccessorIterator<Accessor,true> Iterator;
         Accessor accessor(vis);
         Iterator begin(accessor,0);
         Iterator end(accessor,accessor.size());
         return gm.addFactor(fid, begin, end);
      }

      template<class GM>
      typename GM::IndexType numVarGm(const GM & gm) {
         return gm.numberOfVariables();
      }

      template<class GM>
      typename GM::IndexType numVarFactor(const GM & gm,const typename GM::IndexType factorIndex) {
         return gm.numberOfVariables(factorIndex);
      }

      template<class GM>
      typename GM::IndexType numFactorGm(const GM & gm) {
         return gm.numberOfFactors();
      }

      template<class GM>
      typename GM::IndexType numFactorVar(const GM & gm,const typename GM::IndexType variableIndex) {
         return gm.numberOfFactors(variableIndex);
      }

      template<class NUMPY_OBJECT,class VECTOR>
      size_t extractShape(const NUMPY_OBJECT & a,VECTOR & myshape){
         const boost::python::tuple &shape = boost::python::extract<boost::python::tuple > (a.attr("shape"));
         size_t dimension = boost::python::len(shape);
         myshape.resize(dimension);
         for (size_t d = 0; d < dimension; ++d)
            myshape[d] = boost::python::extract<typename VECTOR::value_type>(shape[d]);
         return dimension;
      }
      template<class NUMPY_OBJECT,class VECTOR>
      size_t extractStrides(const NUMPY_OBJECT & a,VECTOR & mystrides,const size_t dimension){
         intp* strides_ptr = PyArray_STRIDES(a.ptr());
         intp the_rank = dimension;//rank(a);
         for (intp i = 0; i < the_rank; i++) {
            mystrides.push_back(*(strides_ptr + i));
         }
         return dimension;
      }

      template<class NUMPY_OBJECT>
      void  * extractPtr(const NUMPY_OBJECT & a){
          return PyArray_DATA(a.ptr());
      }

      template<class GM>
      typename GM::FunctionIdentifier addFunctionNpPy( GM & gm,boost::python::numeric::array a) {
         //std::cout<<"add function c++\n";
         typedef opengm::ExplicitFunction<typename GM::ValueType, typename GM::IndexType, typename GM::LabelType> ExplicitFunction;
         ExplicitFunction fEmpty;
         std::pair< typename GM::FunctionIdentifier ,ExplicitFunction & > fidnRef=gm.addFunctionWithRefReturn(fEmpty);
         //std::cout<<"added  function c++\n";
         ExplicitFunction & f=fidnRef.second;
         //std::cout<<"get vie to numpy  function c++\n";
         NumpyView<typename GM::ValueType> numpyView(a);
         //std::cout<<"resize \n";

         //for(size_t i=0;i<numpyView.dimension();++i)
         //   std::cout<<numpyView.shapeBegin()[i]<<" ";
         //std::cout<<"\n";

         f.resize(numpyView.shapeBegin(), numpyView.shapeEnd());
         //std::cout<<"fill\n";
         if(numpyView.dimension()==1){
            size_t ind[1];
            size_t i = 0;
            for (ind[0] = 0; ind[0] < f.shape(0); ++ind[0]) {
               f(i) = numpyView(ind[0]);
               ++i;
            }
         }
         else if(numpyView.dimension()==2){
            size_t ind[2];
            size_t i = 0;
            for (ind[1] = 0; ind[1] < f.shape(1); ++ind[1]){
               for (ind[0] = 0; ind[0] < f.shape(0); ++ind[0]) {
                  f(i) = numpyView(ind[0],ind[1]);
                  ++i;
               }
            }
         }

         else{
            opengm::ShapeWalker<typename ExplicitFunction::FunctionShapeIteratorType> walker(f.functionShapeBegin(),f.dimension());
            for (size_t i=0;i<f.size();++i) {
               typename GM::ValueType v=numpyView[walker.coordinateTuple().begin()];
               f(i) = v;
               ++walker;
            }
         }
         return fidnRef.first;

      }

      template<class GM>
      const typename GM::FactorType & getFactorPy(const GM & gm,const typename GM::IndexType factorIndex) {
         return gm.operator[](factorIndex);
      }

      template<class GM>
      const typename GM::FactorType  & getFactorStaticPy(const GM & gm, const int factorIndex) {
         return gm.operator[](factorIndex);
      }

      template<class GM>
      std::string printGmPy(const GM & gm) {
         std::stringstream ostr;
         ostr<<"-number of variables :"<<gm.numberOfVariables()<<"\n";
         for(size_t i=0;i<GM::NrOfFunctionTypes;++i){
            ostr<<"-number of function(type-"<<i<<")"<<gm.numberOfFunctions(i)<<"\n";
         }
         ostr<<"-number of factors :"<<gm.numberOfFactors()<<"\n";
         ostr<<"-max. factor order :"<<gm.factorOrder();
         return ostr.str();
      }
      template<class GM>
      std::string operatorAsString(const GM & gm) {
         if(opengm::meta::Compare<typename GM::OperatorType,opengm::Adder>::value)
            return "adder";
         else
            return "multiplier";
      }

      template<class GM>
      typename GM::ValueType evaluatePyNumpy
      (
         const GM & gm,
         NumpyView<typename GM::IndexType,1> states
      ){
         return gm.evaluate(states.begin1d());
      }

      template<class GM,class INDEX_TYPE>
      typename GM::ValueType evaluatePyList
      (
         const GM & gm,
         boost::python::list states
      ){
         IteratorHolder< PythonIntListAccessor<INDEX_TYPE,true> > holder(states);
         return gm.evaluate(holder.begin());
      }
   }

template<class GM>
void export_gm() {

   typedef GM PyGm;
   typedef typename PyGm::SpaceType PySpace;
   typedef typename PyGm::ValueType ValueType;
   typedef typename PyGm::IndexType IndexType;
   typedef typename PyGm::LabelType LabelType;
   typedef opengm::ExplicitFunction<ValueType,IndexType,LabelType> PyExplicitFunction;

   typedef typename PyGm::FunctionIdentifier PyFid;
   typedef typename PyGm::FactorType PyFactor;
   typedef typename PyFid::FunctionIndexType FunctionIndexType;
   typedef typename PyFid::FunctionTypeIndexType FunctionTypeIndexType;

   class_<PyGm > ("GraphicalModel", init< >())
      .def("__init__", make_constructor(&pygm::gmConstructorPythonNumpy<PyGm,IndexType> ))
      .def("__init__", make_constructor(&pygm::gmConstructorPythonList<PyGm,int> ))
      .def("assign", &pygm::assignPythonNumpy<PyGm,IndexType>)
      .def("assign", &pygm::assignPythonList<PyGm,int>)
      .def("__str__", &pygm::printGmPy<PyGm>)
      .def("space", &PyGm::space , return_internal_reference<>())
      .add_property("numberOfVariables", &pygm::numVarGm<PyGm>)
      .add_property("numberOfFactors", &pygm::numFactorGm<PyGm>)
      .add_property("operator",&pygm::operatorAsString<PyGm>)
      .def("numberOfVariablesForFactor", &pygm::numVarFactor<PyGm>)
      .def("numberOfLabels", &PyGm::numberOfLabels)
      .def("numberOfFactorsForVariable", &pygm::numFactorVar<PyGm>)
      .def("isAcyclic",&PyGm::isAcyclic)
      //.def("addFunction", &PyGm::addFunction< PyExplicitFunction >)
      .def("addFunctionRaw", &pygm::addFunctionNpPy<PyGm>)
      .def("addFactor", &pygm::addFactorPyNumpy<PyGm>)
      .def("addFactor", &pygm::addFactorPyTuple<PyGm,int>)
      .def("addFactor", &pygm::addFactorPyList<PyGm,int>)
      .def("__getitem__", &pygm::getFactorStaticPy<PyGm>, return_internal_reference<>())
      .def("evaluate",&pygm::evaluatePyNumpy<PyGm>)
      .def("evaluate",&pygm::evaluatePyList<PyGm,int>)
      ;
}

template void export_gm<GmAdder>();
template void export_gm<GmMultiplier>();


/**
 * particle_filter.cpp
 *
 * Created on: Dec 12, 2016
 * Author: Tiffany Huang
 */

#include "particle_filter.h"

#include <algorithm>
#include <chrono>
#include <array>
#include <boost/format.hpp>
#include <boost/log/trivial.hpp>
#include <cmath>
#include <ctime>
#include <eigen3/Eigen/Geometry>
#include <iostream>
#include <iterator>
#include <numeric>
#include <random>
#include <string>
#include <vector>
#ifdef _OPENMP
#include <omp.h>
#endif

#include "helper_functions.h"

using Eigen::Isometry2d;
using Eigen::Matrix2Xd;
using Eigen::Vector2d;
using Eigen::VectorXd;
using std::cout;
using std::endl;
using std::string;
using std::vector;

namespace {

/**
 * @brief      Trivial implementation of nearest neighborhood using euclidean
 * distances
 *
 * @param[in]  obs        The obs
 * @param      landmarks  The landmarks
 *
 * @return     the min distance indices in landmark set
 */
int findNN(const Vector2d &obs, Matrix2Xd &predicted) {
  int minIndex = -1;

  if (predicted.cols() < 1) {
    return minIndex;
  }

  VectorXd distances = (predicted.colwise() - obs).colwise().norm();
  double minDist = distances.minCoeff(&minIndex);
  if (not std::isinf(minDist)) {
    predicted.col(minIndex) = Vector2d(std::numeric_limits<double>::infinity(),
                                       std::numeric_limits<double>::infinity());
    return minIndex;
  } else {
    return -1;
  }
}

double Gaussian(const Vector2d &obs, const Vector2d &landmark,
                const Vector2d &std) {
  using std::exp;
  double dx = obs[0] - landmark[0];
  double dy = obs[1] - landmark[1];
  double gaussianNormalizer = 1.0 / (2 * M_PI * std[0] * std[1]);
  return gaussianNormalizer *
         exp(-(dx * dx / (2 * std[0] * std[0]) + (dy * dy)) /
             (2 * std[1] * std[1]));
}

}  // end of namespace

std::pair<Eigen::Matrix2Xd, std::vector<int>>
Particle::computePredictedObservations(const Map &map, double sensorRange,
                                       const Eigen::Vector2d &sensorStd) {
  std::default_random_engine gen;
  gen.seed(std::time(0));
  std::normal_distribution<double> x_d{0.0, sensorStd[0]};
  std::normal_distribution<double> y_d{0.0, sensorStd[1]};

  vector<int> indices;
  const Vector2d pos = {x, y};

  std::vector<Vector2d, Eigen::aligned_allocator<Vector2d>> landmarks;
  for (size_t i = 0; i < map.landmark_list.size(); ++i) {
    Vector2d landmark = {map.landmark_list[i].x_f + x_d(gen),
                         map.landmark_list[i].y_f + y_d(gen)};
    if ((pos - landmark).norm() < sensorRange) {
      indices.push_back(i);
      landmarks.push_back(landmark);
    }
  }
  return std::make_pair(Eigen::Map<const Matrix2Xd>(
                            reinterpret_cast<const double *>(landmarks.data()),
                            2, landmarks.size()),
                        indices);
}

std::vector<int> Particle::observationAssociation(
    const Matrix2Xd &observationsInWorld,
    const Matrix2Xd &predictedObservationsInWorld) {
  associations.clear();

  // Make a local copy
  vector<int> matches;
  Matrix2Xd predictedObservationsInWorld2 = predictedObservationsInWorld;
  for (int i = 0; i < observationsInWorld.cols(); ++i) {
    int nearestPredicted =
        findNN(observationsInWorld.col(i), predictedObservationsInWorld2);
    if (nearestPredicted != -1) {
      matches.push_back(nearestPredicted);
    }
  }
  return matches;
}

void Particle::updateWeight(const Eigen::Matrix2Xd &observationsInWorld,
                            const Eigen::Matrix2Xd &predictedLandmarks,
                            const std::vector<int> &matchedIndices,
                            const Eigen::Vector2d &sensorStd) {
  // BOOST_LOG_TRIVIAL(debug)
  //     << (boost::format(" --- Updating weight of particle %d --- ") % id).str();

  // Reset weight
  weight = 1.0;

  for (size_t i = 0; i < matchedIndices.size(); ++i) {
    Vector2d observation = observationsInWorld.col(i);
    Vector2d predicted = predictedLandmarks.col(matchedIndices[i]);

    const double likelihood = Gaussian(observation, predicted, sensorStd);

    // BOOST_LOG_TRIVIAL(debug)
    //     << (boost::format("observation=%s, landmark=%s, likelihood=%.3e") %
    //         observation.transpose() % predicted.transpose() % likelihood)
    //            .str();
    weight *= likelihood;
  }

  // BOOST_LOG_TRIVIAL(debug)
  //     << (boost::format("Updating weight to %.3e") % weight).str();
}

void ParticleFilter::init(double x, double y, double theta,
                          const Eigen::Vector3d &std) {
  /**
   * TODO: Set the number of particles. Initialize all particles to
   *   first position (based on estimates of x, y, theta and their uncertainties
   *   from GPS) and all weights to 1.
   * TODO: Add random Gaussian noise to each particle.
   * NOTE: Consult particle_filter.h for more information about this method
   *   (and others in this file).
   */
  BOOST_LOG_TRIVIAL(info) << "Initializing particle filters.";
  std::default_random_engine gen;
  gen.seed(std::time(0));

  std::normal_distribution<double> x_d{x, std[0]};
  std::normal_distribution<double> y_d{y, std[1]};
  std::normal_distribution<double> s_d{theta, std[2]};

  particles = vector<Particle>(num_particles);
  weights = vector<double>(num_particles);
  for (int i = 0; i < num_particles; ++i) {
    Particle &p = particles[i];

    p.id = i;
    p.x = x_d(gen);
    p.y = y_d(gen);
    p.theta = s_d(gen);
    p.weight = 1.0;

    weights[i] = 1.0;
  }

  // Set to true after initialization
  is_initialized = true;
}

void ParticleFilter::prediction(double delta_t, const Eigen::Vector3d &std_pose,
                                double velocity, double yaw_rate) {
  /**
   * TODO: Add measurements to each particle and add random Gaussian noise.
   * NOTE: When adding noise you may find std::normal_distribution
   *   and std::default_random_engine useful.
   *  http://en.cppreference.com/w/cpp/numeric/random/normal_distribution
   *  http://www.cplusplus.com/reference/random/default_random_engine/
   */
  using std::abs;
  using std::cos;
  using std::sin;

  // BOOST_LOG_TRIVIAL(info) << "Run prediction.";
  std::default_random_engine gen;
  gen.seed(std::time(0));

  std::normal_distribution<double> x_d{0.0, std_pose[0]};
  std::normal_distribution<double> y_d{0.0, std_pose[1]};
  std::normal_distribution<double> s_d{0.0, std_pose[2]};

  for (size_t i = 0; i < particles.size(); ++i) {
    Particle &p = particles[i];
    double delta_yaw = 0.0;
    double delta_x = 0.0;
    double delta_y = 0.0;
    if (abs(yaw_rate) < 1e-10) {
      delta_x = velocity * delta_t * cos(p.theta);
      delta_y = velocity * delta_t * sin(p.theta);
    } else {
      delta_yaw = yaw_rate * delta_t;
      delta_x = velocity / yaw_rate * (sin(p.theta + delta_yaw) - sin(p.theta));
      delta_y = velocity / yaw_rate * (cos(p.theta) - cos(p.theta + delta_yaw));
    }
    p.x += delta_x + x_d(gen);
    p.y += delta_y + y_d(gen);
    p.theta += delta_yaw + s_d(gen);
  }
}

Eigen::Matrix2Xd Particle::computeObsvervationsInWorld(
    const std::vector<LandmarkObs> &observations) {
  // Construct eigen matrices for convenient computation
  vector<double> obsBuffer(observations.size() * 2);
  for (size_t i = 0, j = 0; i < observations.size(); ++i, j += 2) {
    obsBuffer[j] = observations[i].x;
    obsBuffer[j + 1] = observations[i].y;
  }

  Matrix2Xd obs =
      Eigen::Map<Matrix2Xd>(obsBuffer.data(), 2, observations.size());
  Isometry2d Tparticle = getIsometry2d(x, y, theta);
  return (Tparticle * obs.colwise().homogeneous()).topRows<2>();
}

void ParticleFilter::updateWeights(double sensor_range,
                                   const Eigen::Vector2d &std_landmark,
                                   const vector<LandmarkObs> &observations,
                                   const Map &map_landmarks) {
  /**
   * NOTE: The observations are given in the VEHICLE'S coordinate system.
   *   Your particles are located according to the MAP'S coordinate system.
   *   You will need to transform between the two systems. Keep in mind that
   *   this transformation requires both rotation AND translation (but no
   * scaling). The following is a good resource for the theory:
   *   https://www.willamette.edu/~gorr/classes/GeneralGraphics/Transforms/transforms2d.htm
   *   and the following is a good resource for the actual equation to implement
   *   (look at equation 3.33) http://planning.cs.uiuc.edu/node99.html
   */
  // BOOST_LOG_TRIVIAL(debug) << "Updating particle weights.";
  using Eigen::Isometry2d;
  using Eigen::Vector2f;
  using namespace std::chrono;

  // Update sense_x, sense_y and associations.
  auto start = high_resolution_clock::now();

#ifdef _OPENMP
  #pragma omp parallel for
#endif
  for (size_t i = 0; i < particles.size(); ++i) {
    Particle &p = particles[i];

    const Eigen::Matrix2Xd observationsInWorld =
        p.computeObsvervationsInWorld(observations);

    Matrix2Xd predictedObservations;
    vector<int> predictedLandmarkIndices;
    std::tie(predictedObservations, predictedLandmarkIndices) =
        p.computePredictedObservations(map_landmarks, sensor_range,
                                       std_landmark);

    std::vector<int> matchedIndices =
        p.observationAssociation(observationsInWorld, predictedObservations);

    // Update associations
    p.associations.clear();
    p.sense_x.clear();
    p.sense_y.clear();
    for (int matchedIndex : matchedIndices) {
      // +1 to output the landmark id
      p.associations.push_back(predictedLandmarkIndices[matchedIndex] + 1);
      p.sense_x.push_back(predictedObservations.col(matchedIndex).x());
      p.sense_y.push_back(predictedObservations.col(matchedIndex).y());
    }

    p.updateWeight(observationsInWorld, predictedObservations, matchedIndices,
                   std_landmark);

    weights[i] = p.weight;
  }

  auto elapsedTime = duration_cast<milliseconds>(high_resolution_clock::now() - start).count();
  BOOST_LOG_TRIVIAL(debug) << "updateWeights took " << elapsedTime << " ms";
}

void ParticleFilter::resample() {
  using namespace std;
  if (weights.empty()) {
    return;
  }

  VectorXd weights_ =
      Eigen::Map<VectorXd>(weights.data(), weights.size(), 1);
  weights_ /= weights_.sum();

  // BOOST_LOG_TRIVIAL(info) << "Weights: " << weights_.transpose();

  // First index initialization
  std::default_random_engine gen;
  gen.seed(std::time(0));
  std::uniform_real_distribution<> urd(0, 2.0 * weights_.maxCoeff());
  std::uniform_int_distribution<> rid(0, num_particles - 1);

  int sampledIndex = rid(gen);

  // Resampling the particles
  vector<Particle> newParticles(num_particles);
  // Eigen::VectorXi samples(num_particles);
  // samples[0] = sampledIndex;
  newParticles[0] = particles[sampledIndex];

  double beta = 0.0;

  for (int i = 1; i < num_particles; ++i) {
    beta += urd(gen);
    while (weights_[sampledIndex] < beta) {
      beta -= weights_[sampledIndex];
      sampledIndex = (sampledIndex + 1) % num_particles;
    }
    // samples[i] = sampledIndex; // DEBUG
    newParticles[i] = particles[sampledIndex];
    newParticles[i].weight = weights_[sampledIndex];
    newParticles[i].id = i;
  }

  particles = newParticles;
  // BOOST_LOG_TRIVIAL(info) << "samples: " << samples.transpose();
}

void ParticleFilter::setAssociations(Particle &particle,
                                     const vector<int> &associations,
                                     const vector<double> &sense_x,
                                     const vector<double> &sense_y) {
  // particle: the particle to which assign each listed association,
  //   and association's (x,y) world coordinates mapping
  // associations: The landmark id that goes along with each listed association
  // sense_x: the associations x mapping already converted to world coordinates
  // sense_y: the associations y mapping already converted to world coordinates
  particle.associations = associations;
  particle.sense_x = sense_x;
  particle.sense_y = sense_y;
}

string ParticleFilter::getAssociations(Particle best) {
  vector<int> v = best.associations;
  std::stringstream ss;
  copy(v.begin(), v.end(), std::ostream_iterator<int>(ss, " "));
  string s = ss.str();
  s = s.substr(0, s.length() - 1);  // get rid of the trailing space
  return s;
}

string ParticleFilter::getSenseCoord(Particle best, string coord) {
  vector<double> v;

  if (coord == "X") {
    v = best.sense_x;
  } else {
    v = best.sense_y;
  }

  std::stringstream ss;
  copy(v.begin(), v.end(), std::ostream_iterator<float>(ss, " "));
  string s = ss.str();
  s = s.substr(0, s.length() - 1);  // get rid of the trailing space
  return s;
}


//==================================================================================================
/*!
  @file

  @copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_ARCH_COMMON_SCALAR_FUNCTION_MIN_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_SCALAR_FUNCTION_MIN_HPP_INCLUDED

#include <boost/simd/detail/dispatch/function/overload.hpp>
#include <boost/config.hpp>
#include <boost/simd/function/conformant.hpp>
#include <boost/simd/function/std.hpp>
#include <algorithm>

namespace boost { namespace simd { namespace ext
{
  namespace bd = boost::dispatch;
  namespace bs = boost::simd;
  BOOST_DISPATCH_OVERLOAD ( min_
                          , (typename A0)
                          , bd::cpu_
                          , bd::scalar_< bd::arithmetic_<A0> >
                          , bd::scalar_< bd::arithmetic_<A0> >
                          )
  {
    BOOST_FORCEINLINE A0 operator()( A0 a0, A0 a1) const BOOST_NOEXCEPT
    {
      return (a1 < a0) ? a1 : a0;
    }
  };

  BOOST_DISPATCH_OVERLOAD ( min_
                          , (typename A0)
                          , bd::cpu_
                          , bs::conformant_tag
                          , bd::scalar_< bd::arithmetic_<A0> >
                          , bd::scalar_< bd::arithmetic_<A0> >
                          )
  {
    BOOST_FORCEINLINE A0 operator()( conformant_tag const&, A0 a0, A0 a1) const BOOST_NOEXCEPT
    {
      return  (a1 < a0) ? a1 : a0;
    }
  };

  BOOST_DISPATCH_OVERLOAD ( min_
                          , (typename A0)
                          , bd::cpu_
                          , bs::std_tag
                          , bd::scalar_< bd::arithmetic_<A0> >
                          , bd::scalar_< bd::arithmetic_<A0> >
                          )
  {
    BOOST_FORCEINLINE A0 operator()( std_tag const&, A0 a0, A0 a1) const BOOST_NOEXCEPT
    {
      return std::min(a0, a1);
    }
  };

} } }

#endif


//---------------------------------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;
}

}


#pragma once

#include <boost/type_traits/add_pointer.hpp>
#include <boost/mpl/transform.hpp>
#include <boost/mpl/for_each.hpp>
#include <boost/fusion/include/as_vector.hpp>
#include <boost/fusion/include/mpl.hpp>
#include <boost/fusion/include/for_each.hpp>
#include <libv/lma/ttt/traits/functor_trait.hpp>
#include <libv/lma/ttt/traits/wrap.hpp>

#include <vector>
#include <Eigen/Core>

template<class T> using AlignedVector = std::vector<T,Eigen::aligned_allocator<T>>;

namespace lma
{

  template<class Solver, class Functors> struct SolverAdd
  {
    Solver& solver;
    Functors& functors;
    SolverAdd(Solver& solver_, Functors& functors_):solver(solver_),functors(functors_){}
    template<class Key> void operator()(const ttt::wrap<Key>&)
    {
      auto& v = *boost::fusion::find<Key>(functors);
      for(auto& pair : v) solver.add(pair.first,pair.second);
    }
  };

  template<class L> class FunctorsContainer
  {
    private:
      template<class F> struct F2type
      {
        typedef typename ttt::Extract<decltype(&F::operator())>::Args args_;
        typedef typename boost::mpl::transform<args_,boost::add_pointer<boost::mpl::_1>>::type args;
        typedef typename boost::fusion::result_of::as_vector<args>::type v_args;
        typedef AlignedVector<std::pair<F,v_args>> type;
      };

      typedef typename boost::mpl::transform<L, F2type<boost::mpl::_1> >::type Types;
      typedef typename boost::fusion::result_of::as_vector<Types>::type Functors;

      Functors functors;

    public:
      template<class F, class ... P> FunctorsContainer& add(const F& f, const P& ... p)
      {
        typedef typename F2type<F>::type Container;
        typedef typename F2type<F>::v_args v_args;
        (*boost::fusion::find<Container>(functors)).emplace_back(f,v_args(p...));
        return *this;
      }

      void clear()
      {
        boost::fusion::for_each(functors,[](auto& v){v.clear();});
      }

      template<class Solver> void fill(Solver& solver)
      {
        boost::mpl::for_each<Types,ttt::wrap_>(SolverAdd<Solver,Functors>(solver,functors));
      }
  };

}


/*
 * MutexConstraint.cpp
 * @brief Mutually exclusive constraint
 * @date Mar 20, 2022
 * @author Yoonwoo Kim
 */

#include <gtsam/base/Testable.h>
#include <gtsam/discrete/DecisionTreeFactor.h>
#include <cpp/planning/MutexConstraint.h>
#include <cpp/planning/NotSingleValueConstraint.h>
#include <cpp/planning/SingleValueConstraint.h>
#include <cpp/planning/OrConstraint.h>

#include <boost/make_shared.hpp>
using namespace gtsam;
using namespace std;

namespace gtsam_planner {

/* ************************************************************************* */
MutexConstraint::MutexConstraint(const DiscreteKeys& dkeys,
  const vector<size_t>& values) : DiscreteFactor(dkeys.indices()) {
  for (const DiscreteKey& dkey : dkeys) cardinalities_.insert(dkey);
  for (const size_t& value : values) values_.push_back(value);
}

/* ************************************************************************* */
void MutexConstraint::print(const std::string& s, const KeyFormatter& formatter) const {
  std::cout << s << "MutexConstraint on ";
  for (Key dkey : keys_) std::cout << formatter(dkey) << " ";
  std::cout << std::endl;
}

/* ************************************************************************* */
double MutexConstraint::operator()(const DiscreteValues& values) const {
  size_t count = 0;
  for (size_t i=0; i < values_.size(); i++) {
    size_t value = values.at(keys_[i]);
    if (value == values_[i]) count++;
    if (count > 1.0) return 0.0;
  }
  return 1.0;
}

/* ************************************************************************* */
DecisionTreeFactor MutexConstraint::toDecisionTreeFactor() const {
  vector<DecisionTreeFactor> factors;
  size_t nrKeys = keys_.size();
  for (size_t i = 0; i < nrKeys; i++) {
    SingleValueConstraint single(discreteKey(i), values_[i]);
    DecisionTreeFactor singleTree = single.toDecisionTreeFactor();
    for (size_t j = 0; j < nrKeys; j++) {
      if (i == j) continue;
      NotSingleValueConstraint notSingle(discreteKey(j), values_[j]);
      singleTree = singleTree * notSingle.toDecisionTreeFactor();
    }
    factors.push_back(singleTree);
  }
  OrConstraint mutex(factors);
  DecisionTreeFactor mutex_tree = mutex.toDecisionTreeFactor();
  return mutex_tree;
}

/* ************************************************************************* */
DecisionTreeFactor MutexConstraint::operator*(const DecisionTreeFactor& f) const {
  // TODO: can we do this more efficiently?
  return toDecisionTreeFactor() * f;
}

/* ************************************************************************* */
}  // namespace gtsam


// Copyright John Maddock 2012.

// Use, modification and distribution are subject to the
// Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt
// or copy at http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_MATH_TEST_OUT_OF_RANGE_HPP
#define BOOST_MATH_TEST_OUT_OF_RANGE_HPP

#include <boost/math/special_functions/next.hpp>
#include <boost/test/test_tools.hpp>

/*` check_out_of_range functions check that bad parameters
passed to constructors and functions throw domain_error exceptions.

Usage is `check_out_of_range<DistributionType >(list-of-params);`
Where list-of-params is a list of *valid* parameters from which the distribution can be constructed
- ie the same number of args are passed to the function,
as are passed to the distribution constructor.

Checks:

* Infinity or NaN passed in place of each of the valid params.
* Infinity or NaN as a random variable.
* Out-of-range random variable passed to pdf and cdf (ie outside of "range(distro)").
* Out-of-range probability passed to quantile function and complement.

but does *not* check finite but out-of-range parameters to the constructor
because these are specific to each distribution.
*/

#ifdef BOOST_MSVC
#pragma warning(push)
#pragma warning(disable:4127)
#endif

template <class Distro>
void check_support(const Distro& d)
{ // Checks that
   typedef typename Distro::value_type value_type;
   if((boost::math::isfinite)(range(d).first) && (range(d).first != -boost::math::tools::max_value<value_type>()))
   { // If possible, check that a random variable value just less than the bottom of the supported range throws domain errors.
      value_type m = (range(d).first == 0) ? -boost::math::tools::min_value<value_type>() : boost::math::float_prior(range(d).first);
      BOOST_ASSERT(m != range(d).first);
      BOOST_ASSERT(m < range(d).first);
      BOOST_MATH_CHECK_THROW(pdf(d, m), std::domain_error);
      BOOST_MATH_CHECK_THROW(cdf(d, m), std::domain_error);
      BOOST_MATH_CHECK_THROW(cdf(complement(d, m)), std::domain_error);
   }
   if((boost::math::isfinite)(range(d).second) && (range(d).second != boost::math::tools::max_value<value_type>()))
   { // If possible, check that a random variable value just more than the top of the supported range throws domain errors.
      value_type m = (range(d).second == 0) ? boost::math::tools::min_value<value_type>() : boost::math::float_next(range(d).second);
      BOOST_ASSERT(m != range(d).first);
      BOOST_ASSERT(m > range(d).first);
      BOOST_MATH_CHECK_THROW(pdf(d, m), std::domain_error);
      BOOST_MATH_CHECK_THROW(cdf(d, m), std::domain_error);
      BOOST_MATH_CHECK_THROW(cdf(complement(d, m)), std::domain_error);
   }
   if(std::numeric_limits<value_type>::has_infinity)
   { // Infinity is available,
      if((boost::math::isfinite)(range(d).second))
      {  // and top of range doesn't include infinity,
         // check that using infinity throws domain errors.
         BOOST_MATH_CHECK_THROW(pdf(d, std::numeric_limits<value_type>::infinity()), std::domain_error);
         BOOST_MATH_CHECK_THROW(cdf(d, std::numeric_limits<value_type>::infinity()), std::domain_error);
         BOOST_MATH_CHECK_THROW(cdf(complement(d, std::numeric_limits<value_type>::infinity())), std::domain_error);
      }
      if((boost::math::isfinite)(range(d).first))
      {  // and bottom of range doesn't include infinity,
         // check that using infinity throws domain_error exception.
         BOOST_MATH_CHECK_THROW(pdf(d, -std::numeric_limits<value_type>::infinity()), std::domain_error);
         BOOST_MATH_CHECK_THROW(cdf(d, -std::numeric_limits<value_type>::infinity()), std::domain_error);
         BOOST_MATH_CHECK_THROW(cdf(complement(d, -std::numeric_limits<value_type>::infinity())), std::domain_error);
      }
      // Check that using infinity with quantiles always throws domain_error exception.
      BOOST_MATH_CHECK_THROW(quantile(d, std::numeric_limits<value_type>::infinity()), std::domain_error);
      BOOST_MATH_CHECK_THROW(quantile(d, -std::numeric_limits<value_type>::infinity()), std::domain_error);
      BOOST_MATH_CHECK_THROW(quantile(complement(d, std::numeric_limits<value_type>::infinity())), std::domain_error);
      BOOST_MATH_CHECK_THROW(quantile(complement(d, -std::numeric_limits<value_type>::infinity())), std::domain_error);
   }
   if(std::numeric_limits<value_type>::has_quiet_NaN)
   { // NaN is available.
      BOOST_MATH_CHECK_THROW(pdf(d, std::numeric_limits<value_type>::quiet_NaN()), std::domain_error);
      BOOST_MATH_CHECK_THROW(cdf(d, std::numeric_limits<value_type>::quiet_NaN()), std::domain_error);
      BOOST_MATH_CHECK_THROW(cdf(complement(d, std::numeric_limits<value_type>::quiet_NaN())), std::domain_error);
      BOOST_MATH_CHECK_THROW(pdf(d, -std::numeric_limits<value_type>::quiet_NaN()), std::domain_error);
      BOOST_MATH_CHECK_THROW(cdf(d, -std::numeric_limits<value_type>::quiet_NaN()), std::domain_error);
      BOOST_MATH_CHECK_THROW(cdf(complement(d, -std::numeric_limits<value_type>::quiet_NaN())), std::domain_error);
      BOOST_MATH_CHECK_THROW(quantile(d, std::numeric_limits<value_type>::quiet_NaN()), std::domain_error);
      BOOST_MATH_CHECK_THROW(quantile(d, -std::numeric_limits<value_type>::quiet_NaN()), std::domain_error);
      BOOST_MATH_CHECK_THROW(quantile(complement(d, std::numeric_limits<value_type>::quiet_NaN())), std::domain_error);
      BOOST_MATH_CHECK_THROW(quantile(complement(d, -std::numeric_limits<value_type>::quiet_NaN())), std::domain_error);
   }
   // Check that using probability outside [0,1] with quantiles always throws domain_error exception.
   BOOST_MATH_CHECK_THROW(quantile(d, -1), std::domain_error);
   BOOST_MATH_CHECK_THROW(quantile(d, 2), std::domain_error);
   BOOST_MATH_CHECK_THROW(quantile(complement(d, -1)), std::domain_error);
   BOOST_MATH_CHECK_THROW(quantile(complement(d, 2)), std::domain_error);
}

// Four check_out_of_range versions for distributions with zero to 3 constructor parameters.

template <class Distro>
void check_out_of_range()
{
   Distro d;
   check_support(d);
}

template <class Distro>
void check_out_of_range(typename Distro::value_type p1)
{
   typedef typename Distro::value_type value_type;
   Distro d(p1);
   check_support(d);
   if(std::numeric_limits<value_type>::has_infinity)
   {
      BOOST_MATH_CHECK_THROW(pdf(Distro(std::numeric_limits<value_type>::infinity()), range(d).first), std::domain_error);
 //     BOOST_MATH_CHECK_THROW(pdf(Distro(std::numeric_limits<value_type>::infinity()), range(d).second), std::domain_error);
   }
   if(std::numeric_limits<value_type>::has_quiet_NaN)
   {
      BOOST_MATH_CHECK_THROW(pdf(Distro(std::numeric_limits<value_type>::quiet_NaN()), range(d).first), std::domain_error);
   }
}

template <class Distro>
void check_out_of_range(typename Distro::value_type p1, typename Distro::value_type p2)
{
   typedef typename Distro::value_type value_type;
   Distro d(p1, p2);
   check_support(d);
   if(std::numeric_limits<value_type>::has_infinity)
   {
      BOOST_MATH_CHECK_THROW(pdf(Distro(std::numeric_limits<value_type>::infinity(), p2), range(d).first), std::domain_error);
      BOOST_MATH_CHECK_THROW(pdf(Distro(p1, std::numeric_limits<value_type>::infinity()), range(d).first), std::domain_error);
   }
   if(std::numeric_limits<value_type>::has_quiet_NaN)
   {
      BOOST_MATH_CHECK_THROW(pdf(Distro(std::numeric_limits<value_type>::quiet_NaN(), p2), range(d).first), std::domain_error);
      BOOST_MATH_CHECK_THROW(pdf(Distro(p1, std::numeric_limits<value_type>::quiet_NaN()), range(d).first), std::domain_error);
   }
}

template <class Distro>
void check_out_of_range(typename Distro::value_type p1, typename Distro::value_type p2, typename Distro::value_type p3)
{
   typedef typename Distro::value_type value_type;
   Distro d(p1, p2, p3);
   check_support(d);
   if(std::numeric_limits<value_type>::has_infinity)
   {
      BOOST_MATH_CHECK_THROW(pdf(Distro(std::numeric_limits<value_type>::infinity(), p2, p3), range(d).first), std::domain_error);
      BOOST_MATH_CHECK_THROW(pdf(Distro(p1, std::numeric_limits<value_type>::infinity(), p3), range(d).first), std::domain_error);
      BOOST_MATH_CHECK_THROW(pdf(Distro(p1, p2, std::numeric_limits<value_type>::infinity()), range(d).first), std::domain_error);
   }
   if(std::numeric_limits<value_type>::has_quiet_NaN)
   {
      BOOST_MATH_CHECK_THROW(pdf(Distro(std::numeric_limits<value_type>::quiet_NaN(), p2, p3), range(d).first), std::domain_error);
      BOOST_MATH_CHECK_THROW(pdf(Distro(p1, std::numeric_limits<value_type>::quiet_NaN(), p3), range(d).first), std::domain_error);
      BOOST_MATH_CHECK_THROW(pdf(Distro(p1, p2, std::numeric_limits<value_type>::quiet_NaN()), range(d).first), std::domain_error);
   }
}

#ifdef BOOST_MSVC
#pragma warning(pop)
#endif

#endif // BOOST_MATH_TEST_OUT_OF_RANGE_HPP


#include <iostream>
#include <algorithm>
#include <iterator>
#include <ctime>
#include <typeinfo>

#include <boost/random.hpp>
#include <boost/generator_iterator.hpp>
#include <boost/date_time/gregorian/gregorian.hpp>
#include <boost/date_time/posix_time/posix_time.hpp>
#include <boost/algorithm/string.hpp>

#include "mongoose.h"
#include "json.hpp"

#include "nanocube.h"
#include "nanocube_traversals.h"
#include "naivecube.h"

using json = nlohmann::json;

static const char *s_http_port = "8801";
static struct mg_serve_http_opts s_http_server_opts;

static int qtreeLevel = 10;
static vector<int> schema = {qtreeLevel*2, qtreeLevel*2};
static Naivecube<int> naivecube(schema);

// convert lat,lon to quad tree address
int64_t loc2addr(double lat, double lon, int qtreeLevel)
{
  double xd = (lon + M_PI) / (2.0 * M_PI);
  double yd = (log(tan(M_PI / 4.0 + lat / 2.0)) + M_PI) / (2.0 * M_PI);
  //cout << lat << " " << lon << " " << endl;
  int x = xd * (1 << qtreeLevel), y = yd * (1 << qtreeLevel);

  int64_t z = 0; // z gets the resulting Morton Number.

  for (int i = 0; i < sizeof(x) * 8; i++) {// unroll for more speed...
    z |= (x & 1U << i) << i | (y & 1U << i) << (i + 1);
  }

  return z;
}

void buildCubes()
{
  cout << "Start building Nanocubes..." << endl;
  using namespace boost::gregorian;
  using namespace boost::posix_time;

  ifstream is("../sample_data/flights_100K.csv.txt");
  string s;

  int i = 0;

  while(std::getline(is, s)) {
    vector<string> output;
    boost::split(output,s,boost::is_any_of("\t"));
    if (output.size() != 4) {
      cerr << "Bad line:" << s << endl;
      continue;
    }

    double ori_lat = atof(output[0].c_str()) * M_PI / 180.0;
    double ori_lon = atof(output[1].c_str()) * M_PI / 180.0;
    double des_lat = atof(output[2].c_str()) * M_PI / 180.0;
    double des_lon = atof(output[3].c_str()) * M_PI / 180.0;

    if(ori_lat > 85.0511 || ori_lat < -85.0511 ||
        des_lat > 85.0511 || des_lat < -85.0511 ) {
      cerr << "Invalid latitude: " << ori_lat << ", " << des_lat;
      cerr << " (should be in [-85.0511, 85.0511])" << endl;
      continue;
    }

    int64_t d1 = loc2addr(ori_lat, ori_lon, qtreeLevel);
    int64_t d2 = loc2addr(des_lat, des_lon, qtreeLevel);

    naivecube.insert(1, {d1, d2});

    if (++i % 10000 == 0) {
      //nc.report_size();
      cout << i << endl;
    }
  }
}

static void handle_query_call(struct mg_connection *c, struct http_message *hm) {

  json q = json::parse(string(hm->body.p, hm->body.len));
  json result = NaiveCubeQuery(q, naivecube);

  /* Send result */
  std::string msg_content = result.dump();
  const std::string sep = "\r\n";

  std::stringstream ss;
  ss << "HTTP/1.1 200 OK"             << sep
    << "Content-Type: application/json" << sep
    << "Access-Control-Allow-Origin: *" << sep
    << "Content-Length: %d"             << sep << sep
    << "%s";

  mg_printf(c, ss.str().c_str(), (int) msg_content.size(), msg_content.c_str());
}

static void ev_handler(struct mg_connection *c, int ev, void *ev_data) {
  struct http_message *hm = (struct http_message *) ev_data;

  switch (ev) {
    case MG_EV_HTTP_REQUEST:
      if (mg_vcmp(&hm->uri, "/query") == 0) {
        handle_query_call(c, hm); /* Handle RESTful call */
      }
      else {
        mg_serve_http(c, hm, s_http_server_opts); /* Serve static content */
      }
      break;
    default:
      break;
  }
}

int main(int argc, char *argv[]) {

  struct mg_mgr mgr;
  struct mg_connection *c;

  mg_mgr_init(&mgr, NULL);
  c = mg_bind(&mgr, s_http_port, ev_handler);
  mg_set_protocol_http_websocket(c);

  s_http_server_opts.document_root = "./";
  s_http_server_opts.enable_directory_listing = "no";

  // build Gaussian Cubes
  buildCubes();

  printf("Starting server on port %s\n", s_http_port);

  for (;;) {
    mg_mgr_poll(&mgr, 1000);
  }

  mg_mgr_free(&mgr);

  return 0;
}


#include <boost/geometry/algorithms/union.hpp>


#include <chrono>
#include <cmath>
#include <iostream>
#include <vector>

#include <boost/simd/function/aligned_store.hpp>
#include <boost/simd/function/cos.hpp>
#include <boost/simd/function/exp.hpp>
#include <boost/simd/function/sin.hpp>
#include <boost/simd/memory/allocator.hpp>
#include <boost/simd/pack.hpp>

int main()
{
  using namespace std::chrono;
  namespace bs = boost::simd;
  using pack_t = bs::pack<float>;
  int size     = 80000000;
  std::vector<float, bs::allocator<float>> data(size);
  std::vector<float, bs::allocator<float>> output(size);

  auto t0 = high_resolution_clock::now();
  for (int i = 0; i < size; ++i) {
    data[i] *= 2;
  }
  auto t1 = high_resolution_clock::now();
  std::cout << " time " << duration_cast<microseconds>(t1 - t0).count() << std::endl;
  t0 = high_resolution_clock::now();
  //! [memory-memory]
  for (int i = 0; i < size; i += pack_t::static_size) {
    pack_t v0(&data[i]);
    v0 = v0 * 2;
    bs::aligned_store(v0, &output[i]);
  }
  //! [memory-memory]
  t1 = high_resolution_clock::now();
  std::cout << " time " << duration_cast<microseconds>(t1 - t0).count() << std::endl;

  t0 = high_resolution_clock::now();
  for (int i = 0; i < size; ++i) {
    data[i] = std::exp(std::exp(data[i]));
  }
  t1 = high_resolution_clock::now();
  std::cout << " time " << duration_cast<microseconds>(t1 - t0).count() << std::endl;
  //! [memory-compute]
  t0 = high_resolution_clock::now();
  for (int i = 0; i < size; i += pack_t::static_size) {
    pack_t v0(&data[i]);
    v0 = bs::exp(bs::exp(v0));
    bs::aligned_store(v0, &output[i]);
  }
  //! [memory-compute]
  t1 = high_resolution_clock::now();
  std::cout << " time " << duration_cast<microseconds>(t1 - t0).count() << std::endl;
}


#include "CalibrationMotionLogging.h"

#include <mc_rbdyn/rpy_utils.h>
#include <mc_rtc/io_utils.h>

#include <boost/filesystem.hpp>

#include "../ForceSensorCalibration.h"
#include "../Measurement.h"

namespace bfs = boost::filesystem;

void CalibrationMotionLogging::start(mc_control::fsm::Controller & ctl_)
{
  auto & ctl = static_cast<ForceSensorCalibration &>(ctl_);

  auto & robot = ctl.robot();
  auto robotConf = ctl.config()(robot.name());

  singularityThreshold_ = ctl.config()("SingularityThreshold");

  if(!robotConf.has("forceSensors"))
  {
    mc_rtc::log::error_and_throw<std::runtime_error>("Calibration controller expects a forceSensors entry");
  }
  sensors_ = robotConf("forceSensors");
  auto outputPath_ = (bfs::temp_directory_path() / fmt::format("calib-force-sensors-data-{}", robot.name())).string();

  // Attempt to create the output files
  if(!boost::filesystem::exists(outputPath_))
  {
    if(!boost::filesystem::create_directory(outputPath_))
    {
      mc_rtc::log::error_and_throw<std::runtime_error>("[{}] Could not create output folder {}", name(), outputPath_);
    }
  }

  if(!ctl.datastore().has("measurements"))
  {
    ctl.datastore().make<SensorMeasurements>("measurements");
  }
  auto & measurements = ctl.datastore().get<SensorMeasurements>("measurements");
  measurements.clear();
  for(const auto & s : sensors_)
  {
    measurements[s] = {};
    measurementsCount_[s] = {0, 0};
    if(singularityThreshold_ > 0)
    {
      const auto & sensor = ctl_.robot().forceSensor(s);
      jacobians_[s] = {ctl_.robot(), sensor.parentBody()};
    }
  }
}

bool CalibrationMotionLogging::run(mc_control::fsm::Controller & ctl_)
{
  auto & robot = ctl_.robot();
  auto & real = ctl_.realRobot();
  auto & measurements = ctl_.datastore().get<SensorMeasurements>("measurements");
  for(const auto & s : sensors_)
  {
    const auto & sensor = robot.forceSensor(s);
    const auto & X_0_p = real.bodyPosW()[real.bodyIndexByName(sensor.parentBody())];
    const auto & measure = sensor.wrench();
    measurementsCount_[s].second++;
    if(singularityThreshold_ > 0)
    {
      auto & j = jacobians_[s];
      const auto & jacMat = j.jacobian.jacobian(robot.mb(), robot.mbc());
      j.svd.compute(jacMat);
      if(j.svd.singularValues().tail(1)(0) > singularityThreshold_)
      {
        measurements[s].push_back({X_0_p, measure});
        measurementsCount_[s].first++;
      }
    }
    else
    {
      measurements[s].push_back({X_0_p, measure});
      measurementsCount_[s].first++;
    }
  }
  output("OK");
  return true;
}

void CalibrationMotionLogging::teardown(mc_control::fsm::Controller &)
{
  for(const auto & m : measurementsCount_)
  {
    mc_rtc::log::info("[{}] {} records taken out of {} iterations", m.first, m.second.first, m.second.second);
  }
}

EXPORT_SINGLE_STATE("CalibrationMotionLogging", CalibrationMotionLogging)


// Copyright 2013 Christian Henning
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

/// \brief Unit test for subchroma_image type.

#include <boost/test/unit_test.hpp>

#include <boost/gil.hpp>
#include <boost/gil/extension/toolbox/color_spaces/ycbcr.hpp>
#include <boost/gil/extension/toolbox/image_types/subchroma_image.hpp>

#include <boost/mp11.hpp>

using namespace std;
using namespace boost;
using namespace gil;

BOOST_AUTO_TEST_SUITE( toolbox_tests )

BOOST_AUTO_TEST_CASE( subchroma_image_test )
{
    {
        ycbcr_601_8_pixel_t a( 10, 20, 30 );
        rgb8_pixel_t b;
        bgr8_pixel_t c;

        color_convert( a, b );
        color_convert( a, c );
        BOOST_ASSERT( static_equal( b, c ));

        color_convert( b, a );
    }

    {
        ycbcr_709_8_pixel_t a( 10, 20, 30 );
        rgb8_pixel_t b;
        bgr8_pixel_t c;

        color_convert( a, b );
        color_convert( a, c );
        BOOST_ASSERT( static_equal( b, c ));

        color_convert( b, a );
    }

    {
        using pixel_t = rgb8_pixel_t;
        using image_t = subchroma_image<pixel_t>;

        image_t img( 640, 480 );

        fill_pixels( view( img )
                   , pixel_t( 10, 20, 30 )
                   );
    }

    {
        using pixel_t = rgb8_pixel_t;

        subchroma_image<pixel_t, mp11::mp_list_c<int, 4, 4, 4>> a(640, 480);
        static_assert(a.ss_X == 1 && a.ss_Y == 1, "");
        subchroma_image<pixel_t, mp11::mp_list_c<int, 4, 4, 0>> b(640, 480);
        static_assert(b.ss_X == 1 && b.ss_Y == 2, "");
        subchroma_image<pixel_t, mp11::mp_list_c<int, 4, 2, 2>> c(640, 480);
        static_assert(c.ss_X == 2 && c.ss_Y == 1, "");
        subchroma_image<pixel_t, mp11::mp_list_c<int, 4, 2, 0>> d(640, 480);
        static_assert(d.ss_X == 2 && d.ss_Y == 2, "");
        subchroma_image<pixel_t, mp11::mp_list_c<int, 4, 1, 1>> e(640, 480);
        static_assert(e.ss_X == 4 && e.ss_Y == 1, "");
        subchroma_image<pixel_t, mp11::mp_list_c<int, 4, 1, 0>> f(640, 480);
        static_assert(f.ss_X == 4 && f.ss_Y == 2, "");

        fill_pixels( view( a ), pixel_t( 10, 20, 30 ) );
        fill_pixels( view( b ), pixel_t( 10, 20, 30 ) );
        fill_pixels( view( c ), pixel_t( 10, 20, 30 ) );
        fill_pixels( view( d ), pixel_t( 10, 20, 30 ) );
        fill_pixels( view( e ), pixel_t( 10, 20, 30 ) );
        fill_pixels( view( f ), pixel_t( 10, 20, 30 ) );

    }

    {
        using pixel_t = ycbcr_601_8_pixel_t;
        using factors_t = mp11::mp_list_c<int, 4, 2, 2>;
        using image_t = subchroma_image<pixel_t, factors_t>;

        std::size_t y_width     = 640;
        std::size_t y_height    = 480;

        std::size_t image_size = ( y_width * y_height )
                               + ( y_width / image_t::ss_X ) * ( y_height / image_t::ss_Y )
                               + ( y_width / image_t::ss_X ) * ( y_height / image_t::ss_Y );

        vector< unsigned char > data( image_size );

        image_t::view_t v = subchroma_view< pixel_t, factors_t >( y_width
                                                                , y_height
                                                                , &data.front()
                                                                );
        rgb8_pixel_t p;
        p = *v.xy_at( 0, 0 );
    }
}

BOOST_AUTO_TEST_SUITE_END()