// Header
#include "QuadProgMm.hpp"

// Standard library
#include <algorithm>
#include <vector>

// Boost
#include <boost/foreach.hpp>
#define foreach BOOST_FOREACH

namespace QuadProgMm {
  int Variable::nextId = 0;

  Variable::Variable() : id(++nextId) {}

  LinearExpression::LinearExpression(const std::map<Variable, double>& linearCoefficients_, double constantCoefficient_) :
    linearCoefficients(linearCoefficients_),
    constantCoefficient(constantCoefficient_)
  {}

  LinearExpression::LinearExpression(double d) : LinearExpression({}, d) {}

  LinearExpression::LinearExpression(const Variable& v) : LinearExpression({std::make_pair(v, 1)}, 0) {}

  LinearExpression& LinearExpression::operator += (const LinearExpression& other) {
    foreach(auto c, other.linearCoefficients) {
      linearCoefficients[c.first] += c.second;
    }
    constantCoefficient += other.constantCoefficient;
    return *this;
  }

  LinearExpression& LinearExpression::operator -= (const LinearExpression& other) {
    foreach(auto c, other.linearCoefficients) {
      linearCoefficients[c.first] -= c.second;
    }
    constantCoefficient -= other.constantCoefficient;
    return *this;
  }

  LinearExpression& LinearExpression::operator *= (double d) {
    foreach(auto c, linearCoefficients) {
      linearCoefficients[c.first] *= d;
    }
    constantCoefficient *= d;
    return *this;
  }

  LinearExpression& LinearExpression::operator /= (double d) {
    foreach(auto c, linearCoefficients) {
      linearCoefficients[c.first] /= d;
    }
    constantCoefficient /= d;
    return *this;
  }

  std::set<Variable> LinearExpression::getVariables() const {
    std::set<Variable> variables;
    foreach(auto c, linearCoefficients) {
      variables.insert(c.first);
    }
    return variables;
  }

  double LinearExpression::getLinearCoefficient(const Variable& v) const {
    auto it = linearCoefficients.find(v);
    if (it == linearCoefficients.end()) {
      return 0;
    } else {
      return it->second;
    }
  }

  double LinearExpression::getConstantCoefficient() const {
    return constantCoefficient;
  }

  QuadraticExpression::QuadraticExpression(
    const std::map<std::pair<Variable, Variable>, double>& quadraticCoefficients_,
    const std::map<Variable, double>& linearCoefficients_,
    double constantCoefficient_
  ) :
    quadraticCoefficients(quadraticCoefficients_),
    linearCoefficients(linearCoefficients_),
    constantCoefficient(constantCoefficient_)
  {}

  QuadraticExpression::QuadraticExpression(const LinearExpression& l) : QuadraticExpression({}, l.linearCoefficients, l.constantCoefficient) {}

  QuadraticExpression::QuadraticExpression(const Variable& v) : QuadraticExpression({}, {std::make_pair(v, 1)}, 0) {}

  QuadraticExpression::QuadraticExpression(double d) : QuadraticExpression({}, {}, d) {}

  std::set<Variable> QuadraticExpression::getVariables() const {
    std::set<Variable> variables;
    foreach(auto c, quadraticCoefficients) {
      variables.insert(c.first.first);
      variables.insert(c.first.second);
    }
    foreach(auto c, linearCoefficients) {
      variables.insert(c.first);
    }
    return variables;
  }

  QuadraticExpression QuadraticExpression::multiply(const LinearExpression& l, const LinearExpression& r) {
    double constantCoefficient = l.constantCoefficient * r.constantCoefficient;
    std::map<Variable, double> linearCoefficients = (l.constantCoefficient * r + r.constantCoefficient * l).linearCoefficients;
    std::map<std::pair<Variable, Variable>, double> quadraticCoefficients;

    foreach(auto lc, l.linearCoefficients) {
      foreach(auto rc, r.linearCoefficients) {
        quadraticCoefficients[std::minmax(lc.first, rc.first)] += lc.second * rc.second;
      }
    }

    return QuadraticExpression(quadraticCoefficients, linearCoefficients, constantCoefficient);
  }

  double QuadraticExpression::getQuadraticCoefficient(const Variable& v1, const Variable& v2) const {
    auto it = quadraticCoefficients.find(std::minmax(v1, v2));
    if (it == quadraticCoefficients.end()) {
      return 0;
    } else {
      return it->second;
    }
  }

  double QuadraticExpression::getLinearCoefficient(const Variable& v) const {
    auto it = linearCoefficients.find(v);
    if (it == linearCoefficients.end()) {
      return 0;
    } else {
      return it->second;
    }
  }

  double QuadraticExpression::getConstantCoefficient() const {
    return constantCoefficient;
  }

  QuadraticExpression& QuadraticExpression::operator += (const QuadraticExpression& other) {
    foreach(auto c, other.quadraticCoefficients) {
      quadraticCoefficients[c.first] += c.second;
    }
    foreach(auto c, other.linearCoefficients) {
      linearCoefficients[c.first] += c.second;
    }
    constantCoefficient += other.constantCoefficient;
    return *this;
  }

  QuadraticExpression& QuadraticExpression::operator -= (const QuadraticExpression& other) {
    return *this += -other;
  }

  QuadraticExpression& QuadraticExpression::operator *= (double d) {
    foreach(auto c, quadraticCoefficients) {
      quadraticCoefficients[c.first] *= d;
    }
    foreach(auto c, linearCoefficients) {
      linearCoefficients[c.first] *= d;
    }
    constantCoefficient *= d;
    return *this;
  }

  QuadraticExpression& QuadraticExpression::operator /= (double d) {
    return *this *= 1/d;
  }

  Constraint::Constraint(const LinearExpression& linearExpression_, Type type_) :
    linearExpression(linearExpression_),
    type(type_)
  {}

  const LinearExpression& Constraint::getLinearExpression() const {return linearExpression;}

  Constraint::Type Constraint::getType() const {return type;}
}

namespace QuadProgMm {
  LinearExpression operator + (const Variable& v) {return v;}
  LinearExpression operator + (const LinearExpression& l) {return l;}
  QuadraticExpression operator + (const QuadraticExpression& q) {return q;}

  LinearExpression operator - (const Variable& v) {return v * -1;}
  LinearExpression operator - (const LinearExpression& l) {return l * -1;}
  QuadraticExpression operator - (const QuadraticExpression& q) {return q * -1;}

  LinearExpression operator + (const Variable& l, double r) {return LinearExpression(l) + r;}
  LinearExpression operator + (double l, const Variable& r) {return LinearExpression(l) + r;}
  LinearExpression operator + (const Variable& l, const Variable& r) {return LinearExpression(l) + r;}

  LinearExpression operator - (const Variable& l, double r) {return LinearExpression(l) - r;}
  LinearExpression operator - (double l, const Variable& r) {return LinearExpression(l) - r;}
  LinearExpression operator - (const Variable& l, const Variable& r) {return LinearExpression(l) - r;}

  LinearExpression operator * (const Variable& v, double d) {return d * LinearExpression(v);}
  LinearExpression operator * (double d, const Variable& v) {return d * LinearExpression(v);}

  QuadraticExpression operator * (const LinearExpression& l, const LinearExpression& r) {return QuadraticExpression::multiply(l, r);}

  LinearExpression operator / (const Variable& l, double r) {return LinearExpression(l) / r;}

  Constraint operator == (const LinearExpression& l, const LinearExpression& r) {return Constraint(l - r, Constraint::ZERO);}
  Constraint operator >= (const LinearExpression& l, const LinearExpression& r) {return Constraint(l - r, Constraint::POSITIVE);}
  Constraint operator <= (const LinearExpression& l, const LinearExpression& r) {return Constraint(r - l, Constraint::POSITIVE);}
}


/*
 * 	grasp.hpp
 *
 *	Author(s): Tamas D. Nagy
 *	Created on: 2017-11-08
 *
 *  Grasp the object received in the topic 'saf/vision/target'.
 *
 */

#ifndef GRASP_HPP_
#define GRASP_HPP_

#include <iostream>
#include <sstream>
#include <vector>
#include <map>
#include <cmath>
#include <ros/ros.h>
#include <ros/package.h>
#include <cmath>
#include <limits>

#include <Eigen/Dense>
#include <Eigen/Geometry>

#include <geometry_msgs/Point.h>

#include <irob_utils/tool_pose.hpp>
#include <irob_utils/utils.hpp>
#include <irob_utils/abstract_directions.hpp>
#include <irob_motion/surgeme_client.hpp>
#include <irob_vision_support/vision_client.hpp>

#include <irob_subtask_logic/autosurg_agent.hpp>

namespace saf {

class Grasp : public AutosurgAgent {

protected:

  VisionClient<geometry_msgs::Transform, Eigen::Affine3d> vision;

public:
  Grasp(ros::NodeHandle, ros::NodeHandle priv_nh, std::vector<std::string>);
  ~Grasp();
  void graspObject();

};

}
#endif /* GRASP_HPP_ */


//*****************************************************************
// Copyright 2015 MIT Lincoln Laboratory
// Project:            SPAR
// Authors:            OMD
// Description:        Unit tests for things defined in
//                     variable-delay-query-script.h.
//
// Modifications:
// Date          Name           Modification
// ----          ----           ------------
// 24 Sep 2012   omd            Original Version
//*****************************************************************

#define BOOST_TEST_MODULE DelayGeneratorsTest
#include "common/test-init.h"

#include "delay-generators.h"

#include <boost/function.hpp>
#include <ctime>
#include <memory>
#include <vector>

#include "common/logging.h"

using namespace std;

// Checks that the ExponentialDelayFunctor does indeed return numbers that are,
// in fact, exponentially distributed. The test works as follows:
//
// 1) We generate numbers with a mean of 3 milliseconds until we've seen a total
//    of ten seconds (e.g. the sum of the generated numbers > 10 seconds)
// 2) We count the number of events in the 10 second period.
// 3) If the data were really Poisson we should see, on average, 3,333 events.
// 4) Using a little R to help us (ppois and qpois specifically) we find that
//    the observed number of events will be less than 3062 only 1 in 1 million
//    times and it will be greater than 3611 only 1 in 1 million times.
// 5) Note that if we were generating with a mean of 4 instead of 3 the mean
//    number of observations would be 2500 which is well outside this range.
//    Similarly, if we generated with a mean of 2 instead of 3 the mean would be
//    5000 which is, again, outside this range. Thus, we have a very good chance
//    to catching errors even with this low probability of FA.
BOOST_AUTO_TEST_CASE(ExponentialDelayFunctorDistributionCorrect) {
  int seed = time(NULL);
  LOG(DEBUG) << "Running with seed: " << seed;

  ExponentialDelayFunctor f(3 * kNumMicroSecondsPerMillisecond, seed);

  double total_time = 0;
  const int kNumMicrosInTenSeconds = 10 * kNumMicroSecondsPerSecond;
  int num_events = 0;
  while (true) {
    total_time += f();
    if (total_time > kNumMicrosInTenSeconds) {
      break;
    } else {
      ++num_events;
    }
  }

  // See comments
  BOOST_CHECK_LT(num_events, 3611);
  BOOST_CHECK_GT(num_events, 3062);
}

// Test that the generated numbers are the same if we generate them twice with
// the same seed.
BOOST_AUTO_TEST_CASE(ExponentialDelayFunctorStableSeed) {
  const int kSeed = 11098234;
  ExponentialDelayFunctor f1(10 * kNumMicroSecondsPerMillisecond, kSeed);

  // Probability of getting 10 identical values if they are different is
  // essentially 0.
  const int kNumValsToGenerate = 10;

  std::vector<int> f1_generated;
  for (int i = 0; i < kNumValsToGenerate; ++i) {
    f1_generated.push_back(f1());
  }

  ExponentialDelayFunctor f2(10 * kNumMicroSecondsPerMillisecond, kSeed);
  std::vector<int> f2_generated;
  for (int i = 0; i < kNumValsToGenerate; ++i) {
    f2_generated.push_back(f2());
  }

  for (int i = 0; i < kNumValsToGenerate; ++i) {
    BOOST_CHECK_EQUAL(f1_generated[i], f2_generated[i]);
  }

}

// We pass these functors by value. I was getting strange memory errors so this
// test checks that doing this is valid.
BOOST_AUTO_TEST_CASE(ExponentialDelayFunctorCopyable) {
  auto_ptr<ExponentialDelayFunctor> f1(
      new ExponentialDelayFunctor(10000, 1342));
  BOOST_CHECK_GE((*f1)(), 0);
  // Will eventually copy f2 into f3
  boost::function<int ()> f3;

  {
    boost::function<int ()> f2 = *f1;
    BOOST_CHECK_GE(f2(), 0);

    f3 = f2;
    BOOST_CHECK_GE(f3(), 0);

    f1.reset();
    BOOST_CHECK_GE(f2(), 0);
    BOOST_CHECK_GE(f3(), 0);
  }
  // Now f2 is out of scope, but f3 should still be callable.
  BOOST_CHECK_GE(f3(), 0);
}


//---------------------------------------------------------------------------//
//!
//! \file   Utility_AtomUnit.hpp
//! \author Alex Robinson
//! \brief  The atom unit declaration
//!
//---------------------------------------------------------------------------//

#ifndef UTILITY_ATOM_UNIT_HPP
#define UTILITY_ATOM_UNIT_HPP

// Boost Includes
#include <boost/units/systems/si/amount.hpp>
#include <boost/units/base_unit.hpp>
#include <boost/units/conversion.hpp>
#include <boost/units/io.hpp>

// FRENSIE Includes
#include "Utility_RawPhysicalConstants.hpp"

namespace Utility{

namespace Units{

//! The atom base unit
struct AtomBaseUnit : public boost::units::base_unit<AtomBaseUnit, boost::units::amount_dimension,2>
{
  static const char* name() { return "atom"; }
  static const char* symbol() { return "atom"; }
};

//! The atom unit
typedef AtomBaseUnit::unit_type Atom;

BOOST_UNITS_STATIC_CONSTANT( atom, Atom );

} // end Units namespace

} // end Utility namespace

BOOST_UNITS_DEFINE_CONVERSION_FACTOR( Utility::Units::AtomBaseUnit, boost::units::si::amount, double, 1.0/Utility::RawPhysicalConstants::avogadro_constant );
BOOST_UNITS_DEFINE_CONVERSION_FACTOR( boost::units::si::amount, Utility::Units::AtomBaseUnit, double, Utility::RawPhysicalConstants::avogadro_constant );

BOOST_UNITS_DEFAULT_CONVERSION( Utility::Units::AtomBaseUnit, boost::units::si::amount );

#endif // end UTILITY_ATOM_UNIT_HPP

//---------------------------------------------------------------------------//
// end Utility_AtomUnit.hpp
//---------------------------------------------------------------------------//


// Copyright 2015 The ALMA Project Developers
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//   http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or
// implied. See the License for the specific language governing
// permissions and limitations under the License.

/// @file
/// Writes the distribution in files.

#include <iostream>
#include <fstream>
#include <algorithm>
#include <boost/filesystem.hpp>
#include <boost/mpi.hpp>
#include <Eigen/Dense>
#include <Eigen/Sparse>
#include <cmakevars.hpp>
#include <constants.hpp>
#include <utilities.hpp>
#include <structures.hpp>
#include <bulk_hdf5.hpp>
#include <isotopic_scattering.hpp>
#include <io_utils.hpp>
#include <msgpack.hpp>
#include <geometry_2d.hpp>
#include <boost/math/distributions/gamma.hpp>
#include <boost/property_tree/xml_parser.hpp>
#include <boost/property_tree/ptree.hpp>

///Some alias
using gridData = std::unordered_map<std::string,
        std::unique_ptr<alma::Gamma_grid>>;
using cellData = std::unordered_map<std::string,
        std::unique_ptr<alma::Crystal_structure>>;

/// Probability density function for a Gamma distribution.
///
/// @param[in] k - shape parameter
/// @param[in] theta - scale parameter
/// @param[in] x - point at which to evaluate the function
/// @return the value of the pdf
inline double gamma_pdf(double k, double theta, double x) {
    return boost::math::gamma_p_derivative(k, x / theta) / theta;
}
///To store geometric and input data:
///Parameters of input file
struct input_parameters {

    ///Geometry
    std::vector<alma::geometry_2d>
        system;
    ///Material data
    cellData system_cell;
    gridData system_grid;

    ///Vector of thickness
    std::vector<double>
        thicknesses;
    ///Where to print data
    std::vector<std::size_t> boxesID;
    std::vector<double>      dumptimes;
    ///nbins
    std::size_t nbins;
};

///To read input file:
///This reads the input file
///@param[in] filename - input filename with path
///@param[in] world    - mpi communicator
///@return structure with input parameters
input_parameters
process_input(std::string& filename,
              boost::mpi::communicator& world
){
    // Create empty property tree object
    boost::property_tree::ptree tree;

    // Parse XML input file into the tree
    boost::property_tree::read_xml(filename, tree);

    input_parameters inpars;
    ///Map to store thickness
    std::map<std::string,double> zmap;

    for (const auto& v : tree.get_child("beRTAMC2D")) {
        ///Reading geometry
        if (v.first == "geometry") {
            std::string gfname =
                alma::parseXMLfield<std::string>(v, "file");
            inpars.system =
                alma::read_geometry_XML(gfname);
        }
        if (v.first == "material") {
            std::string name =
                alma::parseXMLfield<std::string>(v, "name");
            std::string hdf5file =
                alma::parseXMLfield<std::string>(v, "database");
            double zsize =
                alma::parseXMLfield<double>(v, "thickness");
            auto data =
                alma::load_bulk_hdf5(hdf5file.c_str(), world);
            inpars.system_cell[name] =
                std::move(std::get<1>(data));
            inpars.system_grid[name] =
                std::move(std::get<3>(data));

            zmap[name] = zsize;
        }

        if (v.first == "spectral") {
            for (auto it = v.second.begin(); it != v.second.end(); it++) {
                if (it->first == "resolution") {
                    inpars.nbins =
                        alma::parseXMLfield<std::size_t>(*it, "ticks");
                }
                if (it->first == "location") {
                    inpars.boxesID.push_back(
                        alma::parseXMLfield<std::size_t>(*it, "bin"));
                }
                if (it->first == "time") {
                    inpars.dumptimes.push_back(
                        alma::parseXMLfield<double>(*it, "t"));
                }
            }
        }

    }

    if (inpars.system_cell.size()>1){
        std::cout << "This is currently not supported\n";
        world.abort(1);
    }

    /// Fill thicknesses
    for (std::size_t i=0;i<inpars.system.size();i++) {
        auto mat = inpars.system[i].material;
        inpars.thicknesses.push_back(
            zmap[mat]);
    }

    /// Order the times
    std::sort(inpars.dumptimes.begin(),inpars.dumptimes.end());

    return inpars;
}

class data {
public:
    double time;
    double Eeff;
    std::vector<double> temperatures;
    std::vector<Eigen::VectorXi> histograms;
    std::vector<std::size_t> nbands;
    std::vector<std::ofstream> outf;
    std::vector<std::ofstream> outfvx;
    std::vector<std::ofstream> outfvy;
    std::vector<std::ofstream> outfd;

    data(input_parameters& inpars){
        temperatures.resize(inpars.system.size());
        histograms.resize(temperatures.size());
        nbands.resize(temperatures.size(),0);
        outf.resize(nbands.size());
        outfvx.resize(nbands.size());
        outfvy.resize(nbands.size());
        outfd.resize(nbands.size());

        for (auto s : inpars.system) {

            auto id = s.get_id();

            if (std::find(inpars.boxesID.begin(),
                inpars.boxesID.end(),id)==inpars.boxesID.end())
                continue;

            outf[id].open("deltaT_omega_"+std::to_string(id)+".csv");
            outfvx[id].open("jx_omega_"+std::to_string(id)+".csv");
            outfvy[id].open("jy_omega_"+std::to_string(id)+".csv");
            outfd[id].open("fd_"+std::to_string(id)+".csv");

            auto mat = inpars.system[id].material;

            auto nqpoints = inpars.system_grid[mat]->nqpoints;

            auto nbands_   = inpars.system_grid[mat
                ]->get_spectrum_at_q(0).omega.size();

            histograms[id].resize(nbands_*nqpoints);
            histograms[id].setZero();
            nbands[id] = nbands_;
        }
    }

    ~data(){
        for (auto& o : outf) {
            if(o.is_open())
                o.close();
        }
        for (auto& o : outfvx) {
            if(o.is_open())
                o.close();
        }
        for (auto& o : outfvy) {
            if(o.is_open())
                o.close();
        }
        for (auto& o : outfd) {
            if(o.is_open())
                o.close();
        }

    }

  // this function is looks like de-serializer, taking an msgpack object
  // and extracting data from it to the current class fields
  void msgpack_unpack(msgpack::object o) {

    // check if received structure is an array
    if(o.type != msgpack::type::ARRAY) { throw msgpack::type_error(); }

    o.via.array.ptr[0].convert(time);
    o.via.array.ptr[1].convert(Eeff);

    std::cout << "time " << time << std::endl;
    std::cout << "Eeff " << Eeff << std::endl;

    // extract value of second array entry which is array itself:
    for (std::size_t i = 0; i < temperatures.size() ; i++) {
      o.via.array.ptr[2].via.array.ptr[i].convert((temperatures[i]));
    }
    for (std::size_t i = 0; i < temperatures.size(); i++) {
        if (nbands[i] == 0)
            continue;
        std::size_t nelements;
        o.via.array.ptr[3+2*i].convert(nelements);

        std::cout << "nelements " << nelements << std::endl;

        histograms[i].setZero();

        for (std::size_t j = 0; j < nelements/2; j++) {
            std::size_t imode;
            o.via.array.ptr[4+2*i].via.array.ptr[2*j].convert(imode);
            o.via.array.ptr[4+2*i].via.array.ptr[2*j+1].convert((histograms[i](imode)));
        }
    }

  }

  // destination of this function is unknown - i've never ran into scenary
  // what it was called. some explaination/documentation needed.
  template <typename MSGPACK_OBJECT>
  void msgpack_object(MSGPACK_OBJECT* o, msgpack::zone* z) const {

  }

  /// This prints the distribution
  ///@param[in] inpar - input parameters
  double print_distribution(input_parameters& inpars){
      static std::map<std::string,Eigen::ArrayXXd> sigmas;  //Broadening
      static std::map<std::string,Eigen::ArrayXXd> vx;
      static std::map<std::string,Eigen::ArrayXXd> vy;
      static std::map<std::string,Eigen::MatrixXd> qpoints1stBZ; //Q points in the first BZ
      static std::map<std::string,double> Cvtot;      //specific heat of each material [J/(nm**3 * K)]
      static std::map<std::string,Eigen::VectorXd> mesh;
      static std::vector<bool> first(temperatures.size(),true);

      /// If not in list ignore
      if (std::find_if(inpars.dumptimes.begin(),inpars.dumptimes.end(),[&](double &t_){
          return alma::almost_equal(time,t_);
      })==inpars.dumptimes.end()){
          return -1.0;
      }

      /// Filling tables

      if (sigmas.empty()) {

        for (std::size_t ibox = 0; ibox < temperatures.size(); ibox++) {
            if (nbands[ibox]==0) {
                continue;
            }
            //std::cout << "Printing " << ibox << std::endl;
            auto mat = inpars.system[ibox].material;
            if (sigmas.count(mat) !=0)
                    continue;
            auto tbox = temperatures[ibox];
            sigmas[mat].resize(nbands[ibox],inpars.system_grid[mat]->nqpoints);
            vx[mat].resize(nbands[ibox],inpars.system_grid[mat]->nqpoints);
            vy[mat].resize(nbands[ibox],inpars.system_grid[mat]->nqpoints);
            qpoints1stBZ[mat].resize(inpars.system_grid[mat]->nqpoints,2);
            sigmas[mat].setZero();
            vx[mat].setZero();
            vy[mat].setZero();
            qpoints1stBZ[mat].setZero();
            double maxf = 0.;

            double prefactor =
                alma::constants::kB / (inpars.system_cell[mat]->V * inpars.thicknesses[ibox]
                /inpars.system_cell[mat]->lattvec(2,2)) / inpars.system_grid[mat]->nqpoints;

            for (std::size_t iq = 0; iq < inpars.system_grid[mat]->nqpoints; ++iq) {
                    auto spectrum = inpars.system_grid[mat]->get_spectrum_at_q(iq);

                    auto q1BZ = inpars.system_cell[mat]->map_to_firstbz(
                        inpars.system_grid[mat]->get_q(iq));

                    qpoints1stBZ[mat](iq,0) = q1BZ.row(0).mean();
                    qpoints1stBZ[mat](iq,1) = q1BZ.row(1).mean();

                    auto maxomega_mode =
                        spectrum.omega.maxCoeff();
                    if (maxomega_mode > maxf)
                        maxf = maxomega_mode;

                    for (std::size_t im = 0; im < nbands[ibox]; ++im) {
                        auto omega_ = spectrum.omega[im];

                        if (alma::almost_equal(omega_,0.))
                            continue;

                        sigmas[mat](im, iq) = inpars.system_grid[
                            mat]->base_sigma(spectrum.vg.col(im));
                        ///Get group velocities in m/s
                        vx[mat](im, iq) = 1.0e+3 * spectrum.vg(0,im);
                        vy[mat](im, iq) = 1.0e+3 * spectrum.vg(1,im);
                    }
            }
            // And refine them by removing outliers.
            auto percent = alma::calc_percentiles_log(sigmas[mat]);
            double lbound = std::exp(percent[0] - 1.5 * (percent[1] - percent[0]));
            sigmas[mat] = (sigmas[mat] < lbound).select(lbound, sigmas[mat]);

            mesh[mat] = Eigen::VectorXd::LinSpaced(inpars.nbins,0.,maxf);
            mesh[mat] = (mesh[mat].array() + mesh[mat](1)/2.0).matrix();

            /// Filling volumetric heat capacity
            Cvtot[mat] = 0.;

            for (std::size_t iq = 0; iq < inpars.system_grid[mat]->nqpoints; ++iq) {
                auto spectrum = inpars.system_grid[mat]->get_spectrum_at_q(iq);
                for (std::size_t im = 0; im < nbands[ibox]; ++im) {
                    auto omega_ = spectrum.omega[im];

                    if (alma::almost_equal(0.,omega_))
                        continue;

                    Cvtot[mat] += alma::bose_einstein_kernel(omega_, tbox);
                }
            }
            Cvtot[mat] *= prefactor;
        }
      }

      for (std::size_t ibox = 0; ibox < temperatures.size(); ibox++) {

          ///If not in print list ignore
          if (std::find(inpars.boxesID.begin(),
              inpars.boxesID.end(),ibox)==inpars.boxesID.end())
              continue;

          auto mat = inpars.system[ibox].material;
          if (inpars.system[ibox].reservoir
              or inpars.system[ibox].reservoir)
            continue;

          Eigen::VectorXd d(mesh[mat]), dvx(mesh[mat]), dvy(mesh[mat]);
          Eigen::VectorXd dfd(inpars.system_grid[mat]->nqpoints);

          d.setZero();
          dvx.setZero();
          dvy.setZero();
          dfd.setZero();

          auto vol  = inpars.system[ibox].get_area()*inpars.thicknesses[ibox];
          double Ebox = 0.;

          for (std::size_t imode = 0; imode < static_cast<std::size_t>(
                histograms[ibox].rows()); imode++){
              auto value = histograms[ibox](imode);
              if (value == 0) {
                  continue;
              }
              auto ib = imode % nbands[ibox];
              auto iq = imode / nbands[ibox];
              auto omega = inpars.system_grid[mat]->get_spectrum_at_q(iq).omega(ib);
              dfd(iq) += value/omega;
              auto sigma = sigmas[mat](ib,iq);
              Eigen::VectorXd v = inpars.system_grid[mat]->get_spectrum_at_q(iq).vg.col(ib).matrix();
              Ebox  += value;
              ///If gamma they do not contribute
              if (alma::almost_equal(omega,0) or alma::almost_equal(sigma,0))
                  continue;
              for (int ii = 0; ii < d.rows(); ii++ ) {
                  double k = omega * omega / sigma;
                  double theta = sigma / omega;
                  double valati = value*gamma_pdf(k,theta,mesh[mat](ii));
                  d(ii)   += valati;
                  dvx(ii) += valati * 1.0e+3 * v(0);
                  dvy(ii) += valati * 1.0e+3 * v(1);
              }
          }

          ///Recover the distribution function
          dfd   *=   Eeff*(inpars.system_cell[mat]->V * inpars.thicknesses[ibox]
                 / inpars.system_cell[mat]->lattvec(2,2)) *
                inpars.system_grid[mat]->nqpoints / (1.0e+12*vol*alma::constants::hbar);
          dvx      *= Eeff * 1.0e+27/vol  ;
          dvy      *= Eeff * 1.0e+27/vol  ;
          d        *= Eeff/(vol*Cvtot[mat]);
          Ebox     *= Eeff/vol;

          std::cout <<"Energy density of box " << ibox << " is " << Ebox << std::endl;
          std::cout <<"Specific heat " << Cvtot[mat] * 1.0e+27 << " J/(m**3 * K)" << std::endl;

          auto &myo   = outf[ibox];
          auto &myox  = outfvx[ibox];
          auto &myoy  = outfvy[ibox];
          auto &myofd = outfd[ibox];

          if (first[ibox]) {
              myo << -1 << ',';
              myox << -1 << ',';
              myoy << -1 << ',';
              for (int ii = 0; ii < d.rows(); ii++ ) {
                  myo << mesh[mat](ii);
                  myox << mesh[mat](ii);
                  myoy << mesh[mat](ii);
                  if (ii != d.rows()-1) {
                    myo  << ',';
                    myox << ',';
                    myoy << ',';
                  }
              }
              myo << std::endl;
              myox << std::endl;
              myoy << std::endl;

              ///Print coordinates

              myofd << "# ";
              for (int ii = 0; ii < dfd.rows();ii++) {
                 myofd << qpoints1stBZ[mat](ii,0);
                 if (ii != dfd.rows()-1) {
		    myofd << ",";
                 }
              }
              myofd << std::endl;
              myofd << "# ";
              for (int ii = 0; ii < dfd.rows();ii++) {
                  myofd << qpoints1stBZ[mat](ii,1);
                  if (ii != dfd.rows()-1) {
                    myofd << ",";
                 }
              }
              myofd << std::endl;

              first[ibox] = false;
          }

          myo  << time << ',';
          myox << time << ',';
          myoy << time << ',';
          myofd << time << ',';
          for (int ii = 0; ii < d.rows(); ii++ ) {
            myo  << d(ii);
            myox << dvx(ii);
            myoy << dvy(ii);

            if (ii != d.rows()-1) {
                myo  << ',';
                myox << ',';
                myoy << ',';
            }
          }

          for (int ii = 0; ii < dfd.rows();ii++) {
            myofd << dfd(ii);
            if (ii != dfd.rows()-1) {
                myofd << ',';
            }
          }

          myo   << std::endl;
          myox  << std::endl;
          myoy  << std::endl;
          myofd << std::endl;

      }

    return time;
  }

};

int main(int argc, char** argv) {
    boost::mpi::environment env;
    boost::mpi::communicator world;

    if (world.size()!=1) {
        std::cout << "Error: dist_reader cannot be "
                     "run in more than one MPI-process"
                    << std::endl;
        world.abort(1);
    }

    // Reference temperature at which to compute
    // heat capacities and scattering rates.

    // Path to the HDF5 file.
    std::string h5filename;
    std::string inputfilename;
    std::string msgpackfilename;
    std::string reader;

    if (argc != 3) {
        std::cout << "USAGE: dist_reader <input.xml> <histogram.msgpack.bin> "
                  << std::endl;
        world.abort(1);
    }

    inputfilename = std::string(argv[1]);
    msgpackfilename = std::string(argv[2]);

    std::cout << "***********************************" << std::endl;
    std::cout << "This is ALMA/dist_reader version " << ALMA_VERSION_MAJOR << "."
              << ALMA_VERSION_MINOR << std::endl;
    std::cout << "***********************************" << std::endl;

    ///Reading geometry and other things
    std::cout << "Reading " << inputfilename << std::endl;

    auto inpars = process_input(inputfilename,world);

    ///Processing:
    std::cout << "Init data storing" << std::endl;
    data simdata(inpars);

    std::ifstream histdat;

    boost::filesystem::path msgpack_path{msgpackfilename};

    if (!(boost::filesystem::exists(msgpack_path))) {
        std::cout << "ERROR:" << std::endl;
        std::cout << "msgpack file " << msgpack_path << " does not exist."
                  << std::endl;
        world.abort(1);
    }

    histdat.open(msgpackfilename.c_str());

    std::string sizeline;
    ///Each
    std::size_t istep = 0;
    while(true){
        std::getline(histdat,sizeline,'#');
        auto length_block = boost::lexical_cast<std::size_t>(sizeline);
        if (length_block == 0)
            break;

        std::vector<char> dataline(length_block);
        histdat.read(dataline.data(),length_block);
        std::cout << "*Read line " << istep << std::endl;
        msgpack::object_handle oh =
            msgpack::unpack(dataline.data(), dataline.size());
        simdata.msgpack_unpack(oh.get());
        double that_time = simdata.print_distribution(inpars);

        /// Stop if last time has been read
        if (alma::almost_equal(that_time,inpars.dumptimes.back())){
            break;
        }

        istep++;
    }

    histdat.close();

    return 0;
}


//==================================================================================================
/*!
  @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_FUNCTION_MINIMUM_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_MINIMUM_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

  /*!
    @ingroup group-reduction
   This function object returns the smallest element of the argument

    @par Header <boost/simd/function/minimum.hpp>

    @see maximum

    @par Example:

      @snippet minimum.cpp minimum

    @par Possible output:

      @snippet minimum.txt minimum

  **/
  scalar_of_t<Value> minimum(Value const& x);
} }
#endif

#include <boost/simd/function/scalar/minimum.hpp>
#include <boost/simd/function/simd/minimum.hpp>

#endif


#pragma once

#include <stack>
#include <queue>
#include <vector>
#include <Eigen/Dense>
#include <memory>
#include <cmath>
#include <limits>
#include <cstdlib>
#include <time.h>
#include <string>
#include <map>
#include <queue>
#include <algorithm>
#include <iostream>
#include <fstream>

// ompl
#include "ompl/datastructures/NearestNeighbors.h"
#include <ompl/datastructures/BinaryHeap.h>
#include "ompl/tools/config/SelfConfig.h"
#include "ompl/datastructures/NearestNeighborsGNAT.h"
#include <ompl/base/SpaceInformation.h>
#include <ompl/base/spaces/SE3StateSpace.h>
#include <ompl/config.h>

// ros
#include <ros/ros.h>
#include <nav_msgs/Odometry.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/Point.h>
#include <std_msgs/Float64.h>
#include <std_msgs/Bool.h>
#include <std_msgs/Empty.h>

// optimized motion planner
#include "motion_planning/RRTX_node.hpp"
#include "motion_planning/dynamic_obstacle.hpp"
#include "optimizer/CHOMP_dynamic.hpp"
#include "optimizer/CHOMP_dynamic_trajectory.hpp"

#include "utils/utility_functions.hpp"
#include "utils/optimizer_params.hpp"
#include "utils/types.hpp"
#include <optimized_motion_planner/obstacle_info.h>
#include <optimized_motion_planner/point_array.h>

namespace optimized_motion_planner {

class RRTX {
public:
	RRTX(const ros::NodeHandle& nh, const ros::NodeHandle& pnh);
	~RRTX();

	// debug
	double get_nn_size();
	void check_params();

private:
	// debug
	int debug{0};
	int iteration_count{0};
	bool if_find_solution_{false};
	std::ofstream solve_time_path_file;
	std::ofstream optimize_time_path_file;
	std::ofstream drone_actual_path_file;
	void visualize_node_tree();
	bool with_chomp_{true};
	double get_distance_check_ob(const std::shared_ptr<RRTXNode>& node1, const std::shared_ptr<RRTXNode>& node2);

	// drone
	double quadrotor_radius_;
	double quadrotor_speed_;

	// time dimension
	double total_planning_time_;
	double generate_random_time(const Eigen::Vector3d& state);
	ros::WallTime start_time_;
	double planning_horizon_time_;

	// ros nodes
	ros::NodeHandle nh_;
	ros::NodeHandle pnh_;

	// rrt nodes
	std::shared_ptr<RRTXNode> start_;
	std::shared_ptr<RRTXNode> goal_;

	// parameter object
	std::shared_ptr<optimized_motion_planner_utils::OptimizerParams> optimizer_params_;
	void set_optimizer_params();

	// geometric Near-neighbor Access Tree
	std::shared_ptr<ompl::NearestNeighborsGNAT<std::shared_ptr<RRTXNode>>> nearest_neighbors_tree_;

	// setup
	bool load_params();
	void setup();
	int dimension_{3};

	// subscriber
	ros::Subscriber sub_solve_trigger_;
	void solve_trigger_callback(const std_msgs::Empty::ConstPtr &msg);
	ros::Subscriber sub_obstacle_info_;
	void obstacle_info_callback(const optimized_motion_planner::obstacle_info::ConstPtr &msg);
	ros::Subscriber sub_dynamic_obstacle_movement_;
	void dynamic_obstacle_movement_callback(const optimized_motion_planner::obstacle_info::ConstPtr &msg);

	// publisher
	ros::Publisher pub_planned_trajectory_;

	// main iteration
	bool solve();
	void saturate(std::shared_ptr<RRTXNode> random_node, const std::shared_ptr<RRTXNode>& nearest_node, double distance) const;
	bool node_in_free_space_check(const std::shared_ptr<RRTXNode>& random_node);
	bool edge_in_free_space_check(const std::shared_ptr<RRTXNode>& node1, const std::shared_ptr<RRTXNode>& node2);
	bool extend(std::shared_ptr<RRTXNode> random_node);

	// RRTX node
	int node_index_{0};
	std::vector<std::shared_ptr<RRTXNode>> node_list_;
	void add_node_to_nearest_neighbors_tree(const std::shared_ptr<RRTXNode>& node);
	std::shared_ptr<RRTXNode> generate_random_node();

	// for calculate the shrinking ball radius
	void calculateRRG();
	double max_distance_{1}; // the maximum length of a motion to be added to a tree
	double r_rrt_{5}; // a constant for r-disc rewiring calculations
	double rrg_r_{3}; // current value of the radius used for the neighbors

	// extend related
	void update_neighbors_list(std::shared_ptr<RRTXNode> random_node);
	bool find_best_parent(std::shared_ptr<RRTXNode> random_node);

	// cost related members
	double combine_cost(double cost1, double cost2);
	bool is_cost_better_than(double cost1, double cost2);

	// rewire
	double epsilon_{0.5};
	ompl::BinaryHeap<std::shared_ptr<RRTXNode>, RRTXNode::node_compare> node_queue;
	void rewire_neighbors_v1(std::shared_ptr<RRTXNode> random_node);
	void rewire_neighbors_v2(std::shared_ptr<RRTXNode> random_node);
	void cull_neighbors(std::shared_ptr<RRTXNode> random_node);
	void verify_queue(std::shared_ptr<RRTXNode> node);
	void reduce_inconsistency_v1();
	void reduce_inconsistency_v2();
	void update_lmc_v1(std::shared_ptr<RRTXNode> node);
	void update_lmc_v2(std::shared_ptr<RRTXNode> node);

	// find the solution path
	std::vector<Eigen::Vector3d> solution_path;
	std::vector<Eigen::Vector3d> solution_path_tmp;
	bool update_solution_path();
	void publish_without_optimizer();
	void publish_with_optimizer();

	// environment update
	void update_obstacle();
	std::map<std::string, std::shared_ptr<Obstacle>> obstacle_map;
	std::vector<std::shared_ptr<Obstacle>> obstacle_update_info_list;
	void remove_obstacle(const std::shared_ptr<Obstacle>& obstacle);
	void add_obstacle(const std::shared_ptr<Obstacle>& obstacle);
	void propogate_descendants();
	bool check_if_node_inside_obstacle(const std::shared_ptr<Obstacle>& obstacle, const std::shared_ptr<RRTXNode>& node);
	bool check_if_node_inside_obstacle_for_adding_ob(const std::shared_ptr<Obstacle>& obstacle, const std::shared_ptr<RRTXNode>& node);
	bool check_if_node_inside_all_obstacles(const std::shared_ptr<RRTXNode>& node, bool consider_dynamic_obstacle);
	std::vector<std::shared_ptr<RRTXNode>> orphan_node_list;
	void verify_orphan(std::shared_ptr<RRTXNode> node);
	bool check_if_node_in_orphan_list(const std::shared_ptr<RRTXNode>& node);
	bool if_environment_change{false};

	// dynamic obstacle
	std::map<int, std::shared_ptr<DynamicObstacle>> dynamic_obstacle_map_tmp_;
	std::map<int, std::shared_ptr<DynamicObstacle>> dynamic_obstacle_map_;
	std::vector<int> dynamic_obstacle_id_list_;
	int get_dynamic_obstacle_id(const std::string& dynamic_obstacle_name);
	bool exist_dynamic_obstacle_{false};
	int dynamic_obstacle_callback_count{0};
	void add_dynamic_obstacle(int dynamic_obstacle_id);
	bool if_add_dynamic_obstacle_{false};

	// utils
	void make_parent_of(std::shared_ptr<RRTXNode> parent_node, std::shared_ptr<RRTXNode> node);
	double discretization_;
	double min_clearence_;

	// path write to file name
	int path_file_num_{0};
	int chomp_path_file_num_{0};
};

}


#ifndef __HEAD_POSE_ESTIMATION
#define __HEAD_POSE_ESTIMATION

#include <opencv2/core/core.hpp>
#include <dlib/opencv.h>
#include <dlib/image_processing.h>
#include <dlib/image_processing/frontal_face_detector.h>

#include <vector>
#include <array>
#include <string>

const static cv::Point3f P3D_SELLION(0., 0.,0.);
const static cv::Point3f P3D_RIGHT_EYE(-20., -65.5,-5.);
const static cv::Point3f P3D_LEFT_EYE(-20., 65.5,-5.);
const static cv::Point3f P3D_RIGHT_EAR(-100., -77.5,-6.);
const static cv::Point3f P3D_LEFT_EAR(-100., 77.5,-6.);
const static cv::Point3f P3D_NOSE(21.0, 0., -48.0);
const static cv::Point3f P3D_STOMMION(10.0, 0., -75.0);
const static cv::Point3f P3D_MENTON(0., 0.,-133.0);

static const int MAX_FEATURES_TO_TRACK=100;

// Interesting facial features with their landmark index
enum FACIAL_FEATURE {
    NOSE=30,
    RIGHT_EYE=36,
    LEFT_EYE=45,
    RIGHT_SIDE=0,
    LEFT_SIDE=16,
    EYEBROW_RIGHT=21,
    EYEBROW_LEFT=22,
    MOUTH_UP=51,
    MOUTH_DOWN=57,
    MOUTH_RIGHT=48,
    MOUTH_LEFT=54,
    SELLION=27,
    MOUTH_CENTER_TOP=62,
    MOUTH_CENTER_BOTTOM=66,
    MENTON=8
};

const static int MODE_ITERATIVE = 0;
const static int MODE_P3P = 1;
const static int MODE_EPNP = 2;

typedef cv::Matx44d head_pose;

class HeadPoseEstimation {

public:
    HeadPoseEstimation(const std::string& face_detection_model = "shape_predictor_68_face_landmarks.dat",
        int mode=MODE_ITERATIVE,
        float fx = 0,
        float fy = 0,
        float cx = 0,
        float cy = 0,
        float k1 = 0,
        float k2 = 0,
        float p1 = 0,
        float p2 = 0,
        float k3 = 0);

    int detect(cv::Mat& image);

    head_pose pose(size_t face_idx) const;

    std::vector<head_pose> poses() const;

    virtual inline double todeg(double rad) {  return rad * 180 / M_PI; }

    cv::Matx33f cameraMatrix;
    /*
        cameraMatrix – Input camera matrix  A =   [{fx},  {0},    {cx},
                                                {0},    {fy},   {cy},
                                                {0},    {0},    {1}]
    */
    cv::Mat1f distCoeffs;
    /*
        distCoeffs – Input vector of distortion coefficients (k_1, k_2, p_1, p_2[, k_3[, k_4, k_5, k_6],[s_1, s_2, s_3, s_4]])
    */

    mutable cv::Mat resultMat;

    int mode;

private:
    dlib::cv_image<dlib::bgr_pixel> current_image;

    dlib::frontal_face_detector detector;
    dlib::shape_predictor pose_model;

    std::vector<dlib::rectangle> faces;

    std::vector<dlib::full_object_detection> shapes;

    /** Return the point corresponding to the dictionary marker.
    */
    cv::Point2f coordsOf(size_t face_idx, FACIAL_FEATURE feature) const;

    /** Returns true if the lines intersect (and set r to the intersection
     *  coordinates), false otherwise.
     */
    bool intersection(cv::Point2f o1, cv::Point2f p1,
                      cv::Point2f o2, cv::Point2f p2,
                      cv::Point2f &r) const;
};

#endif // __HEAD_POSE_ESTIMATION


#include <ripple/basics/hardened_hash.h>
#include <ripple/beast/unit_test.h>
#include <boost/functional/hash.hpp>
#include <array>
#include <cstdint>
#include <iomanip>
#include <functional>
#include <unordered_map>
#include <unordered_set>

namespace ripple {
namespace detail {

template <class T>
class test_user_type_member
{
private:
    T t;

public:
    explicit test_user_type_member (T const& t_ = T())
        : t (t_)
    {
    }

    template <class Hasher>
    friend void hash_append (Hasher& h, test_user_type_member const& a) noexcept
    {
        using beast::hash_append;
        hash_append (h, a.t);
    }
};

template <class T>
class test_user_type_free
{
private:
    T t;

public:
    explicit test_user_type_free (T const& t_ = T())
        : t (t_)
    {
    }

    template <class Hasher>
    friend void hash_append (Hasher& h, test_user_type_free const& a) noexcept
    {
        using beast::hash_append;
        hash_append (h, a.t);
    }
};

}
}

namespace ripple {

namespace detail {

template <class T>
using test_hardened_unordered_set =
    std::unordered_set <T, hardened_hash <>>;

template <class T>
using test_hardened_unordered_map =
    std::unordered_map <T, int, hardened_hash <>>;

template <class T>
using test_hardened_unordered_multiset =
    std::unordered_multiset <T, hardened_hash <>>;

template <class T>
using test_hardened_unordered_multimap =
    std::unordered_multimap <T, int, hardened_hash <>>;

}

template <std::size_t Bits, class UInt = std::uint64_t>
class unsigned_integer
{
private:
    static_assert (std::is_integral<UInt>::value &&
        std::is_unsigned <UInt>::value,
            "UInt must be an unsigned integral type");

    static_assert (Bits%(8*sizeof(UInt))==0,
        "Bits must be a multiple of 8*sizeof(UInt)");

    static_assert (Bits >= (8*sizeof(UInt)),
        "Bits must be at least 8*sizeof(UInt)");

    static std::size_t const size = Bits/(8*sizeof(UInt));

    std::array <UInt, size> m_vec;

public:
    using value_type = UInt;

    static std::size_t const bits = Bits;
    static std::size_t const bytes = bits / 8;

    template <class Int>
    static
    unsigned_integer
    from_number (Int v)
    {
        unsigned_integer result;
        for (std::size_t i (1); i < size; ++i)
            result.m_vec [i] = 0;
        result.m_vec[0] = v;
        return result;
    }

    void*
    data() noexcept
    {
        return &m_vec[0];
    }

    void const*
    data() const noexcept
    {
        return &m_vec[0];
    }

    template <class Hasher>
    friend void hash_append(Hasher& h, unsigned_integer const& a) noexcept
    {
        using beast::hash_append;
        hash_append (h, a.m_vec);
    }

    friend
    std::ostream&
    operator<< (std::ostream& s, unsigned_integer const& v)
    {
        for (std::size_t i (0); i < size; ++i)
            s <<
                std::hex <<
                std::setfill ('0') <<
                std::setw (2*sizeof(UInt)) <<
                v.m_vec[i]
                ;
        return s;
    }
};

using sha256_t = unsigned_integer <256, std::size_t>;

#ifndef __INTELLISENSE__
static_assert (sha256_t::bits == 256,
    "sha256_t must have 256 bits");
#endif

}

namespace ripple {

class hardened_hash_test
    : public beast::unit_test::suite
{
public:
    template <class T>
    void
    check ()
    {
        T t{};
        hardened_hash <>() (t);
        pass();
    }

    template <template <class T> class U>
    void
    check_user_type()
    {
        check <U <bool>> ();
        check <U <char>> ();
        check <U <signed char>> ();
        check <U <unsigned char>> ();
        check <U <wchar_t>> ();
        check <U <short>> ();
        check <U <unsigned short>> ();
        check <U <int>> ();
        check <U <unsigned int>> ();
        check <U <long>> ();
        check <U <long long>> ();
        check <U <unsigned long>> ();
        check <U <unsigned long long>> ();
        check <U <float>> ();
        check <U <double>> ();
        check <U <long double>> ();
    }

    template <template <class T> class C >
    void
    check_container()
    {
        {
            C <detail::test_user_type_member <std::string>> c;
        }

        pass();

        {
            C <detail::test_user_type_free <std::string>> c;
        }

        pass();
    }

    void
    test_user_types()
    {
        testcase ("user types");
        check_user_type <detail::test_user_type_member> ();
        check_user_type <detail::test_user_type_free> ();
    }

    void
    test_containers()
    {
        testcase ("containers");
        check_container <detail::test_hardened_unordered_set>();
        check_container <detail::test_hardened_unordered_map>();
        check_container <detail::test_hardened_unordered_multiset>();
        check_container <detail::test_hardened_unordered_multimap>();
    }

    void
    run () override
    {
        test_user_types();
        test_containers();
    }
};

BEAST_DEFINE_TESTSUITE(hardened_hash,basics,ripple);

}


/*********************************************************************************
 *  OKVIS - Open Keyframe-based Visual-Inertial SLAM
 *  Copyright (c) 2015, Autonomous Systems Lab / ETH Zurich
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions are met:
 *
 *   * Redistributions of source code must retain the above copyright notice,
 *     this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above copyright notice,
 *     this list of conditions and the following disclaimer in the documentation
 *     and/or other materials provided with the distribution.
 *   * Neither the name of Autonomous Systems Lab / ETH Zurich nor the names of
 *     its contributors may be used to endorse or promote products derived from
 *     this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 *  AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 *  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 *  ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
 *  LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 *  CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 *  SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 *  INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 *  CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 *  ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *
 *  Created on: Jul 28, 2015
 *      Author: Stefan Leutenegger (s.leutenegger@imperial.ac.uk)
 *********************************************************************************/

/**
 * @file cameras/RadialTangentialDistortion8.hpp
 * @brief Header file for the RadialTangentialDistortion class.
 * @author Stefan Leutenegger
 */

#ifndef INCLUDE_VIO_CAMERAS_RADIALTANGENTIALDISTORTION8_HPP_
#define INCLUDE_VIO_CAMERAS_RADIALTANGENTIALDISTORTION8_HPP_

#include <Eigen/Core>
#include <memory>
#include <string>
#include <vector>
#include "DistortionBase.hpp"

/// \brief vio Main namespace of this package.
namespace vio {
/// \brief cameras Namespace for camera-related functionality.
namespace cameras {

class RadialTangentialDistortion8 : public DistortionBase {
 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  /// \brief The default constructor with all zero ki
  inline RadialTangentialDistortion8();

  /// \brief Constructor initialising ki
  /// @param[in] k1 radial parameter 1
  /// @param[in] k2 radial parameter 2
  /// @param[in] p1 tangential parameter 1
  /// @param[in] p2 tangential parameter 2
  /// @param[in] k3 radial parameter 3
  /// @param[in] k4 radial parameter 4
  /// @param[in] k5 radial parameter 5
  /// @param[in] k6 radial parameter 6
  inline RadialTangentialDistortion8(float k1, float k2, float p1, float p2,
                                     float k3, float k4, float k5,
                                     float k6);

  //////////////////////////////////////////////////////////////
  /// \name Methods related to generic parameters
  /// @{

  /// \brief set the generic parameters
  /// @param[in] parameters Parameter vector -- length must correspond numDistortionIntrinsics().
  /// @return    True if the requirements were followed.
  inline bool setParameters(const Eigen::VectorXd & parameters) override;

  /// \brief Obtain the generic parameters.
  inline bool getParameters(Eigen::VectorXd & parameters) const override {
    parameters = parameters_.cast<double>();
    return true;
  }

  /// \brief The class type.
  inline std::string type() const override {
    return "RadialTangentialDistortion8";
  }

  /// \brief Number of distortion parameters
  inline int numDistortionIntrinsics() const override {
    return NumDistortionIntrinsics;
  }

  static const int NumDistortionIntrinsics = 8;  ///< The Number of distortion parameters.
  /// @}

  /// \brief Unit test support -- create a test distortion object
  /// The distortion coefficients are copied from DUO MLX
  static std::shared_ptr<DistortionBase> createTestObject() {
    return std::shared_ptr<DistortionBase>(
        new RadialTangentialDistortion8(2.710e+01, 5.254e+01, -1.800e-03, -4.265e-04,
                                        5.976e+00, 2.751e+01, 6.474e+01, 2.519e+01));
  }
  /// \brief Unit test support -- create a test distortion object
  /// The distortion coefficients are copied from DUO MLX
  static RadialTangentialDistortion8 testObject() {
    return RadialTangentialDistortion8(2.710e+01, 5.254e+01, -1.800e-03, -4.265e-04,
                                       5.976e+00, 2.751e+01, 6.474e+01, 2.519e+01);
  }

  //////////////////////////////////////////////////////////////
  /// \name Distortion functions
  /// @{

  /// \brief Distortion only
  /// @param[in]  pointUndistorted The undistorted normalised (!) image point.
  /// @param[out] pointDistorted   The distorted normalised (!) image point.
  /// @return     True on success (no singularity)
  template<typename T> inline bool distortT(const Eigen::Matrix<T, 2, 1> & pointUndistorted,
                                            Eigen::Matrix<T, 2, 1> * pointDistorted) const;
#ifdef __ARM_NEON__
  inline void distortT4f(const float* pointUndistorted,
    float* pointDistorted, unsigned int status[4]) const;

  inline void distortT4f(const float* pointUndistorted, float* pointDistorted,
    float* pointJacobian, unsigned int status[4]) const;
  void distort4f(const float* uv_undist4f, float* uv_dist4f,
                 unsigned int status[4]) const override {
    this->distortT4f(uv_undist4f, uv_dist4f, status);
  }
#endif  // __ARM_NEON__
  inline bool distort(const Eigen::Vector2d & pointUndistorted,
                      Eigen::Vector2d * pointDistorted) const override {
    return this->distortT<double>(pointUndistorted, pointDistorted);
  };

  /// \brief Distortion only
  /// @param[in]  pointUndistorted The undistorted normalised (!) image point.
  /// @param[out] pointDistorted   The distorted normalised (!) image point.
  /// @return     True on success (no singularity)
  inline bool distort(const Eigen::Vector2f & pointUndistorted,
                      Eigen::Vector2f * pointDistorted) const override {
    // this shows much better result than using lookup table
    return this->distortT<float>(pointUndistorted, pointDistorted);
  };

  /// \brief Distortion and Jacobians (the fast version)
  ///        Computing parameterJacobian is currently NOT supported!
  /// @param[in]  pointUndistorted  The undistorted normalised (!) image point.
  /// @param[out] pointDistorted    The distorted normalised (!) image point.
  /// @param[out] pointJacobian     The Jacobian w.r.t. changes on the image point.
  /// @param[out] parameterJacobian The Jacobian w.r.t. changes on the intrinsics vector.
  /// @return     True on success (no singularity)
  template<typename T> inline bool distortT(
      const Eigen::Matrix<T, 2, 1> & pointUndistorted,
      Eigen::Matrix<T, 2, 1>  * pointDistorted,
      Eigen::Matrix<T, 2, 2> * pointJacobian,
      Eigen::Matrix<T, 2, Eigen::Dynamic> * parameterJacobian = NULL) const;

  /// \brief Distortion and Jacobians (the slow version)
  /// @param[in]  pointUndistorted  The undistorted normalised (!) image point.
  /// @param[out] pointDistorted    The distorted normalised (!) image point.
  /// @param[out] pointJacobian     The Jacobian w.r.t. changes on the image point.
  /// @param[out] parameterJacobian The Jacobian w.r.t. changes on the intrinsics vector.
  /// @return     True on success (no singularity)
  template<typename T> inline bool distortT_slow(
      const Eigen::Matrix<T, 2, 1> & pointUndistorted,
      Eigen::Matrix<T, 2, 1>  * pointDistorted,
      Eigen::Matrix<T, 2, 2> * pointJacobian,
      Eigen::Matrix<T, 2, Eigen::Dynamic> * parameterJacobian = NULL) const;

  inline bool distort(const Eigen::Vector2d& pointUndistorted,
                      Eigen::Vector2d* pointDistorted,
                      Eigen::Matrix2d* pointJacobian,
                      Eigen::Matrix2Xd* parameterJacobian = NULL) const override {
    return this->distortT<double>(pointUndistorted, pointDistorted,
                                  pointJacobian, parameterJacobian);
  }

  /// \brief Distortion and Jacobians.
  inline bool distort(const Eigen::Vector2f& pointUndistorted,
                      Eigen::Vector2f* pointDistorted,
                      Eigen::Matrix2f* pointJacobian,
                      Eigen::Matrix2Xf* parameterJacobian = NULL) const override {
    return this->distortT<float>(pointUndistorted, pointDistorted,
                                 pointJacobian, parameterJacobian);
  }

  /// \brief Distortion and Jacobians using external distortion intrinsics parameters.
  /// @param[in]  pointUndistorted  The undistorted normalised (!) image point.
  /// @param[in]  parameters        The distortion intrinsics vector.
  /// @param[out] pointDistorted    The distorted normalised (!) image point.
  /// @param[out] pointJacobian     The Jacobian w.r.t. changes on the image point.
  /// @param[out] parameterJacobian The Jacobian w.r.t. changes on the intrinsics vector.
  /// @return     True on success (no singularity)
  inline bool distortWithExternalParameters(
      const Eigen::Vector2d & pointUndistorted,
      const Eigen::VectorXd & parameters, Eigen::Vector2d * pointDistorted,
      Eigen::Matrix2d * pointJacobian = NULL,
      Eigen::Matrix2Xd * parameterJacobian = NULL) const override;
  /// @}

  //////////////////////////////////////////////////////////////
  /// \name Undistortion functions
  /// @{

  /// \brief Undistortion only (use look up table)
  /// @param[in]  pointDistorted   The distorted normalised (!) image point.
  /// @param[out] pointUndistorted The undistorted normalised (!) image point.
  /// @return     True on success (no singularity)
  inline bool undistort(const Eigen::Vector2d & pointDistorted,
                        Eigen::Vector2d * pointUndistorted) const override;
  inline bool undistort(const Eigen::Vector2f & pointDistorted,
                        Eigen::Vector2f * pointUndistorted) const override;

  /// \brief Undistortion only (use undistort Gauss Newton)
  /// @param[in]  pointDistorted   The distorted normalised (!) image point.
  /// @param[out] pointUndistorted The undistorted normalised (!) image point.
  /// @param[out] pointJacobian    The Jacobian w.r.t. changes on the image point.
  /// @return     True on success (no singularity)
  inline bool undistort(const Eigen::Vector2d & pointDistorted,
                        Eigen::Vector2d * pointUndistorted,
                        Eigen::Matrix2d * pointJacobian) const override;

  /// \brief Undistortion: use look up table for pointUndistorted & pointJacobian
  /// @param[in]  pointDistorted   The distorted normalised (!) image point.
  /// @param[out] pointUndistorted The undistorted normalised (!) image point.
  /// @param[out] pointJacobian    The Jacobian w.r.t. changes on the image point.
  /// @return     True on success (no singularity)
  inline bool undistort(const Eigen::Vector2f & pointDistorted,
                        Eigen::Vector2f * pointUndistorted,
                        Eigen::Matrix2f * pointJacobian) const override;
  /// @}
#ifdef __ARM_NEON__
  inline void undistort4f(const float* uv_dist4f, float* uv_undist4f,
                          unsigned int status[4]) const override;
  inline void undistort4f(const float* uv_dist4f, float* uv_undist4f,
                          float* jacobians_out, unsigned int status[4]) const override;
#endif

 protected:
  // Gauss-Newton iterative solution for undistortion
  inline bool undistort_GN(float u_dist, float v_dist, float* u_undist, float* v_undist,
                           Eigen::Matrix2f * pointJacobian = nullptr) const;
  Eigen::Matrix<float, NumDistortionIntrinsics, 1> parameters_;  ///< all distortion parameters

  float k1_;  ///< radial parameter 1
  float k2_;  ///< radial parameter 2
  float p1_;  ///< tangential parameter 1
  float p2_;  ///< tangential parameter 2
  float k3_;  ///< radial parameter 3
  float k4_;  ///< radial parameter 4
  float k5_;  ///< radial parameter 5
  float k6_;  ///< radial parameter 6
};

}  // namespace cameras
}  // namespace vio

#include "implementation/RadialTangentialDistortion8.hpp"

#endif /* INCLUDE_VIO_CAMERAS_RADIALTANGENTIALDISTORTION8_HPP_ */


// Copyright 2018 The Simons Foundation, Inc. - All Rights Reserved.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//    http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#include <Eigen/Dense>
#include <algorithm>
#include <cmath>
#include <iostream>
#include "Utils/random_utils.hpp"
#include <vector>
#include "Utils/json_utils.hpp"
#include "abstract_hilbert.hpp"

#ifndef NETKET_QUBITS_HPP
#define NETKET_QUBITS_HPP

namespace netket {

/**
  Hilbert space for qubits.
*/

class Qubit : public AbstractHilbert {
  std::vector<double> local_;

  int nqubits_;

 public:
  explicit Qubit(const json &pars) {
    int nqubits;

    if (!FieldExists(pars["Hilbert"], "Nqubits")) {
      std::cerr << "Nqubits is not defined" << std::endl;
    }

    nqubits = pars["Hilbert"]["Nqubits"];

    Init(nqubits);
  }

  void Init(int nqubits) {
    nqubits_ = nqubits;

    local_.resize(2);

    local_[0] = 0;
    local_[1] = 1;
  }

  bool IsDiscrete() const override { return true; }

  int LocalSize() const override { return 2; }

  int Size() const override { return nqubits_; }

  std::vector<double> LocalStates() const override { return local_; }

  void RandomVals(Eigen::VectorXd &state,
                  netket::default_random_engine &rgen) const override {
    std::uniform_int_distribution<int> distribution(0, 1);

    assert(state.size() == nqubits_);

    // unconstrained random
    for (int i = 0; i < state.size(); i++) {
      state(i) = distribution(rgen);
    }
  }

  void UpdateConf(Eigen::VectorXd &v, const std::vector<int> &tochange,
                  const std::vector<double> &newconf) const override {
    assert(v.size() == nqubits_);

    int i = 0;
    for (auto sf : tochange) {
      v(sf) = newconf[i];
      i++;
    }
  }
};

}  // namespace netket
#endif


#include <boost/date_time/gregorian_calendar.hpp>


#include <boost/accumulators/statistics/weighted_mean.hpp>


#pragma once

#include "../typedstateobserver.hh"
#include "../../StateObject/StationaryMapState/stationarymapstate.hh"
#include "../../StateObject/WalkState/walkstate.hh"
#include "../../stats/average.hh"

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

namespace bold
{
  class BehaviourControl;
  class Voice;

  class StationaryMapper : public StateObserver
  {
  public:
    StationaryMapper(std::shared_ptr <Voice> voice, std::shared_ptr <BehaviourControl> behaviourControl);

    void observe(SequentialTimer& timer) override;

  private:
    void updateStateObject() const;

    static void integrate(std::vector<Average<Eigen::Vector2d>>& estimates, Eigen::Vector2d pos, double mergeDistance);

    std::vector<Average<Eigen::Vector2d>> d_ballEstimates;
    std::vector<Average<Eigen::Vector2d>> d_goalEstimates;
    std::vector<Average<Eigen::Vector2d>> d_teammateEstimates;
    RadialOcclusionMap d_occlusionMap;
    bool d_hasData;
    std::shared_ptr<Voice> d_voice;
    std::shared_ptr<BehaviourControl> d_behaviourControl;
  };
}


// Boost.Geometry (aka GGL, Generic Geometry Library)

// Copyright (c) 2007-2012 Barend Gehrels, Amsterdam, the Netherlands.
// Copyright (c) 2008-2012 Bruno Lalande, Paris, France.
// Copyright (c) 2009-2012 Mateusz Loskot, London, UK.

// Use, modification and distribution is 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)
//
// Custom pointer-to-point example

#include <iostream>

#include <boost/foreach.hpp>

#include <boost/geometry/geometry.hpp>
#include <boost/geometry/geometries/register/ring.hpp>

BOOST_GEOMETRY_REGISTER_RING_TEMPLATED(std::vector)

// Sample point, having x/y
struct my_point
{
    my_point(double a = 0, double b = 0)
        : x(a), y(b)
    {}
    double x,y;
};

namespace boost { namespace geometry { namespace traits {

template<> struct tag<my_point*>
{ typedef point_tag type; };

template<> struct coordinate_type<my_point*>
{ typedef double type; };

template<> struct coordinate_system<my_point*>
{ typedef cs::cartesian type; };

template<> struct dimension<my_point*> : boost::mpl::int_<2> {};

template<>
struct access<my_point*, 0>
{
    static double get(my_point const* p)
    {
        return p->x;
    }

    static void set(my_point* p, double const& value)
    {
        p->x = value;
    }
};

template<>
struct access<my_point*, 1>
{
    static double get(my_point const* p)
    {
        return p->y;
    }

    static void set(my_point* p, double const& value)
    {
        p->y = value;
    }
};

}}} // namespace boost::geometry::traits

int main()
{
    typedef std::vector<my_point*> ring_type;

    ring_type a, b;

    a.push_back(new my_point(0, 1));
    a.push_back(new my_point(2, 5));
    a.push_back(new my_point(5, 3));
    a.push_back(new my_point(0, 1));

    b.push_back(new my_point(3, 0));
    b.push_back(new my_point(0, 3));
    b.push_back(new my_point(4, 5));
    b.push_back(new my_point(3, 0));

    double aa = boost::geometry::area(a);
    double ab = boost::geometry::area(b);

    std::cout << "a: " << aa << std::endl;
    std::cout << "b: " << ab << std::endl;

    // This will NOT work because would need dynamicly allocating memory for point* in algorithms:
    //std::vector<ring_type> unioned;
    //boost::geometry::union<ring_type>(a, b, unioned);

    /* This once worked, using pointers, but has to be fixed or deprecated
    typedef boost::geometry::model::ring<boost::geometry::model::d2::point_xy<double> > ring_2d;
    std::vector<ring_2d> unioned;
    std::vector<ring_2d> intersected;

    boost::geometry::intersection(a, b, intersected);
    boost::geometry::union_(a, b, unioned);

    double ai = 0, au = 0;
    BOOST_FOREACH(ring_2d const& ring, intersected)
    {
        ai += boost::geometry::area(ring);
    }
    BOOST_FOREACH(ring_2d const& ring, unioned)
    {
        au += boost::geometry::area(ring);
    }

    std::cout << "a: " << aa << std::endl;
    std::cout << "b: " << ab << std::endl;
    std::cout << "a & b: " << ai << std::endl;
    std::cout << "a | b: " << au << std::endl;
    std::cout << "a + b - (a & b): " << (aa + ab - ai) << std::endl;
    */

    // free
    BOOST_FOREACH(my_point* p, a)
    {
        delete p;
    }

    BOOST_FOREACH(my_point* p, b)
    {
        delete p;
    }

    return 0;
}


// Boost.Geometry (aka GGL, Generic Geometry Library)

// Copyright (c) 2007-2012 Barend Gehrels, Amsterdam, the Netherlands.
// Copyright (c) 2008-2012 Bruno Lalande, Paris, France.
// Copyright (c) 2009-2012 Mateusz Loskot, London, UK.

// Parts of Boost.Geometry are redesigned from Geodan's Geographic Library
// (geolib/GGL), copyright (c) 1995-2010 Geodan, Amsterdam, the Netherlands.

// Use, modification and distribution is 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_GEOMETRY_ALGORITHMS_SIMPLIFY_HPP
#define BOOST_GEOMETRY_ALGORITHMS_SIMPLIFY_HPP

#include <cstddef>

#include <boost/range.hpp>
#include <boost/variant/static_visitor.hpp>
#include <boost/variant/apply_visitor.hpp>
#include <boost/variant/variant_fwd.hpp>

#include <boost/geometry/core/cs.hpp>
#include <boost/geometry/core/closure.hpp>
#include <boost/geometry/core/exterior_ring.hpp>
#include <boost/geometry/core/interior_rings.hpp>
#include <boost/geometry/core/mutable_range.hpp>
#include <boost/geometry/core/tags.hpp>

#include <boost/geometry/geometries/concepts/check.hpp>
#include <boost/geometry/strategies/agnostic/simplify_douglas_peucker.hpp>
#include <boost/geometry/strategies/concepts/simplify_concept.hpp>
#include <boost/geometry/strategies/default_strategy.hpp>

#include <boost/geometry/algorithms/clear.hpp>
#include <boost/geometry/algorithms/convert.hpp>
#include <boost/geometry/algorithms/not_implemented.hpp>

#include <boost/geometry/algorithms/detail/distance/default_strategies.hpp>

namespace boost { namespace geometry
{

#ifndef DOXYGEN_NO_DETAIL
namespace detail { namespace simplify
{

struct simplify_range_insert
{
    template<typename Range, typename Strategy, typename OutputIterator, typename Distance>
    static inline void apply(Range const& range, OutputIterator out,
                             Distance const& max_distance, Strategy const& strategy)
    {
        if (boost::size(range) <= 2 || max_distance < 0)
        {
            std::copy(boost::begin(range), boost::end(range), out);
        }
        else
        {
            strategy.apply(range, out, max_distance);
        }
    }
};

struct simplify_copy
{
    template <typename Range, typename Strategy, typename Distance>
    static inline void apply(Range const& range, Range& out,
                             Distance const& , Strategy const& )
    {
        std::copy
            (
                boost::begin(range), boost::end(range), std::back_inserter(out)
            );
    }
};

template<std::size_t Minimum>
struct simplify_range
{
    template <typename Range, typename Strategy, typename Distance>
    static inline void apply(Range const& range, Range& out,
                    Distance const& max_distance, Strategy const& strategy)
    {
        // Call do_container for a linestring / ring

        /* For a RING:
            The first/last point (the closing point of the ring) should maybe
            be excluded because it lies on a line with second/one but last.
            Here it is never excluded.

            Note also that, especially if max_distance is too large,
            the output ring might be self intersecting while the input ring is
            not, although chances are low in normal polygons

            Finally the inputring might have 3 (open) or 4 (closed) points (=correct),
                the output < 3 or 4(=wrong)
        */

        if (boost::size(range) <= int(Minimum) || max_distance < 0.0)
        {
            simplify_copy::apply(range, out, max_distance, strategy);
        }
        else
        {
            simplify_range_insert::apply
                (
                    range, std::back_inserter(out), max_distance, strategy
                );
        }
    }
};

struct simplify_polygon
{
private:

    template
    <
        std::size_t Minimum,
        typename IteratorIn,
        typename IteratorOut,
        typename Distance,
        typename Strategy
    >
    static inline void iterate(IteratorIn begin, IteratorIn end,
                    IteratorOut it_out,
                    Distance const& max_distance, Strategy const& strategy)
    {
        for (IteratorIn it_in = begin; it_in != end;  ++it_in, ++it_out)
        {
            simplify_range<Minimum>::apply(*it_in, *it_out, max_distance, strategy);
        }
    }

    template
    <
        std::size_t Minimum,
        typename InteriorRingsIn,
        typename InteriorRingsOut,
        typename Distance,
        typename Strategy
    >
    static inline void apply_interior_rings(
                    InteriorRingsIn const& interior_rings_in,
                    InteriorRingsOut& interior_rings_out,
                    Distance const& max_distance, Strategy const& strategy)
    {
        traits::resize<InteriorRingsOut>::apply(interior_rings_out,
            boost::size(interior_rings_in));

        iterate<Minimum>(
            boost::begin(interior_rings_in), boost::end(interior_rings_in),
            boost::begin(interior_rings_out),
            max_distance, strategy);
    }

public:
    template <typename Polygon, typename Strategy, typename Distance>
    static inline void apply(Polygon const& poly_in, Polygon& poly_out,
                    Distance const& max_distance, Strategy const& strategy)
    {
        std::size_t const minimum = core_detail::closure::minimum_ring_size
            <
                geometry::closure<Polygon>::value
            >::value;

        // Note that if there are inner rings, and distance is too large,
        // they might intersect with the outer ring in the output,
        // while it didn't in the input.
        simplify_range<minimum>::apply(exterior_ring(poly_in),
                                       exterior_ring(poly_out),
                                       max_distance, strategy);

        apply_interior_rings<minimum>(interior_rings(poly_in),
                                      interior_rings(poly_out),
                                      max_distance, strategy);
    }
};

template<typename Policy>
struct simplify_multi
{
    template <typename MultiGeometry, typename Strategy, typename Distance>
    static inline void apply(MultiGeometry const& multi, MultiGeometry& out,
                    Distance const& max_distance, Strategy const& strategy)
    {
        traits::resize<MultiGeometry>::apply(out, boost::size(multi));

        typename boost::range_iterator<MultiGeometry>::type it_out
                = boost::begin(out);
        for (typename boost::range_iterator<MultiGeometry const>::type
                it_in = boost::begin(multi);
             it_in != boost::end(multi);
             ++it_in, ++it_out)
        {
            Policy::apply(*it_in, *it_out, max_distance, strategy);
        }
    }
};

}} // namespace detail::simplify
#endif // DOXYGEN_NO_DETAIL

#ifndef DOXYGEN_NO_DISPATCH
namespace dispatch
{

template
<
    typename Geometry,
    typename Tag = typename tag<Geometry>::type
>
struct simplify: not_implemented<Tag>
{};

template <typename Point>
struct simplify<Point, point_tag>
{
    template <typename Distance, typename Strategy>
    static inline void apply(Point const& point, Point& out,
                    Distance const& , Strategy const& )
    {
        geometry::convert(point, out);
    }
};

template <typename Linestring>
struct simplify<Linestring, linestring_tag>
    : detail::simplify::simplify_range<2>
{};

template <typename Ring>
struct simplify<Ring, ring_tag>
    : detail::simplify::simplify_range
            <
                core_detail::closure::minimum_ring_size
                    <
                        geometry::closure<Ring>::value
                    >::value
            >
{};

template <typename Polygon>
struct simplify<Polygon, polygon_tag>
    : detail::simplify::simplify_polygon
{};

template
<
    typename Geometry,
    typename Tag = typename tag<Geometry>::type
>
struct simplify_insert: not_implemented<Tag>
{};

template <typename Linestring>
struct simplify_insert<Linestring, linestring_tag>
    : detail::simplify::simplify_range_insert
{};

template <typename Ring>
struct simplify_insert<Ring, ring_tag>
    : detail::simplify::simplify_range_insert
{};

template <typename MultiPoint>
struct simplify<MultiPoint, multi_point_tag>
    : detail::simplify::simplify_copy
{};

template <typename MultiLinestring>
struct simplify<MultiLinestring, multi_linestring_tag>
    : detail::simplify::simplify_multi<detail::simplify::simplify_range<2> >
{};

template <typename MultiPolygon>
struct simplify<MultiPolygon, multi_polygon_tag>
    : detail::simplify::simplify_multi<detail::simplify::simplify_polygon>
{};

} // namespace dispatch
#endif // DOXYGEN_NO_DISPATCH

namespace resolve_strategy
{

struct simplify
{
    template <typename Geometry, typename Distance, typename Strategy>
    static inline void apply(Geometry const& geometry,
                             Geometry& out,
                             Distance const& max_distance,
                             Strategy const& strategy)
    {
        dispatch::simplify<Geometry>::apply(geometry, out, max_distance, strategy);
    }

    template <typename Geometry, typename Distance>
    static inline void apply(Geometry const& geometry,
                             Geometry& out,
                             Distance const& max_distance,
                             default_strategy)
    {
        typedef typename point_type<Geometry>::type point_type;

        typedef typename strategy::distance::services::default_strategy
        <
            point_tag, segment_tag, point_type
        >::type ds_strategy_type;

        typedef strategy::simplify::douglas_peucker
        <
            point_type, ds_strategy_type
        > strategy_type;

        BOOST_CONCEPT_ASSERT(
            (concept::SimplifyStrategy<strategy_type, point_type>)
        );

        apply(geometry, out, max_distance, strategy_type());
    }
};

struct simplify_insert
{
    template
    <
        typename Geometry,
        typename OutputIterator,
        typename Distance,
        typename Strategy
    >
    static inline void apply(Geometry const& geometry,
                             OutputIterator& out,
                             Distance const& max_distance,
                             Strategy const& strategy)
    {
        dispatch::simplify_insert<Geometry>::apply(geometry, out, max_distance, strategy);
    }

    template <typename Geometry, typename OutputIterator, typename Distance>
    static inline void apply(Geometry const& geometry,
                             OutputIterator& out,
                             Distance const& max_distance,
                             default_strategy)
    {
        typedef typename point_type<Geometry>::type point_type;

        typedef typename strategy::distance::services::default_strategy
        <
            point_tag, segment_tag, point_type
        >::type ds_strategy_type;

        typedef strategy::simplify::douglas_peucker
        <
            point_type, ds_strategy_type
        > strategy_type;

        BOOST_CONCEPT_ASSERT(
            (concept::SimplifyStrategy<strategy_type, point_type>)
        );

        apply(geometry, out, max_distance, strategy_type());
    }
};

} // namespace resolve_strategy

namespace resolve_variant {

template <typename Geometry>
struct simplify
{
    template <typename Distance, typename Strategy>
    static inline void apply(Geometry const& geometry,
                             Geometry& out,
                             Distance const& max_distance,
                             Strategy const& strategy)
    {
        resolve_strategy::simplify::apply(geometry, out, max_distance, strategy);
    }
};

template <BOOST_VARIANT_ENUM_PARAMS(typename T)>
struct simplify<boost::variant<BOOST_VARIANT_ENUM_PARAMS(T)> >
{
    template <typename Distance, typename Strategy>
    struct visitor: boost::static_visitor<void>
    {
        Distance const& m_max_distance;
        Strategy const& m_strategy;

        visitor(Distance const& max_distance, Strategy const& strategy)
            : m_max_distance(max_distance)
            , m_strategy(strategy)
        {}

        template <typename Geometry>
        void operator()(Geometry const& geometry, Geometry& out) const
        {
            simplify<Geometry>::apply(geometry, out, m_max_distance, m_strategy);
        }
    };

    template <typename Distance, typename Strategy>
    static inline void
    apply(boost::variant<BOOST_VARIANT_ENUM_PARAMS(T)> const& geometry,
          boost::variant<BOOST_VARIANT_ENUM_PARAMS(T)>& out,
          Distance const& max_distance,
          Strategy const& strategy)
    {
        boost::apply_visitor(
            visitor<Distance, Strategy>(max_distance, strategy),
            geometry,
            out
        );
    }
};

} // namespace resolve_variant

/*!
\brief Simplify a geometry using a specified strategy
\ingroup simplify
\tparam Geometry \tparam_geometry
\tparam Distance A numerical distance measure
\tparam Strategy A type fulfilling a SimplifyStrategy concept
\param strategy A strategy to calculate simplification
\param geometry input geometry, to be simplified
\param out output geometry, simplified version of the input geometry
\param max_distance distance (in units of input coordinates) of a vertex
    to other segments to be removed
\param strategy simplify strategy to be used for simplification, might
    include point-distance strategy

\image html svg_simplify_country.png "The image below presents the simplified country"
\qbk{distinguish,with strategy}
*/
template<typename Geometry, typename Distance, typename Strategy>
inline void simplify(Geometry const& geometry, Geometry& out,
                     Distance const& max_distance, Strategy const& strategy)
{
    concept::check<Geometry>();

    geometry::clear(out);

    resolve_variant::simplify<Geometry>::apply(geometry, out, max_distance, strategy);
}

/*!
\brief Simplify a geometry
\ingroup simplify
\tparam Geometry \tparam_geometry
\tparam Distance \tparam_numeric
\note This version of simplify simplifies a geometry using the default
    strategy (Douglas Peucker),
\param geometry input geometry, to be simplified
\param out output geometry, simplified version of the input geometry
\param max_distance distance (in units of input coordinates) of a vertex
    to other segments to be removed

\qbk{[include reference/algorithms/simplify.qbk]}
 */
template<typename Geometry, typename Distance>
inline void simplify(Geometry const& geometry, Geometry& out,
                     Distance const& max_distance)
{
    concept::check<Geometry>();

    simplify(geometry, out, max_distance, default_strategy());
}

#ifndef DOXYGEN_NO_DETAIL
namespace detail { namespace simplify
{

/*!
\brief Simplify a geometry, using an output iterator
    and a specified strategy
\ingroup simplify
\tparam Geometry \tparam_geometry
\param geometry input geometry, to be simplified
\param out output iterator, outputs all simplified points
\param max_distance distance (in units of input coordinates) of a vertex
    to other segments to be removed
\param strategy simplify strategy to be used for simplification,
    might include point-distance strategy

\qbk{distinguish,with strategy}
\qbk{[include reference/algorithms/simplify.qbk]}
*/
template<typename Geometry, typename OutputIterator, typename Distance, typename Strategy>
inline void simplify_insert(Geometry const& geometry, OutputIterator out,
                            Distance const& max_distance, Strategy const& strategy)
{
    concept::check<Geometry const>();

    resolve_strategy::simplify_insert::apply(geometry, out, max_distance, strategy);
}

/*!
\brief Simplify a geometry, using an output iterator
\ingroup simplify
\tparam Geometry \tparam_geometry
\param geometry input geometry, to be simplified
\param out output iterator, outputs all simplified points
\param max_distance distance (in units of input coordinates) of a vertex
    to other segments to be removed

\qbk{[include reference/algorithms/simplify_insert.qbk]}
 */
template<typename Geometry, typename OutputIterator, typename Distance>
inline void simplify_insert(Geometry const& geometry, OutputIterator out,
                            Distance const& max_distance)
{
    // Concept: output point type = point type of input geometry
    concept::check<Geometry const>();
    concept::check<typename point_type<Geometry>::type>();

    simplify_insert(geometry, out, max_distance, default_strategy());
}

}} // namespace detail::simplify
#endif // DOXYGEN_NO_DETAIL

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_ALGORITHMS_SIMPLIFY_HPP


#include <iostream>
#include <Eigen/Core>
#include "celerite/celerite.h"

// Timer for the benchmark.
#if defined(_MSC_VER)
    //no sys/time.h in visual c++
    //http://jakascorner.com/blog/2016/04/time-measurement.html
    #include <chrono>
    double get_timestamp ()
    {
      using micro_s = std::chrono::microseconds;
      auto tnow = std::chrono::steady_clock::now();
      auto d_micro = std::chrono::duration_cast<micro_s>(tnow.time_since_epoch()).count();
      return double(d_micro) * 1.0e-6;
    }
#else
   //no std::chrono in g++ 4.8
   #include <sys/time.h>
   double get_timestamp ()
   {
     struct timeval now;
     gettimeofday (&now, NULL);
     return double(now.tv_usec) * 1.0e-6 + double(now.tv_sec);
   }
#endif

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

  size_t nterms = 3;
  if (argc >= 2) nterms = atoi(argv[1]);
  size_t N_max = pow(2, 19);
  if (argc >= 3) N_max = atoi(argv[2]);
  size_t niter = 5;
  if (argc >= 4) niter = atoi(argv[3]);

  // Set up the coefficients.
  double jitter = 0.01;
  Eigen::VectorXd alpha = Eigen::VectorXd::Random(nterms),
                  beta_real = Eigen::VectorXd::Random(nterms),
                  beta_complex_real = Eigen::VectorXd::Random(nterms),
                  beta_complex_imag = Eigen::VectorXd::Random(nterms);
  alpha.array() += 1.0;
  beta_real.array() += 1.0;
  beta_complex_real.array() += 1.0;
  beta_complex_imag.array() += 1.0;

  // Generate some fake data.
  Eigen::VectorXd x0 = Eigen::VectorXd::Random(N_max),
                  yerr0 = Eigen::VectorXd::Random(N_max),
                  y0;

  // Set the scale of the uncertainties.
  yerr0.array() *= 0.1;
  yerr0.array() += 1.0;

  // The times need to be sorted.
  std::sort(x0.data(), x0.data() + x0.size());

  // Compute the y values.
  y0 = sin(x0.array());

  celerite::solver::CholeskySolver<double> solver;

  for (size_t N = 64; N <= N_max; N *= 2) {
    Eigen::VectorXd x = x0.topRows(N),
                    yerr = yerr0.topRows(N),
                    y = y0.topRows(N);

    // Benchmark the solver.
    double strt,
           real_compute_time = 0.0, real_solve_time = 0.0,
           complex_compute_time = 0.0, complex_solve_time = 0.0;
    for (size_t i = 0; i < niter; ++i) {
      strt = get_timestamp();
      solver.compute(jitter, alpha, beta_real, x, yerr);
      real_compute_time += get_timestamp() - strt;
    }

    for (size_t i = 0; i < niter; ++i) {
      strt = get_timestamp();
      solver.dot_solve(y);
      real_solve_time += get_timestamp() - strt;
    }

    for (size_t i = 0; i < niter; ++i) {
      strt = get_timestamp();
      solver.compute(jitter, alpha, beta_real, alpha, beta_complex_real, beta_complex_imag, x, yerr);
      complex_compute_time += get_timestamp() - strt;
    }

    for (size_t i = 0; i < niter; ++i) {
      strt = get_timestamp();
      solver.dot_solve(y);
      complex_solve_time += get_timestamp() - strt;
    }

    // Print the results.
    std::cout << N;
    std::cout << " ";
    std::cout << real_compute_time / niter;
    std::cout << " ";
    std::cout << real_solve_time / niter;
    std::cout << " ";
    std::cout << complex_compute_time / niter;
    std::cout << " ";
    std::cout << complex_solve_time / niter;
    std::cout << "\n";
  }

  return 0;
}


//==================================================================================================
/*!

  Copyright 2015 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)
*/
//==================================================================================================
#include <boost/simd/constant/powlowlim.hpp>
#include <boost/simd/as.hpp>
#include <simd_test.hpp>

STF_CASE_TPL( "Check powlowlim behavior for integral types"
            , (std::uint32_t)(std::uint64_t)
              (std::int32_t)(std::int64_t)
            )
{
  using boost::simd::as;
  using boost::simd::detail::powlowlim;
  using boost::simd::Powlowlim;
  T ref = T(0);
  STF_TYPE_IS(decltype(Powlowlim<T>()), T);
  STF_EQUAL(Powlowlim<T>(), ref);
  STF_EQUAL(powlowlim( as(T{}) ),ref);
}

STF_CASE_TPL( "Check powlowlim behavior for double"
            , (double)
            )
{
  using boost::simd::as;
  using boost::simd::detail::powlowlim;
  using boost::simd::Powlowlim;
  T ref = T(-17183);

  STF_TYPE_IS(decltype(Powlowlim<T>()), T);
  STF_IEEE_EQUAL(Powlowlim<T>(), ref);
  STF_IEEE_EQUAL(powlowlim( as(T{}) ), ref);
}

STF_CASE_TPL( "Check powlowlim behavior for float"
            , (float)
            )
{
  using boost::simd::as;
  using boost::simd::detail::powlowlim;
  using boost::simd::Powlowlim;
  T ref = T(-2400);
  STF_TYPE_IS(decltype(Powlowlim<T>()), T);
  STF_IEEE_EQUAL(Powlowlim<T>(), ref);
  STF_IEEE_EQUAL(powlowlim( as(T{}) ), ref);
}