#include <hpp/bezier-com-traj/solve.hh>
#include <hpp/bezier-com-traj/solver/solver-abstract.hpp>
#include <hpp/bezier-com-traj/solve_end_effector.hh>
#include <eigenpy/memory.hpp>
#include <eigenpy/eigenpy.hpp>

#include <boost/python.hpp>
#include <boost/python/exception_translator.hpp>

EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(bezier_com_traj::ContactData)
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(bezier_com_traj::ProblemData)
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(bezier_com_traj::ResultData)
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(bezier_com_traj::ResultDataCOMTraj)
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(bezier_com_traj::bezier_t)

namespace bezier_com_traj
{
using namespace boost::python;
typedef double real;

ResultDataCOMTraj* zeroStepCapturability(centroidal_dynamics::Equilibrium* eq, const Vector3& com ,const Vector3& dCom ,const Vector3& l0, const bool useAngMomentum
                              , const double timeDuration, const double timeStep)
{
    bezier_com_traj::ContactData data;
    data.contactPhase_ = eq;
    bezier_com_traj::ProblemData pData;
    pData.c0_  = com;
    pData.dc0_ = dCom;
    pData.l0_  = l0;
    pData.contacts_.push_back(data);
    pData.useAngularMomentum_ = useAngMomentum;
    std::vector<double> Ts;
    Ts.push_back(timeDuration);
    ResultDataCOMTraj  res = solve0step(pData, Ts, timeStep);
    return new ResultDataCOMTraj(res);
}

ResultDataCOMTraj* zeroStepCapturabilityWithKinConstraints(centroidal_dynamics::Equilibrium* eq, const Vector3& com ,const Vector3& dCom ,const Vector3& l0, const bool useAngMomentum
                              , const double timeDuration, const double timeStep, const MatrixXX& Kin, const MatrixXX& kin)
{
    bezier_com_traj::ContactData data;
    data.Kin_ = Kin;
    data.kin_ = kin;
    data.contactPhase_ = eq;
    bezier_com_traj::ProblemData pData;
    pData.c0_  = com;
    pData.dc0_ = dCom;
    pData.l0_  = l0;
    pData.contacts_.push_back(data);
    pData.useAngularMomentum_ = useAngMomentum;
    std::vector<double> Ts;
    Ts.push_back(timeDuration);
    ResultDataCOMTraj  res = solve0step(pData, Ts, timeStep);
    return new ResultDataCOMTraj(res);
}

struct res_data_exception : std::exception
{
  char const* what() const throw() { return "attributes not accessible for false resData"; }
};

void translate(const res_data_exception & e)
{
    // Use the Python 'C' API to set up an exception object
    PyErr_SetString(PyExc_RuntimeError, e.what());
}

struct contact_data_exception : std::exception
{
  char const* what() const throw() { return "attribute not defined yet for ContactData"; }
};

void translateContactData(const contact_data_exception & e)
{
    // Use the Python 'C' API to set up an exception object
    PyErr_SetString(PyExc_RuntimeError, e.what());
}

VectorX get_xD(const ResultData& res)
{
    if (res.x.size() > 0)
        return res.x;
    std::cout << "x is not defined" << std::endl;
    throw res_data_exception();
}

double get_costD(const ResultData& res)
{
    return res.cost_;
}

bool get_succD(const ResultData& res)
{
    return res.success_;
}

bezier_t* getC_of_t(const ResultDataCOMTraj& res)
{
    return new bezier_t(res.c_of_t_);
}

bezier_t* getDL_of_t(const ResultDataCOMTraj& res)
{
    return new bezier_t(res.dL_of_t_);
}

VectorX get_x(const ResultDataCOMTraj& res)
{
    if (res.x.size() > 0)
        return res.x;
    std::cout << "x is not defined" << std::endl;
    throw res_data_exception();
}

double get_cost(const ResultDataCOMTraj& res)
{
    return res.cost_;
}

bool get_succ(const ResultDataCOMTraj& res)
{
    return res.success_;
}

/** BEGIN CONTACT DATA **/
centroidal_dynamics::Equilibrium* getContactPhase_(const ContactData& data)
{
    if (data.contactPhase_)
        return data.contactPhase_;
    std::cout << "contactPhase_ is not assigned" << std::endl;
    throw contact_data_exception();
}

void setContactPhase_(ContactData& data, centroidal_dynamics::Equilibrium* eq)
{
    data.contactPhase_ = eq;
}

boost::python::tuple get_Ang(const ContactData& res)
{
    if(res.ang_.size() == 0)
    {
        std::cout << " no angular momentum constraints assigned  " << std::endl;
        throw contact_data_exception();
    }
    return boost::python::make_tuple(res.Ang_, res.ang_);
}

boost::python::tuple get_Kin(const ContactData& res)
{
    if(res.kin_.size() == 0)
    {
        std::cout << " no kinematic constraints assigned  " << std::endl;
        throw contact_data_exception();
    }
    return boost::python::make_tuple(res.Kin_, res.kin_);
}

void set_Kin(ContactData& res, const MatrixX3 & val, const VectorX& val2)
{
    if(val2.size() != val.rows())
    {
        std::cout << " Kinematic inequality matrix sizes do not match  " << std::endl;
        throw contact_data_exception();
    }
    res.Kin_ = val;
    res.kin_ = val2;
}

void set_Ang(ContactData& res, const MatrixX3 & val, const VectorX& val2)
{
    if(val2.size() != val.rows())
    {
        std::cout << " Angular inequality matrix sizes do not match  " << std::endl;
        throw contact_data_exception();
    }
    res.Ang_ = val;
    res.ang_ = val2;
}

/** END CONTACT DATA **/

/** BEGIN Constraints**/
int get_Flag(const Constraints& res)
{
    return (int)res.flag_;
}
bool get_ConstrainAcc(const Constraints& res)
{
    return res.constraintAcceleration_;
}
double get_MaxAcc(const Constraints& res)
{
    return res.maxAcceleration_;
}
double get_ReduceH(const Constraints& res)
{
    return res.reduce_h_;
}

void set_Flag(Constraints& res, const int val)
{
    res.flag_ = (ConstraintFlag)val;
}
void set_ConstrainAcc(Constraints& res, const bool val)
{
    res.constraintAcceleration_ = val;
}

void set_MaxAcc(Constraints& res, const double val)
{
    res.maxAcceleration_ = val;
}

void set_ReduceH(Constraints& res, const double val)
{
    res.reduce_h_ = val;
}

/** END Constraints **/

/** BEGIN ProblemData**/
point_t get_c0_(const ProblemData& res)
{
    return res.c0_;
}
point_t get_dc0_(const ProblemData& res)
{
    return res.dc0_;
}

point_t get_ddc0_(const ProblemData& res)
{
    return res.ddc0_;
}

point_t get_c1_(const ProblemData& res)
{
    return res.c1_;
}

point_t get_dc1_(const ProblemData& res)
{
    return res.dc1_;
}

point_t get_ddc1_(const ProblemData& res)
{
    return res.ddc1_;
}

void set_c0_(ProblemData& res, const point_t& val)
{
    res.c0_ = val;
}

void set_dc0_(ProblemData& res, const point_t& val)
{
    res.dc0_ = val;
}

void set_ddc0_(ProblemData& res, const point_t& val)
{
    res.ddc0_ = val;
}

void set_c1_(ProblemData& res, const point_t& val)
{
    res.c1_ = val;
}

void set_dc1_(ProblemData& res, const point_t& val)
{
    res.dc1_ = val;
}

void set_ddc1_(ProblemData& res, const point_t& val)
{
    res.ddc1_ = val;
}

bool get_useAngularMomentum_(const ProblemData& res)
{
    return res.useAngularMomentum_;
}
void set_useAngularMomentum_(ProblemData& res, const bool val)
{
    res.useAngularMomentum_ = val;
}

CostFunction get_costFunction_(const ProblemData& res)
{
    return res.costFunction_;
}

void set_costFunction_(ProblemData& res, const CostFunction val)
{
    res.costFunction_ = val;
}

GIWCRepresentation get_GIWC_representation_(const ProblemData& res)
{
    return res.representation_;
}

void set_GIWC_representation_(ProblemData& res, const GIWCRepresentation val)
{
    res.representation_ = val;
}

Constraints* get_constraints_(ProblemData& res)
{
    return &res.constraints_;
}

void set_constraints_(ProblemData& res, const Constraints& val)
{
    res.constraints_ = val;
}

std::vector<ContactData> get_contacts_(const ProblemData& res)
{
    return res.contacts_;
}

void addContact(ProblemData& res, const ContactData& val)
{
    res.contacts_.push_back(ContactData(val));
}

void clearContacts(ProblemData& res)
{
    res.contacts_.clear();
}

/** END ProblemData **/

/** BEGIN computeCOMTraj **/

ResultDataCOMTraj* computeCOMTrajPointer(const ProblemData& pData, const VectorX& Ts, const double timeStep)
{
    ResultDataCOMTraj  res = computeCOMTraj(pData, Ts, timeStep);
    return new ResultDataCOMTraj(res);
}

ResultDataCOMTraj* computeCOMTrajPointerChooseSolver(const ProblemData& pData, const VectorX& Ts, const double timeStep, const solvers::SolverType solver)
{
    ResultDataCOMTraj  res = computeCOMTraj(pData, Ts, timeStep, solver);
    return new ResultDataCOMTraj(res);
}

/** END computeCOMTraj **/
/** BEGIN end effector **/

struct DummyPath{

  point3_t operator()(double /*u*/)const{
    return point3_t::Zero();
  }

};

typedef std::pair<Eigen::Matrix<real, Eigen::Dynamic, Eigen::Dynamic>,
                  Eigen::Matrix<real, Eigen::Dynamic, Eigen::Dynamic> > linear_points_t;
typedef Eigen::Matrix<real, 3, Eigen::Dynamic> point_list_t;

struct MatrixVector
{
    linear_points_t res;
    Eigen::Matrix<real, Eigen::Dynamic, Eigen::Dynamic> A() {return res.first;}
    Eigen::Matrix<real, Eigen::Dynamic, Eigen::Dynamic> b() {return res.second;}
};

ResultDataCOMTraj* computeEndEffector(const ProblemData& pData, const double time){

   ResultDataCOMTraj  res =solveEndEffector<DummyPath>(pData,DummyPath(),time, 0);
   return new ResultDataCOMTraj(res);
}

MatrixVector* computeEndEffectorConstraintsPython(const ProblemData& pData, const double time){
   std::vector<bezier_t::point_t> pi = computeConstantWaypoints(pData,time);
   MatrixVector* res = new MatrixVector();
   res->res =computeEndEffectorConstraints(pData,time, pi);
   return res;
}

MatrixVector* computeEndEffectorVelocityCostPython(const ProblemData& pData, const double time){
   std::vector<bezier_t::point_t> pi = computeConstantWaypoints(pData,time);
   MatrixVector* res = new MatrixVector();
   res->res = computeVelocityCost(pData,time,pi);
   return res;
}

MatrixVector* computeEndEffectorDistanceCostPython(const ProblemData& pData,const double time,const int numPoints,point_list_t pts_l){
   std::vector<bezier_t::point_t> pi = computeConstantWaypoints(pData,time);
    // transform the matrice 3xN in a std::vector<point3_t> of size N :
   std::vector<point3_t> pts_path;
   for(size_t c = 0 ; c < pts_l.cols() ; ++c){
      pts_path.push_back(pts_l.block<3,1>(0,c));
   }
   MatrixVector* res = new MatrixVector();
   res->res =computeDistanceCostFunction(numPoints,pData,time,pts_path);
   return res;
}

Eigen::Matrix<real, Eigen::Dynamic, Eigen::Dynamic> computeEndEffectorConstantWaypoints(const ProblemData& pData, const double time){
   std::vector<bezier_t::point_t> pi = computeConstantWaypoints(pData,time);
   Eigen::Matrix<real, Eigen::Dynamic, Eigen::Dynamic> res (3, pi.size());
   int col = 0;
   for(std::vector<bezier_t::point_t>::const_iterator cit = pi.begin(); cit != pi.end(); ++cit, ++col)
    res.block<3,1>(0,col) = *cit;
   return res;
 }

/** END end effector **/

BOOST_PYTHON_MODULE(hpp_bezier_com_traj)
{
    using namespace boost::python;
    register_exception_translator<res_data_exception>(&translate);
    register_exception_translator<contact_data_exception>(&translateContactData);
    /** BEGIN eigenpy init**/
    eigenpy::enableEigenPy();

    eigenpy::enableEigenPySpecific<point_t,point_t>();
    eigenpy::enableEigenPySpecific<Vector3,Vector3>();
    eigenpy::enableEigenPySpecific<VectorX,VectorX>();
    eigenpy::enableEigenPySpecific<MatrixX3,MatrixX3>();
    eigenpy::enableEigenPySpecific<MatrixX3,MatrixX3>();

    /** END eigenpy init**/
    /*eigenpy::exposeAngleAxis();
    eigenpy::exposeQuaternion();*/

    class_<ResultDataCOMTraj>("ResultDataCOMTraj", init<> ())
                .add_property("c_of_t",  make_function(&getC_of_t,
                                                       return_value_policy<manage_new_object>()))
                .add_property("dL_of_t",make_function(&getDL_of_t,
                                                       return_value_policy<manage_new_object>()))
                .add_property("success", &get_succ)
                .add_property("cost",    &get_cost)
                .add_property("x",       &get_x)
            ;

    class_<ResultData>("ResultData", init<> ())
                .add_property("success", &get_succD)
                .add_property("cost",    &get_costD)
                .add_property("x",       &get_xD)
            ;

    class_<ContactData>("ContactData", init<centroidal_dynamics::Equilibrium*>()[with_custodian_and_ward<1,2>()])
                .add_property("contactPhase_", make_function(&getContactPhase_,
                                                             return_value_policy<reference_existing_object>()), &setContactPhase_)
                .add_property("Kin_",&get_Kin)
                .add_property("Ang_",&get_Ang)
                .def("setKinematicConstraints", &set_Kin)
                .def("setAngularConstraints", &set_Ang)
            ;

    class_<ProblemData>("ProblemData", init<>())
            .add_property("c0_",&get_c0_, &set_c0_ )
            .add_property("dc0_",&get_dc0_, &set_dc0_)
            .add_property("ddc0_",&get_ddc0_, &set_ddc0_ )
            .add_property("c1_",&get_c1_, &set_c1_)
            .add_property("dc1_",&get_dc1_, &set_dc1_)
            .add_property("ddc1_",&get_ddc1_, &set_ddc1_ )
            .add_property("useAngularMomentum_",&get_useAngularMomentum_, &set_useAngularMomentum_)
            .add_property("costFunction_",&get_costFunction_, &set_costFunction_ )
            .add_property("GIWCrepresentation_",&get_GIWC_representation_, &set_GIWC_representation_ )
            .add_property("constraints_",make_function(&get_constraints_,
                                                       return_value_policy<reference_existing_object>()), &set_constraints_ )
            .def("clearContacts", clearContacts)
            .def("addContact", addContact)
            ;

    class_<Constraints>("Constraints", init<>())
                .add_property("flag_",&get_Flag, &set_Flag )
                .add_property("constrainAcceleration_",&get_ConstrainAcc, &set_ConstrainAcc )
                .add_property("maxAcceleration_",&get_MaxAcc, &set_MaxAcc )
                .add_property("reduce_h_",&get_ReduceH, &set_ReduceH )
            ;

    enum_<CostFunction>("CostFunction")
            .value("ACCELERATION", ACCELERATION)
            .value("DISTANCE_TRAVELED", DISTANCE_TRAVELED)
            .value("TARGET_END_VELOCITY", TARGET_END_VELOCITY)
            .value("UNKNOWN_COST", UNKNOWN_COST)
            .export_values();

    enum_<GIWCRepresentation>("GIWCRepresentation")
            .value("DOUBLE_DESCRIPTION", DOUBLE_DESCRIPTION)
            .value("FORCE", FORCE)
            .value("UNKNOWN_REPRESENTATION", UNKNOWN_REPRESENTATION)
            .export_values();

    enum_<solvers::SolverType>("SolverType")
            .value("SOLVER_QUADPROG", solvers::SOLVER_QUADPROG)
            //.value("SOLVER_QUADPROG_SPARSE", solvers::SOLVER_QUADPROG_SPARSE)
#ifdef USE_GLPK_SOLVER
            .value("SOLVER_GLPK", solvers::SOLVER_GLPK)
#endif
            .export_values();

    enum_<ConstraintFlag>("ConstraintFlag")
            .value("INIT_POS", INIT_POS)
            .value("INIT_VEL", INIT_VEL)
            .value("INIT_ACC", INIT_ACC)
            .value("INIT_JERK",INIT_JERK)
            .value("END_POS", END_POS)
            .value("END_VEL", END_VEL)
            .value("END_ACC", END_ACC)
            .value("END_JERK",END_JERK)
            .value("ONE_FREE_VAR",ONE_FREE_VAR)
            .value("TWO_FREE_VAR",TWO_FREE_VAR)
            .value("THREE_FREE_VAR",THREE_FREE_VAR)
            .value("FOUR_FREE_VAR",FOUR_FREE_VAR)
            .value("FIVE_FREE_VAR",FIVE_FREE_VAR)
            .value("UNKNOWN", UNKNOWN)
            .export_values();

    class_<MatrixVector>("MatrixVector", no_init)
          .def_readonly("A", &MatrixVector::A)
          .def_readonly("b", &MatrixVector::b);

    def("zeroStepCapturability", &zeroStepCapturability, return_value_policy<manage_new_object>());
    def("zeroStepCapturability", &zeroStepCapturabilityWithKinConstraints, return_value_policy<manage_new_object>());
    def("computeCOMTraj", &computeCOMTrajPointer, return_value_policy<manage_new_object>());
    def("computeCOMTraj", &computeCOMTrajPointerChooseSolver, return_value_policy<manage_new_object>());
    def("computeEndEffector", &computeEndEffector, return_value_policy<manage_new_object>());
    def("computeEndEffectorConstraints", &computeEndEffectorConstraintsPython, return_value_policy<manage_new_object>());
    def("computeEndEffectorVelocityCost", &computeEndEffectorVelocityCostPython, return_value_policy<manage_new_object>());
    def("computeEndEffectorDistanceCost", &computeEndEffectorDistanceCostPython, return_value_policy<manage_new_object>());
    def("computeEndEffectorConstantWaypoints", &computeEndEffectorConstantWaypoints);

}

} // namespace bezier_com_traj


// Copyright 2020-2022 The Defold Foundation
// Copyright 2014-2020 King
// Copyright 2009-2014 Ragnar Svensson, Christian Murray
// Licensed under the Defold License version 1.0 (the "License"); you may not use
// this file except in compliance with the License.
//
// You may obtain a copy of the License, together with FAQs at
// https://www.defold.com/license
//
// 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 <assert.h>
#include <stdio.h>
#include <dlib/math.h>
#include "easing.h"

namespace dmEasing
{
    #include "easing_lookup.h"

    float GetValue(Type type, float t)
    {
        return GetValue(Curve(type), t);
    }

    float GetValue(Curve curve, float t)
    {
        t = dmMath::Clamp(t, 0.0f, 1.0f);
        int sample_count;
        int index1, index2;
        float val1, val2;
        float* lookup;

        if (curve.type == dmEasing::TYPE_FLOAT_VECTOR)
        {
            sample_count = curve.vector->size;
            lookup       = curve.vector->values;

            if (sample_count == 0)
            {
                return 0.0f;
            } else if (sample_count == 1) {
                return lookup[0];
            }

        } else {
            sample_count = EASING_SAMPLES; // 64 samples for built in curves
            lookup       = EASING_LOOKUP;
            lookup      += curve.type * (EASING_SAMPLES + 1); // NOTE: + 1 as the last sample is duplicated
        }

        index1 = (int) (t * (sample_count-1));
        index2 = dmMath::Min(index1 + 1, sample_count-1);

        val1 = lookup[index1];
        val2 = lookup[index2];

        float diff = (t - index1 * (1.0f / (sample_count-1))) * (sample_count-1);
        return val1 * (1.0f - diff) + val2 * diff;
    }
}


#include <cctbx/boost_python/flex_fwd.h>

#include <cctbx/miller/index_generator.h>
#include <scitbx/boost_python/iterator_wrappers.h>
#include <scitbx/boost_python/is_polymorphic_workaround.h>
#include <boost/python/class.hpp>
#include <boost/python/return_internal_reference.hpp>

SCITBX_BOOST_IS_POLYMORPHIC_WORKAROUND(cctbx::miller::index_generator)

namespace cctbx { namespace miller { namespace boost_python {

namespace {

  struct index_generator_wrappers
  {
    typedef index_generator w_t;

    static index<>
    next(w_t& o)
    {
      index<> result = o.next();
      if (result.is_zero()) {
        PyErr_SetString(PyExc_StopIteration, "At end of iteration.");
        boost::python::throw_error_already_set();
      }
      return result;
    }

    static void
    wrap()
    {
      using namespace boost::python;
      typedef return_internal_reference<> rir;
      class_<w_t>("index_generator", no_init)
        .def(init<uctbx::unit_cell const&,
                  sgtbx::space_group_type const&,
                  bool,
                  double>(
             args("unit_cell", "space_group_type", "anomalous_flag",
                  "resolution_limit")))
        .def(init<sgtbx::space_group_type const&,
                  bool,
                  index<> const&>(
             args("space_group_type", "anomalous_flag", "max_index")))
        .def("unit_cell", &w_t::unit_cell, rir())
        .def("space_group_type", &w_t::space_group_type, rir())
        .def("anomalous_flag", &w_t::anomalous_flag)
        .def("asu", &w_t::asu, rir())
        .def("next", next)
        .def("__next__", next)
        .def("__iter__", scitbx::boost_python::pass_through)
        .def("to_array", &w_t::to_array)
      ;
    }
  };

} // namespace <anoymous>

  void wrap_index_generator()
  {
    index_generator_wrappers::wrap();
  }

}}} // namespace cctbx::miller::boost_python


/** This is free and unencumbered software released into the public domain.

The authors of ISIS do not claim copyright on the contents of this file.
For more details about the LICENSE terms and the AUTHORS, you will
find files of those names at the top level of this repository. **/

/* SPDX-License-Identifier: CC0-1.0 */

#include <boost/foreach.hpp>
#include "opencv2/opencv.hpp"
#include "opencv2/xfeatures2d.hpp"

#include "MSERAlgorithm.h"
#include <QVariant>

namespace Isis {

  /**
   * Constructs the algorithm with default variables.
   */
  MSERAlgorithm::MSERAlgorithm() :
                 Feature2DAlgorithm("MSER", "Feature2D",
                                    MSERType::create()) {
    setupParameters();
  }

  /**
   * Constructs the algorithm with the input variables
   *
   * @param cvars  The variables and values the algorithm will use.
   *               Variables that are not included will be set to their default.
   * @param config The config string used to construct cvars.
   */
  MSERAlgorithm::MSERAlgorithm(const PvlFlatMap &cvars, const QString &config) :
                 Feature2DAlgorithm("MSER", "Feature2D",
                                    MSERType::create(), cvars) {
    setConfig(config);
    PvlFlatMap variables = setupParameters();
    variables.merge(cvars);
    const int delta = toInt(variables.get("Delta"));
    const int minArea = toInt(variables.get("MinArea"));
    const int maxArea = toInt(variables.get("MaxArea"));
    const double maxVariation = toDouble(variables.get("MaxVariation"));
    const double minDiversity = toDouble(variables.get("MinDiversity"));
    const int maxEvolution = toInt(variables.get("MaxEvolution"));
    const double areaThreshold = toDouble(variables.get("AreaThreshold"));
    const double minMargin = toDouble(variables.get("MinMargin"));
    const int edgeBlurSize = toInt(variables.get("EdgeBlurSize"));

    m_algorithm = MSERType::create(delta, minArea, maxArea, maxVariation, minDiversity,
                                   maxEvolution, areaThreshold, minMargin, edgeBlurSize);

    m_variables.merge(variables);
  }

  /**
   * Destroys the algorithm
   */
  MSERAlgorithm::~MSERAlgorithm() { }

  /**
   * Sets up the algorithm parameters with default values.
   *
   * @return PvlFlatMap Algorithm parameters and their default values.
   */
  PvlFlatMap MSERAlgorithm::setupParameters() {
    PvlFlatMap variables;
    variables.add("Delta",         "5");
    variables.add("MinArea",       "60");
    variables.add("MaxArea",       "14400");
    variables.add("MaxVariation",  "0.25");
    variables.add("MinDiversity",  "0.2");
    variables.add("MaxEvolution",  "200");
    variables.add("AreaThreshold", "1.01");
    variables.add("MinMargin",     "0.003");
    variables.add("EdgeBlurSize",  "5");
    m_variables = variables;
    return (m_variables);
  }

  /**
   * Returns a description of the algorithm.
   *
   * @return @b QString A description of the algorithm.
   */
  QString MSERAlgorithm::description() const {
    QString desc = "The OpenCV MSER Feature2D detector/extractor algorithm."
                   " See the documentation at "
     "http://docs.opencv.org/3.1.0/d3/d28/classcv_1_1MSER.html";
    return (desc);
  }

  /**
   * Creates an instance of the algorithm.
   *
   * @param cvars  The variables and values the algorithm will use.
   *               Variables that are not included will be set to their default.
   * @param config The config string used to construct cvars.
   */
  Feature2DAlgorithm *MSERAlgorithm::create(const PvlFlatMap &vars, const QString &config) {
    return ( new MSERAlgorithm(vars, config) );
  }

  /**
   * Returns true if the algorithm has a detector.
   *
   * @return @b true if the algorithm has a detector.
   */
  bool MSERAlgorithm::hasDetector() const {
    return true;
  }

  /**
   * Returns true if the algorithm has an extractor.
   *
   * @return @b true if the algorithm has an extractor.
   */
  bool MSERAlgorithm::hasExtractor() const {
    return false;
  }

  /**
   * Returns true if the algorithm has a matcher.
   *
   * @return @b true if the algorithm has a matcher.
   */
  bool MSERAlgorithm::hasMatcher() const {
    return false;
  }

  /**
   * Returns the variables and their values used by the algorithm.
   *
   * @return @b PvlFlatMap The variables and their values as keyword, value pairs.
   */
  PvlFlatMap MSERAlgorithm::getAlgorithmVariables( ) const {
    return ( variables() );
  }

/**
 * @brief Set parameters as provided by the variables
 *
 * @param variables Container of parameters to set
 *
 * @return @b int Always -1, variables cannot be set after initialization.
 *
 * @throws IException::Programmer "BriefDescriptorAlgorithm does not have the ability
 *                                 to set algorithm parameters."
 */
  int MSERAlgorithm::setAlgorithmVariables(const PvlFlatMap &variables) {
    QString msg = "MSERAlgorithm does not have the ability to set algorithm parameters.";
    throw IException(IException::Programmer, msg, _FILEINFO_);

    return (-1);
  }

};  // namespace Isis


#ifndef ASLAM_BACKEND_ROTATION_EXPRESSION_HPP
#define ASLAM_BACKEND_ROTATION_EXPRESSION_HPP

#include <Eigen/Core>
#include <boost/shared_ptr.hpp>
#include <aslam/backend/JacobianContainer.hpp>
#include "HomogeneousExpression.hpp"
#include "EuclideanExpression.hpp"
#include "TransformationExpression.hpp"
#include <set>
#include <sm/kinematics/RotationalKinematics.hpp>

namespace aslam {
  namespace backend {

    class RotationExpressionNode;
    class TransformationExpression;
    class EuclideanExpression;
    class HomogeneousExpression;

    class RotationExpression
    {
    public:
        SM_DEFINE_EXCEPTION(Exception, std::runtime_error);

      /// \breif initialize from an existing node.
      RotationExpression(boost::shared_ptr<RotationExpressionNode> root);

      /// \brief Initialize from an existing node. The node will not be deleted.
      RotationExpression(RotationExpressionNode * root);

      virtual ~RotationExpression();

      /// \brief Evaluate the rotation matrix.
      Eigen::Matrix3d toRotationMatrix();

      EuclideanExpression toParameters(sm::kinematics::RotationalKinematics::Ptr rk);

      /// \brief return the expression that inverts the rotation.
      RotationExpression inverse();

      /// \brief Evaluate the Jacobians in the form (1 - (S \delta v)^\times) \bar C
      void evaluateJacobians(JacobianContainer & outJacobians) const;

      RotationExpression operator*(const RotationExpression & p);
      EuclideanExpression operator*(const EuclideanExpression & p);
      HomogeneousExpression operator*(const HomogeneousExpression & p);

      TransformationExpression toTransformationExpression();

      void getDesignVariables(DesignVariable::set_t & designVariables) const;

      boost::shared_ptr<RotationExpressionNode> root() { return _root; }
    private:

      RotationExpression();
      boost::shared_ptr<RotationExpressionNode> _root;
    };

  } // namespace backend
} // namespace aslam

#endif /* ASLAM_BACKEND_ROTATION_EXPRESSION_HPP */


#include <iostream>
#include <unordered_map>
#include <cstdlib>

#include "hpyplm.h"
#include "corpus/corpus.h"
#include "cpyp/m.h"
#include "cpyp/random.h"
#include "cpyp/crp.h"
#include "cpyp/tied_parameter_resampler.h"

#include "cpyp/boost_serializers.h"
#include <boost/serialization/vector.hpp>
#include <boost/archive/binary_oarchive.hpp>

#define kORDER 3

using namespace std;
using namespace cpyp;

Dict dict;

int main(int argc, char** argv) {
  if (argc != 4) {
    cerr << argv[0] << " <training.txt> <output.lm> <nsamples>\n\nEstimate a " << kORDER << "-gram HPYP LM and write it to a file\n100 is usually sufficient for <nsamples>\n";
    return 1;
  }
  MT19937 eng;
  string train_file = argv[1];
  string output_file = argv[2];
  {
    ifstream test(output_file);
    if (test.good()) {
      cerr << "File " << output_file << " appears to exist: please remove\n";
      return 1;
    }
  }
  int samples = atoi(argv[3]);
  assert(samples > 0);

  vector<vector<unsigned> > corpus;
  set<unsigned> vocabe, tv;
  const unsigned kSOS = dict.Convert("<s>");
  const unsigned kEOS = dict.Convert("</s>");
  cerr << "Reading corpus...\n";
  ReadFromFile(train_file, &dict, &corpus, &vocabe);
  cerr << "E-corpus size: " << corpus.size() << " sentences\t (" << vocabe.size() << " word types)\n";
  PYPLM<kORDER> lm(vocabe.size(), 1, 1, 1, 1);
  vector<unsigned> ctx(kORDER - 1, kSOS);
  for (int sample=0; sample < samples; ++sample) {
    for (const auto& s : corpus) {
      ctx.resize(kORDER - 1);
      for (unsigned i = 0; i <= s.size(); ++i) {
        unsigned w = (i < s.size() ? s[i] : kEOS);
        if (sample > 0) lm.decrement(w, ctx, eng);
        lm.increment(w, ctx, eng);
        ctx.push_back(w);
      }
    }
    if (sample % 10 == 9) {
      cerr << " [LLH=" << lm.log_likelihood() << "]" << endl;
      if (sample % 30u == 29) lm.resample_hyperparameters(eng);
    } else { cerr << '.' << flush; }
  }
  cerr << "Writing LM to " << output_file << " ...\n";
  ofstream ofile(output_file.c_str(), ios::out | ios::binary);
  if (!ofile.good()) {
    cerr << "Failed to open " << output_file << " for writing\n";
    return 1;
  }
  boost::archive::binary_oarchive oa(ofile);
  oa & dict;
  oa & lm;

  return 0;
}


/*
 * Copyright 2017, Andrea Del Prete, LAAS-CNRS
 *
 */

#ifndef _DeviceTorqueCtrl_H_
#define _DeviceTorqueCtrl_H_

#include <Eigen/Cholesky>

#include <boost/random/mersenne_twister.hpp>
#include <boost/random/normal_distribution.hpp>
#include <boost/random/variate_generator.hpp>

#include <tsid/robots/robot-wrapper.hpp>
#include <tsid/tasks/task-se3-equality.hpp>

#include <dynamic-graph/entity.h>
#include <dynamic-graph/signal.h>
#include <dynamic-graph/signal-ptr.h>
#include <dynamic-graph/linear-algebra.h>
#include <sot/core/device.hh>
#include <sot/core/abstract-sot-external-interface.hh>

/* HELPER */
#include <dynamic-graph/signal-helper.h>
#include <sot/core/robot-utils.hh>

namespace dgsot = dynamicgraph::sot;

namespace dynamicgraph {
namespace sot {
namespace torque_control {

/** Version of device for testing the code without the
 * real robot. This version of the device assumes that the robot
 * is torque controlled (i.e. the control input are the desired
 * joint torques). These desired joint torques are used to compute
 * the joint accelerations and the contact forces.
 * The joint accelerations are then integrated to get the
 * measured joint angles.
 *
 * The feet are supposed to be fixed to the ground. The accelerometer
 * and the gyrometer output a constant value.
 *
 * A white Gaussian noise is added to the force/torque sensor
 * measurements.
 *
 * TODO: The original Device class should be a clean abstraction for
 * the concept of device, but currently it is not. It defines a lot
 * of input/output signals that are specific of HRP-2 (e.g. zmpSIN,
 * attitudeSIN, forcesSOUT) and some design choices (e.g. secondOrderIntegration).
 * It would be nice to clean the original Device from all this stuff
 * and move it in the specific subclasses.
 */
class DeviceTorqueCtrl : public dgsot::Device {
 public:
  static const std::string CLASS_NAME;
  static const double TIMESTEP_DEFAULT;
  static const double FORCE_SENSOR_NOISE_STD_DEV;

  virtual const std::string& getClassName() const { return CLASS_NAME; }

  DeviceTorqueCtrl(std::string RobotName);
  virtual ~DeviceTorqueCtrl();

  virtual void setStateSize(const unsigned int& size);
  virtual void setState(const dynamicgraph::Vector& st);
  virtual void setVelocity(const dynamicgraph::Vector& vel);
  virtual void setControlInputType(const std::string& cit);

  virtual void init(const double& dt, const std::string& urdfFile);

 protected:
  virtual void integrate(const double& dt);
  void computeForwardDynamics();

  void sendMsg(const std::string& msg, MsgType t = MSG_TYPE_INFO, const char* = "", int = 0) {
    logger_.stream(t) << ("[DeviceTorqueCtrl] " + msg) << '\n';
  }

  /// \brief Current integration step.
  double timestep_;
  bool m_initSucceeded;

  /// input force sensor values
  dynamicgraph::SignalPtr<dynamicgraph::Vector, int>* forcesSIN_[4];

  /// Accelerations measured by accelerometers
  dynamicgraph::Signal<dynamicgraph::Vector, int> accelerometerSOUT_;
  /// Rotation velocity measured by gyrometers
  dynamicgraph::Signal<dynamicgraph::Vector, int> gyrometerSOUT_;
  /// base 6d pose + joints' angles measured by encoders
  dynamicgraph::Signal<dynamicgraph::Vector, int> robotStateSOUT_;
  /// joints' velocities computed by the integrator
  dynamicgraph::Signal<dynamicgraph::Vector, int> jointsVelocitiesSOUT_;
  /// joints' accelerations computed by the integrator
  dynamicgraph::Signal<dynamicgraph::Vector, int> jointsAccelerationsSOUT_;
  /// motor currents
  dynamicgraph::Signal<dynamicgraph::Vector, int> currentSOUT_;
  /// proportional and derivative position-control gains
  dynamicgraph::Signal<dynamicgraph::Vector, int> p_gainsSOUT_;
  dynamicgraph::Signal<dynamicgraph::Vector, int> d_gainsSOUT_;

  DECLARE_SIGNAL_IN(kp_constraints, dynamicgraph::Vector);
  DECLARE_SIGNAL_IN(kd_constraints, dynamicgraph::Vector);
  DECLARE_SIGNAL_IN(rotor_inertias, dynamicgraph::Vector);
  DECLARE_SIGNAL_IN(gear_ratios, dynamicgraph::Vector);

  /// Intermediate variables to avoid allocation during control
  dynamicgraph::Vector accelerometer_;
  dynamicgraph::Vector gyrometer_;

  dynamicgraph::Vector base6d_encoders_;      /// base 6d pose + joints' angles
  dynamicgraph::Vector jointsVelocities_;     /// joints' velocities
  dynamicgraph::Vector jointsAccelerations_;  /// joints' accelerations

  dynamicgraph::Vector wrenches_[4];
  dynamicgraph::Vector temp6_;

  bool m_isTorqueControlled;

  /// robot geometric/inertial data
  tsid::robots::RobotWrapper* m_robot;
  pinocchio::Data* m_data;
  tsid::tasks::TaskSE3Equality* m_contactRF;
  tsid::tasks::TaskSE3Equality* m_contactLF;
  unsigned int m_nk;  // number of contact forces

  tsid::math::Vector m_q, m_v, m_dv, m_f;
  tsid::math::Vector m_q_sot, m_v_sot, m_dv_sot;

  typedef Eigen::LDLT<Eigen::MatrixXd> Cholesky;
  Cholesky m_K_chol;  /// cholesky decomposition of the K matrix
  Eigen::MatrixXd m_K;
  Eigen::VectorXd m_k;
  Eigen::MatrixXd m_Jc;  /// constraint Jacobian

  double m_numericalDamping;          /// numerical damping to regularize constraint resolution
  Eigen::JacobiSVD<Matrix> m_Jc_svd;  /// svd of the constraint matrix
  Vector m_dJcv;
  Matrix m_Z;                      /// base of constraint null space
  Matrix m_ZMZ;                    /// projected mass matrix: Z_c^T*M*Z_c
  Eigen::LDLT<Matrix> m_ZMZ_chol;  /// Cholesky decomposition of _ZMZ
  Vector m_dv_c;                   /// constrained accelerations
  Vector m_dvBar;                  /// solution of Jc*dv=-dJc*v
  Vector m_tau_np6;
  int m_nj;  /// number of joints

  typedef boost::mt11213b ENG;                      // uniform random number generator
  typedef boost::normal_distribution<double> DIST;  // Normal Distribution
  typedef boost::variate_generator<ENG, DIST> GEN;  // Variate generator
  ENG randomNumberGenerator_;
  DIST normalDistribution_;
  GEN normalRandomNumberGenerator_;

  RobotUtilShrPtr m_robot_util;
};

}  // end namespace torque_control
}  // end namespace sot
}  // end namespace dynamicgraph
#endif /* DevicePosCtrl*/


//==================================================================================================
/*!
  @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_CONSTANT_MAXINIT_HPP_INCLUDED
#define BOOST_SIMD_CONSTANT_MAXINIT_HPP_INCLUDED

/*!
  @ingroup group-constant
  @defgroup constant-Maxinit Maxinit (function template)

  Generates the constant maxinit which is identical to minf for
  arithmetic real types but that must be specialized for complex values ordering.
  maxinit is the value to be returned when a maximum of the elements function
  is used on an  empty container.

  @headerref{<boost/simd/constant/maxinit.hpp>}

  @par Description

  1.  @code
      template<typename T> T Maxinit();
      @endcode

  2.  @code
      template<typename T> T Maxinit( boost::simd::as_<T> const& target );
      @endcode

  Generates a value of type @c T containing the Maxinit constant.

  @par Parameters

  | Name                | Description                                                         |
  |--------------------:|:--------------------------------------------------------------------|
  | **target**          | a [placeholder](@ref type-as) value encapsulating the constant type |

  @par Return Value
  A value of type @c T that evaluates to `Minf<T>()`.

  @par Requirements
  - **T** models Value
**/

#include <boost/simd/constant/scalar/maxinit.hpp>
#include <boost/simd/constant/simd/maxinit.hpp>

#endif


#include <complex>
#include <iostream>
#include <fstream>
#include <string>

#include <boost/regex.hpp>
#include <boost/filesystem.hpp>

#include <Eigen/Core>

#include <vtkDataArraySelection.h>
#include <vtkXMLImageDataReader.h>

#include "FullSampleSimulation.h"
#include "DropletSampleSimulation.h"

using json = nlohmann::json;

void print_usage() {
	std::cout <<
		std::endl <<
		"Usage: ./rt-solver [ -h | -c FILE | -x FILE ]" <<
		std::endl << std::endl <<
		"\t-h" << std::endl <<
		"\t\tPrint this message and exit." <<
		std::endl << std::endl <<
		"\t-c FILE" << std::endl <<
		"\t\tCreate a default settings file with the specified name and exit." <<
		std::endl << std::endl <<
		"\t-x FILE" << std::endl <<
		"\t\tRun the code with the specified settings file." <<
		std::endl << std::endl;
}

std::string minify_json(std::string filename) {

	std::ifstream f(filename);
	if(!f.is_open())
		throw std::string(
			"Could not open the specified setting file");

	boost::regex empty_re("\\s*");
	boost::regex comment_re("\\s*#.*");
	boost::smatch match_result;

	std::string line, minified_json;
	while(std::getline(f, line)) {
		if(!boost::regex_match(line, match_result, empty_re) &&
				!boost::regex_match(line, match_result, comment_re))
			minified_json.append(line+"\n");
	}
	return minified_json;
}

void create_default_settings_file(std::string filename) {

	const char *file_content = R"V0G0N(
{
	"Light source": {
		# Widths (µm) of the light source (array of size [spatial_dim-1]). Should be smaller
		# than the director field mesh in the transverse direction if the sample type is
		# "Full".
		"Source widths": [10, 10],

		# Number of rays per dimension for the light source
		"Source N rays per dim": [200, 200],

		# Mean wavelength (µm) for the spectrum of the source
		"Mean wavelength": 0.6,

		# Full width (µm) of the light spectrum
		"Spectral FWHM": 0.2,

		# Number of wavelengths in the spectrum
		"N wavelengths": 1
	},
	"Geometry": {
		# Type of sample: "Droplet" or "Full". If "Droplet" is choosed, the given vti file for
		# the director field should have a cubic mesh (only values inside a sphere of same
		# diameter than the mesh will be considered).
		"Sample type": "Full",

		# Relative path to a VTI file containing a VTK array "n" for the director values.
		# IMPORTANT: the specified director field should have C_1 regularity (which exclude
		# the presence of defects and n->-n jumps).
		"Director field VTI file": "",

		"Droplet sample parameters": {
			# Since the ray mapping is singular near the vertical part of the droplet boundary,
			# the light source for droplet rays is shrinked by the following factor
			"Source shrink factor": 0.92,

			# Distance (µm) between the boundary of the droplet and the sample plates (the
			# droplet is centered inside the sample).
			"Distance from upper sample plate": 10
		}
	},
	"Material properties": {
		# Ordinary refractive index of the liquid crystal
		"Ordinary refractive index": 1.5,

		# Extraordinary refractive index of the liquid crystal
		"Extraordinary refractive index": 1.6,

		# Refractive index of the host fluid in case of a droplet sample
		"Host fluid refractive index": 1.55,

		# Refractive index of the external medium outside the sample
		"External medium refractive index": 1,

		# Refractive indices of the isotropic layers defining the lower part of the sample
		# (light propagates from bottom to top in the z direction, layers should be specified
		# with increasing z)
		"Refractive indices of the lower isotropic layers": [1.5],

		# Refractive indices of the isotropic layers defining the upper part of the sample
		# (light propagates from bottom to top in the z direction, layers should be specified
		# with increasing z)
		"Refractive indices of the upper isotropic layers": [1.5],

		# Thicknesses of the isotropic layers defining the lower part of the sample
		# (light propagates from bottom to top in the z direction, layers should be specified
		# with increasing z)
		"Thicknesses of the lower isotropic layers": [1000],

		# Thicknesses of the isotropic layers defining the upper part of the sample
		# (light propagates from bottom to top in the z direction, layers should be specified
		# with increasing z)
		"Thicknesses of the upper isotropic layers": [1000]
	},
	"Visualisation": {
		# Results will be stored in this folder (automatically created if it does not exists)
		"Results folder name": "results/",

		# Widths (µm) of the target plane on which fields are reconstructed. Will be used to
		# set the transverse size of output data. Should be smaller than the source widths.
		"Target output widths": [8, 8],

		# Number of pixels per dimension for the target plane on which fields are
		# reconstructed. Will be used to set the transverse dims of output data
		"Target N pixels per dim": [100, 100],

		"Bulk output": {
			# Base name for bulk data files
			"Base name": "bulk",

			# Should we run the homotopy continuation algorithm to reconstruct and save optical
			# fields in the bulk of the liquid crystal?
			"Export reconstructed fields": true,

			# Should we save bulk data associated with the rays?
			"Export ray data": true
		},
		"Screen output": {
			# Base name for screen data files
			"Base name": "screen",

			# Should we run the homotopy continuation algorithm to reconstruct and save optical
			# fields on the focal plane?
			"Export reconstructed fields": true,

			# Should we save data associated with the rays on the focal plane?
			"Export ray data": true,

			# Numerical aperture of the focal lens
			"Numerical aperture": 0.4
		}
	},
	"InverseScreenMap parameters": {
		# Number of adaptative refinements when inverting the ray mapping. Should be tweaked to
		# get a good compromise between accuracy and speed.
		"N refinement cycles": 4,

		# Homotopy settings used in the coarse phase of the inversion process
		"Coarse step HC parameters": {
			# Tolerance of the newton algorithm when computing corrector steps
			"Newton tolerance": 1e-8,

			# The homotopy step size will be adjusted so that the newton algorithm for the
			# corrector step runs in at most N iteration, where N is set in this parameter.
			"Optimum newton step number": 4,

			# Maximum size for the homotopy step size (µm)
			"Max arclength step": 0.1,

			# Maximum distance (µm) run by a homotopy path before stopping the search for
			# inverses.
			"Max arclength": 6
		},

		# Homotopy settings used in the refinement phase of the inversion process
		"Refinement step HC parameters": {
			# Tolerance of the newton algorithm when computing corrector steps
			"Newton tolerance": 1e-8,

			# The homotopy step size will be adjusted so that the newton algorithm for the
			# corrector step runs in at most N iteration, where N is set in this parameter.
			"Optimum newton step number": 4,

			# Maximum size for the homotopy step size (µm)
			"Max arclength step": 0.1,

			# Maximum distance (µm) run by a homotopy path before stopping the search for
			# inverses.
			"Max arclength": 6
		}
	}
})V0G0N";

	std::ofstream f(filename);
	if(!f.is_open())
		throw std::string(
			"Could not open the specified setting file, maybe you"
			"prepended a nonexistent directory?");

	f << file_content;
}

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

	std::string param_file_name;

	bool failed_parsing = (argc>1) ? false : true;
	int i=1;
	while(i<argc) {
		if(strcmp(argv[i],"-x")==0) {
			if(argv[++i][0]!='-') {
				param_file_name = argv[i];
				break;
			}
			else {
				failed_parsing = true;
				break;
			}
		}
		else if(strcmp(argv[i],"-h")==0) {
			print_usage();
			return 0;
		}
		else if(strcmp(argv[i],"-c")==0) {
			if(argv[++i][0]!='-') {
				create_default_settings_file(argv[i]);
				return 0;
			}
		}
		else {
			failed_parsing = true;
			break;
		}
	}
	if(failed_parsing) {
		std::cout <<
			std::endl << "Syntax error!" << std::endl;
		print_usage();
		return -1;
	}

	Eigen::initParallel();
	json j;

	try {
		j = json::parse(minify_json(param_file_name));

		std::string filename = j.at("Geometry").at("Director field VTI file");
		boost::filesystem::path path(filename);
		if(!boost::filesystem::exists(path))
			throw std::string(
				"The given vti file for the director file does not exists");

		vtkObject::GlobalWarningDisplayOff();

		std::cout <<
			"Loading director field values" << std::endl;
		auto reader = vtkSmartPointer<vtkXMLImageDataReader>::New();
		reader->SetFileName(filename.c_str());
		reader->UpdateInformation();

		bool found_n_data = false;
		for(unsigned int i=0; i<reader->GetNumberOfPointArrays(); i++) {
			auto array_name = reader->GetPointArrayName(i);
			if(!std::strcmp(array_name, "n")) {
				found_n_data = true;
				reader->SetPointArrayStatus(array_name, 1);
			}
			else
				reader->SetPointArrayStatus(array_name, 0);
		}
		if(!found_n_data)
			throw std::string(
				"Could not find a vector field named \"n\" in the given vti file");

		reader->Update();
		auto nfield_vti_data = reader->GetOutput();
		auto nfield_vti_vals = vtkDoubleArray::FastDownCast(
			nfield_vti_data->GetPointData()->GetAbstractArray("n"));

		int dims[3];			nfield_vti_data->GetDimensions(dims);
		double spacings[3];		nfield_vti_data->GetSpacing(spacings);

		unsigned int spatial_dim;
		if(dims[2]<=1)
			throw std::string(
				"Not enough points in the z-direction");
		else if(dims[0]==1 && dims[1]==1)
			throw std::string(
				"1D samples not supported");
		else if(dims[1]==1)
			throw std::string(
				"2D samples should be specified in the YZ plane");
		else if(dims[0]==1)
			spatial_dim = 2;
		else
			spatial_dim = 3;

		double lc_thickness = (dims[2]-1)*spacings[2];
		unsigned int N_lc_steps = dims[2];

		switch(spatial_dim) {
			case 2: {
				auto n_values = std::make_shared<std::vector<Vector<3,double> > >();
				Vector<3,double> n_val;
				for(int iy=-2; iy<dims[1]+2; iy++) {
					for(int iz=-2; iz<dims[2]+2; iz++) {
						if(iy>=0 && iy<dims[1] && iz>=0 && iz<dims[2])
							for(int c=0; c<3; c++)
								n_val(c) = nfield_vti_vals->GetComponent(iy+dims[1]*iz, c);
						n_values->push_back(n_val);
					}
				}

				Vector<2,unsigned long> full_dims({dims[1]+4,dims[2]+4});
				Vector<2,double> full_origin(
					{-(dims[1]+3)*spacings[1]/2.,-(dims[2]+3)*spacings[2]/2.});
				Vector<2,double> full_lengths(
					{(dims[1]+3)*spacings[1],(dims[2]+3)*spacings[2]});
				auto full_mesh = std::make_shared<CartesianMesh<2> >(
					full_origin, full_lengths, full_dims);

				std::shared_ptr<Simulation<2> > sim;
				if(j.at("Geometry").at("Sample type") == "Full") {
					Vector<2,unsigned long> lc_dims({dims[1],dims[2]});
					Vector<2,double> lc_origin(
						{-(dims[1]-1)*spacings[1]/2.,-(dims[2]-1)*spacings[2]/2.});
					Vector<2,double> lc_lengths(
						{(dims[1]-1)*spacings[1],(dims[2]-1)*spacings[2]});
					auto lc_mesh = std::make_shared<CartesianMesh<2> >(
						lc_origin, lc_lengths, lc_dims);
					auto lc_domain = std::make_shared<ParallelotopeDomain<2> >(*lc_mesh);
					auto n_field = std::make_shared<CubicInterpolatedMapping<2,3,double> >(
						n_values, full_mesh, lc_domain);
					n_field->normalize();

					sim = std::make_shared<FullSampleSimulation<2> >(
						j, lc_thickness, N_lc_steps, n_field);
				}
				else
					throw(std::string(
						"Error: \"Sample type\" value should be \"Full\"\n"
						"in dimension 2."));
				sim->run();
				break;
			}
			case 3: {
				auto n_values = std::make_shared<std::vector<Vector<3,double> > >();
				Vector<3,double> n_val;
				for(int ix=-2; ix<dims[0]+2; ix++) {
					for(int iy=-2; iy<dims[1]+2; iy++) {
						for(int iz=-2; iz<dims[2]+2; iz++) {
							if(ix>=0 && ix<dims[0] && iy>=0 && iy<dims[1]
									&& iz>=0 && iz<dims[2])
								for(int c=0; c<3; c++)
									n_val(c) = nfield_vti_vals->GetComponent(
										ix+dims[0]*(iy+dims[1]*iz), c);
							else
								n_val = 0;
							n_values->push_back(n_val);
						}
					}
				}

				Vector<3,unsigned long> full_dims({dims[0]+4,dims[1]+4,dims[2]+4});
				Vector<3,double> full_origin({-(dims[0]+3)*spacings[0]/2.,
					-(dims[1]+3)*spacings[1]/2.,-(dims[2]+3)*spacings[2]/2.});
				Vector<3,double> full_lengths({(dims[0]+3)*spacings[0],
					(dims[1]+3)*spacings[1],(dims[2]+3)*spacings[2]});
				auto full_mesh = std::make_shared<CartesianMesh<3> >(
					full_origin, full_lengths, full_dims);

				std::shared_ptr<Simulation<3> > sim;
				if(j.at("Geometry").at("Sample type") == "Full") {
					Vector<3,unsigned long> lc_dims({dims[0],dims[1],dims[2]});
					Vector<3,double> lc_origin({-(dims[0]-1)*spacings[0]/2.,
						-(dims[1]-1)*spacings[1]/2.,-(dims[2]-1)*spacings[2]/2.});
					Vector<3,double> lc_lengths({(dims[0]-1)*spacings[0],
						(dims[1]-1)*spacings[1],(dims[2]-1)*spacings[2]});
					auto lc_mesh = std::make_shared<CartesianMesh<3> >(
						lc_origin, lc_lengths, lc_dims);
					auto lc_domain = std::make_shared<ParallelotopeDomain<3> >(*lc_mesh);
					auto n_field = std::make_shared<CubicInterpolatedMapping<3,3,double> >(
						n_values, full_mesh, lc_domain);
					n_field->normalize();

					sim = std::make_shared<FullSampleSimulation<3> >(
						j, lc_thickness, N_lc_steps, n_field);
				}
				else if(j.at("Geometry").at("Sample type") == "Droplet") {
					auto lc_domain = std::make_shared<SphericalDomain<3> >(
						Vector<3,double>({0,0,0}), (dims[2]-1)*spacings[2]/2.);
					auto n_field = std::make_shared<CubicInterpolatedMapping<3,3,double> >(
						n_values, full_mesh, lc_domain);
					n_field->extrapolate_data();
					n_field->normalize();
					lc_thickness += 2*j.at("Geometry").at("Droplet sample parameters").at(
						"Distance from upper sample plate").get<double>();

					sim = std::make_shared<DropletSampleSimulation>(
						j, lc_thickness, N_lc_steps, n_field);
				}
				else
					throw(std::string(
						"Error: \"Sample type\" value should be \"Full\"\n"
						"or \"Droplet\" in dimension 3."));
				sim->run();
				break;
			}
		}
	}
	catch(std::string &s) {
		std::cerr <<
			std::endl << std::endl <<
			"----------------------------------------------------------------" <<
			std::endl <<
			"Exception on processing: " << std::endl <<
			s << std::endl <<
			"Aborting!" << std::endl <<
			"----------------------------------------------------------------" <<
			std::endl << std::endl;
		return -1;
	}
	catch(std::exception &exc) {
		std::cerr <<
			std::endl << std::endl <<
			"----------------------------------------------------------------" <<
			std::endl <<
			"Exception on processing: " << std::endl <<
			exc.what() << std::endl <<
			"Aborting!" << std::endl <<
			"----------------------------------------------------------------" <<
			std::endl << std::endl;
		return -1;
	}
	catch(...) {
		std::cerr <<
			std::endl << std::endl <<
			"----------------------------------------------------------------" <<
			std::endl <<
			"Unknown exception!" << std::endl <<
			"Aborting!" << std::endl <<
			"----------------------------------------------------------------" <<
			std::endl << std::endl;
		return -1;
	}

	return 0;
}


//
// Copyright (c) 2009 Rutger ter Borg
//
// 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)
//

#ifndef BOOST_NUMERIC_BINDINGS_STRIDE_HPP
#define BOOST_NUMERIC_BINDINGS_STRIDE_HPP

#include <boost/numeric/bindings/size.hpp>
#include <boost/mpl/min.hpp>
#include <boost/mpl/and.hpp>
#include <boost/mpl/less_equal.hpp>
#include <boost/mpl/equal_to.hpp>
#include <boost/mpl/range_c.hpp>
#include <boost/mpl/times.hpp>
#include <boost/mpl/greater.hpp>
#include <boost/mpl/plus.hpp>
#include <boost/type_traits/is_same.hpp>
#include <boost/static_assert.hpp>

namespace boost {
namespace numeric {
namespace bindings {
namespace detail {

template< typename T, typename AddressingIndex, typename Enable = void >
struct stride_impl {

    typedef tag::stride_type< AddressingIndex::value > key_type;
    typedef typename result_of_get< T, key_type >::type result_type;

    static result_type invoke( const T& t ) {
        return get< key_type >( t );
    }

};

//
// Strides for ranks outside the scope of the object are fixed at
// the dot product of its existing sizes and strides.
//
// Object    rank    result of strideN(), with N > rank
// scalar    0       1
// vector    1       size1 * stride1
// matrix    2       size1 * stride1 + size2 * stride2
// tensor    N       sum_i( size_i, stride_i )  (dot( size, stride))
//
// Iff size_i and stride_i are integral constants, results will be known at
// compile time. Otherwise, the result_type will be std::ptrdiff_t.
//
template< typename T, typename AddressingIndex >
struct stride_impl< T, AddressingIndex,
        typename boost::enable_if<
            mpl::equal_to< rank<T>, tag::scalar >
        >::type > {

    typedef typename mpl::int_<1> result_type;

    static result_type invoke( const T& t ) {
        return result_type();
    }

};

template< typename T, typename State, typename AddressingIndex >
struct fold_stride_size {

    typedef tag::addressing_index< AddressingIndex::value > index_type;
    typedef typename result_of::size< T, index_type >::type size_type;
    typedef typename stride_impl< T, index_type >::result_type stride_type;

    typedef typename mpl::if_<
        mpl::or_<
            is_same< State, std::ptrdiff_t >,
            is_same< size_type, std::ptrdiff_t >,
            is_same< stride_type, std::ptrdiff_t >
        >,
        std::ptrdiff_t,
        mpl::plus<
            State,
            mpl::times<
                size_type,
                stride_type
            >
        >
    >::type type;

};

//
// If Result isn't a ptrdiff_t, just invoke the integral constant
// and return that. Otherwise, runtime stuff is involved, so we'll
// have to evaluate sum_i( size_i, stride_i ).
//
template< typename T, typename Result, int AddressingIndex >
struct apply_fold {
    static Result invoke( const T& t ) {
        return Result();
    }
};

template< typename T, int AddressingIndex >
struct apply_fold< T, std::ptrdiff_t, AddressingIndex > {

    static std::ptrdiff_t invoke( const T& t ) {
        return size( t, tag::addressing_index< AddressingIndex >() ) *
            stride_impl< T, tag::addressing_index< AddressingIndex > >::invoke( t ) +
            apply_fold< T, std::ptrdiff_t, AddressingIndex-1 >::invoke( t );
    }

};

template< typename T >
struct apply_fold< T, std::ptrdiff_t, 0 > {

    static std::ptrdiff_t invoke( const T& ) {
        return 0;
    }

};

// Could be made generic for dimensions > 2,
//  but not enough time right now

template< typename T, typename AddressingIndex >
struct stride_impl< T, AddressingIndex,
        typename boost::enable_if<
            mpl::and_<
                 mpl::greater< rank<T>, tag::scalar >,
                 mpl::greater< AddressingIndex, rank<T> >
           >
        >::type > {

    typedef mpl::range_c< int, 1, rank<T>::value+1 > index_range;
    typedef typename mpl::fold<
        index_range,
        mpl::int_< 0 >,
        fold_stride_size<
            T,
            mpl::_1,
            mpl::_2
        >
    >::type result_type;

    static result_type invoke( const T& t ) {
        return apply_fold< T, result_type, rank<T>::value >::invoke( t );
    }

};

} // namespace detail

namespace result_of {

template< typename T, typename Tag = tag::addressing_index<1> >
struct stride {
    BOOST_STATIC_ASSERT( (is_tag<Tag>::value) );
    typedef typename detail::stride_impl< T, Tag >::result_type type;
};

} // namespace result_of

//
// Overloads for free template functions stride( x, tag ),
//
template< typename T, typename Tag >
inline typename result_of::stride< const T, Tag >::type
stride( const T& t, Tag ) {
    return detail::stride_impl< const T, Tag >::invoke( t );
}

// Overloads for free template functions stride( x )
// Valid for types with rank <= 1 (scalars, vectors)
// In theory, we could provide overloads for matrices here, too,
// if their minimal_rank is at most 1.

template< typename T >
inline typename
boost::enable_if<
    mpl::less< rank<T>, mpl::int_<2> >,
    typename result_of::stride< const T, tag::addressing_index<1> >::type
>::type
stride( const T& t ) {
    return detail::stride_impl<const T, tag::addressing_index<1> >::invoke( t );
}

#define GENERATE_STRIDE_INDEX( z, which, unused ) \
GENERATE_FUNCTIONS( stride, which, tag::addressing_index<which> )

BOOST_PP_REPEAT_FROM_TO(1,3,GENERATE_STRIDE_INDEX,~)

GENERATE_FUNCTIONS( stride, _row, tag::addressing_index<1> )
GENERATE_FUNCTIONS( stride, _column, tag::addressing_index<2> )
GENERATE_FUNCTIONS( stride, _major, typename addressing_index_major<T>::type )
GENERATE_FUNCTIONS( stride, _minor, typename addressing_index_minor<T>::type )

} // namespace bindings
} // namespace numeric
} // namespace boost

#endif


// Difference from running average.

// Import standard headers.
#include <vector>
#include <sstream>

// Import 3rd party headers.
#include <boost/date_time.hpp>
#include <opencv2/opencv.hpp>
#include <bites.hpp>

// Import application headers.
#include "sherlock.hpp"

int main(int argc, char** argv)
{
    // Parse command-line arguments.
    int DEVICE, WIDTH, HEIGHT, DURATION;
    std::istringstream(std::string(argv[1])) >> DEVICE;
    std::istringstream(std::string(argv[2])) >> WIDTH;
    std::istringstream(std::string(argv[3])) >> HEIGHT;
    std::istringstream(std::string(argv[4])) >> DURATION;

    // Create the OpenCV video capture object.
    cv::VideoCapture cap(DEVICE);
    cap.set(3, WIDTH);
    cap.set(4, HEIGHT);

    // Create the output window.
    const char* title = "diff average 1";
    cv::namedWindow(title, CV_WINDOW_NORMAL);

    // Maintain accumulation of differences.
    cv::Mat image_acc;

    // Keep track of previous iteration's timestamp.
    boost::posix_time::ptime tstamp_prev;

    // Monitor framerates for the given seconds past.
    std::vector<float> periods = { 1, 5, 10 };
    bites::RateTicker framerate (periods);

    // Run the loop for designated amount of time.
    auto now = boost::posix_time::microsec_clock::universal_time();
    auto dur = boost::posix_time::seconds(DURATION);
    auto end = now + dur;
    while (end > boost::posix_time::microsec_clock::universal_time())
    {
        // Take a snapshot.
        cv::Mat frame;
        cap >> frame;

        const auto RTYPE = CV_32FC3;

        // Compute alpha value.
        auto alpha = sherlock::getAlpha(tstamp_prev, 1.0);

        // Initalize accumulation if so indicated.
        if (image_acc.empty()){
            image_acc = cv::Mat::ones(frame.size(), frame.type());
            image_acc.convertTo(image_acc, RTYPE);
        }

        // Compute difference.
        cv::Mat converted;
        frame.convertTo(converted, RTYPE);
        cv::Mat image_diff;
        cv::absdiff(
            image_acc,
            converted,
            image_diff
            );

        // Accumulate.
        cv::accumulateWeighted(
            converted,
            image_acc,
            alpha
            );

        image_diff.convertTo(image_diff, frame.type());

        // Write the framerate on top of the image.
        auto fps = framerate.tick();
        std::ostringstream line;
        line << std::fixed << std::setprecision(2);
        line << fps[0] << ", " << fps[1] << ", " << fps[2];
        std::list<std::string> lines;
        lines.push_back(line.str());
        sherlock::writeOSD(image_diff, lines, 0.04);

        // Display the snapshot.
        cv::imshow(title, image_diff);

        // Allow HighGUI to process event.
        cv::waitKey(1);
    }
}


// Boost enable_if library

// Copyright 2003 (c) The Trustees of Indiana University.

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

//    Authors: Jaakko Jarvi (jajarvi at osl.iu.edu)
//             Jeremiah Willcock (jewillco at osl.iu.edu)
//             Andrew Lumsdaine (lums at osl.iu.edu)

#include <boost/test/minimal.hpp>

#include <boost/utility/enable_if.hpp>
#include <boost/type_traits/is_arithmetic.hpp>

using boost::enable_if_c;
using boost::disable_if_c;
using boost::enable_if;
using boost::disable_if;
using boost::is_arithmetic;

template <class T, class Enable = void>
struct tester;

template <class T>
struct tester<T, typename enable_if_c<is_arithmetic<T>::value>::type> {
  BOOST_STATIC_CONSTANT(bool, value = true);
};

template <class T>
struct tester<T, typename disable_if_c<is_arithmetic<T>::value>::type> {
  BOOST_STATIC_CONSTANT(bool, value = false);
};

template <class T, class Enable = void>
struct tester2;

template <class T>
struct tester2<T, typename enable_if<is_arithmetic<T> >::type> {
  BOOST_STATIC_CONSTANT(bool, value = true);
};

template <class T>
struct tester2<T, typename disable_if<is_arithmetic<T> >::type> {
  BOOST_STATIC_CONSTANT(bool, value = false);
};

int test_main(int, char*[])
{

  BOOST_CHECK(tester<int>::value);
  BOOST_CHECK(tester<double>::value);

  BOOST_CHECK(!tester<char*>::value);
  BOOST_CHECK(!tester<void*>::value);

  BOOST_CHECK(tester2<int>::value);
  BOOST_CHECK(tester2<double>::value);

  BOOST_CHECK(!tester2<char*>::value);
  BOOST_CHECK(!tester2<void*>::value);

  return 0;
}


#include <boost/math/tools/tuple.hpp>


// Copyright (c) 2019 Graphcore Ltd. All rights reserved.
#define BOOST_TEST_MODULE ConstExprUnsqueezeTest

#include <memory>
#include <vector>

#include <boost/test/unit_test.hpp>

#include <filereader.hpp>
#include <popart/builder.hpp>
#include <popart/dataflow.hpp>
#include <popart/inputshapeinfo.hpp>
#include <popart/ir.hpp>
#include <popart/names.hpp>
#include <popart/op/identity.hpp>
#include <popart/op/l1.hpp>
#include <popart/sgd.hpp>
#include <popart/tensor.hpp>
#include <popart/tensordata.hpp>
#include <popart/tensors.hpp>
#include <popart/testdevice.hpp>

using namespace popart;

BOOST_AUTO_TEST_CASE(ConstExprTest_Unsqueeze0) {
  // clang-format off
  //
  // {(c0) -> [Unsqueeze] -> (h0)
  // {(h0), (in1)} -> [Add] -> (*)
  //
  // where c0 is a constant, should become
  //
  // {(h0), (in1)} -> [Add] -> (*)
  //
  // clang-format on

  std::vector<float> raw_const_data(2 * 3 * 4, 0.0);
  ConstVoidData const_data = {raw_const_data.data(), {"FLOAT", Shape{2, 3, 4}}};

  TensorInfo in_info{"FLOAT", Shape{2, 3, 1, 4, 1}};

  auto builder          = Builder::create();
  auto aiOnnx           = builder->aiOnnxOpset9();
  auto const_node       = aiOnnx.constant(const_data, "const_data");
  auto unsqueeze_output = aiOnnx.unsqueeze({const_node}, {2, 4});
  auto in_id            = builder->addInputTensor(in_info);
  auto out_id           = aiOnnx.add({unsqueeze_output, in_id});
  auto l1               = builder->aiGraphcoreOpset1().l1loss({out_id}, 0.1);

  auto proto       = builder->getModelProto();
  auto model_proto = io::getModelFromString(proto);

  // Create the IR, adding outId as an anchor
  auto art       = AnchorReturnType("All");
  auto data_flow = DataFlow(1, {{out_id, art}});
  auto optimizer = ConstSGD(0.01);
  auto device    = createTestDevice(TEST_TARGET);

  Ir ir;
  ir.prepare({model_proto,
              InputShapeInfo(),
              data_flow,
              l1,
              &optimizer,
              *device,
              {}, // no SessionOptions
              Patterns({}).enableRuntimeAsserts(false)});

  // Check the ir
  // 1) that the Add Op is present,
  BOOST_CHECK(ir.opsOfType(Onnx::AiOnnx::OpSet9::Add).size() == 1);
  // 2) that the Concat Op is not present,
  BOOST_CHECK(ir.opsOfType(Onnx::AiOnnx::OpSet9::Unsqueeze).size() == 0);
  // 3) that the shape of the output tensor is as specified.
  Shape ref_shape{2, 3, 1, 4, 1};
  BOOST_CHECK(ir.getMainGraphTensors().get(out_id)->info.shape() == ref_shape);
}


#include <boost/circular_buffer.hpp>

#include "fitness_graph.h"
#include "ui_fitness_graph.h"

#include "../species.h"

fitness_graph::fitness_graph(QWidget *parent) :
	QWidget(parent),
	ui(new Ui::fitness_graph)
{
	ui->setupUi(this);

	auto chart_fitness = new QtCharts::QChart();
	chart_fitness->setTheme(QtCharts::QChart::ChartThemeDark);

	series_fitness = new QtCharts::QLineSeries();
	series_min_fitness = new QtCharts::QLineSeries();
	series_max_fitness = new QtCharts::QLineSeries();

	series_fitness_avg = new QtCharts::QLineSeries();
	series_min_fitness_avg = new QtCharts::QLineSeries();
	series_max_fitness_avg = new QtCharts::QLineSeries();

	QPen pen;
	pen.setStyle(Qt::DashLine);
	pen.setColor("white");
	series_fitness_avg->setPen(pen);
	series_min_fitness_avg->setPen(pen);
	series_max_fitness_avg->setPen(pen);

	chart_fitness->addSeries(series_fitness);
	chart_fitness->addSeries(series_min_fitness);
	chart_fitness->addSeries(series_max_fitness);
	chart_fitness->addSeries(series_fitness_avg);
	chart_fitness->addSeries(series_min_fitness_avg);
	chart_fitness->addSeries(series_max_fitness_avg);

	chart_fitness->createDefaultAxes();
	chart_fitness->legend()->hide();
	ui->fitness->setChart(chart_fitness);
	ui->fitness->setRenderHint(QPainter::Antialiasing);
	ui->fitness->setRubberBand(QtCharts::QChartView::RectangleRubberBand);

	auto_range = true;

	resize_avg_window(21);
}

fitness_graph::~fitness_graph()
{
	delete ui;
}

void fitness_graph::insert_fitness(int epoch, double fitness, double minimum_fitness, double maximum_fitness) {
	series_fitness->append(epoch, fitness);
	series_min_fitness->append(epoch, minimum_fitness);
	series_max_fitness->append(epoch, maximum_fitness);
	append_avg(epoch, fitness, minimum_fitness, maximum_fitness);

	const auto [min, max] = std::minmax({ fitness, minimum_fitness, maximum_fitness });
	if(epoch < series_range.xmin) series_range.xmin = epoch;
	if(min < series_range.ymin) series_range.ymin = min;
	if(epoch > series_range.xmax) series_range.xmax = epoch;
	if(max > series_range.ymax) series_range.ymax = max;
	if(auto_range) {
		ui->fitness->chart()->axisX()->setRange(series_range.xmin, series_range.xmax);
		ui->fitness->chart()->axisY()->setRange(series_range.ymin, series_range.ymax);
	}
}

void fitness_graph::on_auto_range_clicked(bool checked) {
	if(checked) {
		ui->fitness->chart()->axisX()->setRange(series_range.xmin, series_range.xmax);
		ui->fitness->chart()->axisY()->setRange(series_range.ymin, series_range.ymax);
	}
	auto_range = checked;
}

void fitness_graph::resize_avg_window(size_t size) {
	score_total_avg = 0;
	score_total_min = 0;
	score_total_max = 0;

	// Set window size to highest odd number not larger than size
	score_window.set_capacity((size % 2 == 1) ? size : size - 1);
	score_window.clear();
}

void fitness_graph::append_avg(double epoch, double fitness, double minimum_fitness, double maximum_fitness) {

	// Remove oldest value from total scores if window is full
	if(score_window.full()) {
		const auto [avg, min, max] = score_window.front();
		score_total_avg -= avg;
		score_total_min -= min;
		score_total_max -= max;
	}

	// Add new values to window
	score_window.push_back({fitness, minimum_fitness, maximum_fitness});

	// Add the new values to the totals
	score_total_avg += fitness;
	score_total_min += minimum_fitness;
	score_total_max += maximum_fitness;

	// Plot the average if we have enough data
	const auto centre = epoch - (score_window.capacity() - 1) / 2;
	if(centre >= 0) {
		series_fitness_avg->append(centre, score_total_avg / score_window.size());
		series_max_fitness_avg->append(centre, score_total_min / score_window.size());
		series_min_fitness_avg->append(centre, score_total_max / score_window.size());
	}
}


#include <boost/chrono.hpp>
#include <iostream>
#include <boost/thread/thread.hpp>
#include <vector>

boost::thread sleepsort(const std::vector<int>& v)
{
    // The grouping thread
    return boost::thread{ [v]() {
        std::vector<boost::thread> threads;
        threads.reserve(v.size());

        // The sleep-sort algo {{{
        for (auto e: v) {
            threads.emplace_back(
                [e]() {
                    boost::this_thread::sleep_for(boost::chrono::seconds(e));
                    std::cout << e << std::endl;
                }
            );
        }
        // }}}

        for (auto& t : threads) {
            t.join();
        }
    } };
}

int main(int argc, char* argv[]) {
    std::vector<int> v{8, 42, 38, 111, 2, 39, 1};
    sleepsort(v).join();
}


#pragma once
#include <memory>
#include <boost/serialization/access.hpp>
#include <boost/serialization/base_object.hpp>
#include "SimpleLayer.hpp"
#include "../optimizer/StochasticGradientDescent.hpp"
#include "neuron/RecurrentNeuron.hpp"

namespace snn::internal
{
    class Recurrence final : public SimpleLayer<RecurrentNeuron>
    {
    private:
        friend class boost::serialization::access;
        template <class Archive>
        void serialize(Archive& ar, unsigned version);

    public:
        Recurrence() = default;  // use restricted to Boost library only
        Recurrence(LayerModel& model, std::shared_ptr<NeuralNetworkOptimizer> optimizer);
        Recurrence(const Recurrence&) = default;
        ~Recurrence() = default;
        std::unique_ptr<BaseLayer> clone(std::shared_ptr<NeuralNetworkOptimizer> optimizer) const override;
    };

    template <class Archive>
    void Recurrence::serialize(Archive& ar, const unsigned version)
    {
        boost::serialization::void_cast_register<Recurrence, SimpleLayer>();
        ar & boost::serialization::base_object<SimpleLayer>(*this);
    }
}


/**
 * Copyright (c) 2012 Andreas Heider, Julius Adorf, Markus Grimm
 *
 * MIT License (http://www.opensource.org/licenses/mit-license.php)
 */

#include <boost/foreach.hpp>

#include "tpofinder/util.h"

using namespace cv;
namespace bfs = boost::filesystem;

namespace tpofinder {

    Mat readHomography(const bfs::path& path) {
        FileStorage in(path.string(), FileStorage::READ);
        Mat h;
        in["homography"] >> h;
        in.release();
        return h;
    }

    void writeHomography(const bfs::path& path, const Mat& homography) {
        FileStorage out(path.string(), FileStorage::WRITE);
        out << "homography" << homography;
        out.release();
    }

    void invertHomography(const bfs::path& src, const bfs::path& dst) {
        Mat h = readHomography(src);
        Mat hInv;
        invert(h, hInv);
        hInv /= hInv.at<double>(2, 2);
        writeHomography(dst, hInv);
    }

    Scalar readColor(const bfs::path& path) {
        FileStorage in(path.string(), FileStorage::READ);
        Mat c;
        in["color"] >> c;
        in.release();
        return Scalar(c.at<uint8_t > (0, 0), c.at<uint8_t > (0, 1), c.at<uint8_t > (0, 2), c.at<uint8_t >(0, 3));
    }

    void writeColor(const bfs::path& path, const Scalar& color) {
        FileStorage out(path.string(), FileStorage::WRITE);
        Mat c = (Mat_<uint8_t > (1, 4) << color[0], color[1], color[2], color[3]);
        out << "color" << c;
        out.release();
    }

    void perspectiveTransformKeypoints(const vector<KeyPoint>& src,
            vector<KeyPoint>& dst, const Mat& mtx) {
        Mat_<Point2f> srcMat;
        for (size_t i = 0; i < src.size(); i++) {
            srcMat.push_back(src[i].pt);
        }
        Mat_<Point2f> dstMat;
        perspectiveTransform(srcMat, dstMat, mtx);
        for (size_t i = 0; i < src.size(); i++) {
            KeyPoint k = src[i];
            k.pt = dstMat.at<Point2f > (i);
            dst.push_back(k);
        }
    }

    vector<int> findInliers(const vector<Point2f>& pts1, const vector<Point2f>& pts2,
            const Mat& homography, const float reprojThreshold) {
        CV_Assert(pts1.size() == pts2.size());
        CV_Assert(!homography.empty());

        vector<int> inliers;
        vector<Point2f> pts1_;
        perspectiveTransform(pts1, pts1_, homography);
        for (size_t i = 0; i < pts1.size(); i++) {
            if (norm(pts1_[i] - pts2[i]) <= reprojThreshold) {
                inliers.push_back(i);
            }
        }
        return inliers;
    }

}


#ifndef HDGRAPH_HPP
#define HDGRAPH_HPP

#include <boost/graph/graph_utility.hpp>
#include "globals.hpp"
#include "rbgraph.hpp"

//=============================================================================
// Data structures

/**
  Scoped enumeration type whose underlying size is 1 byte, used for character
  state.

  State is paired with a character in the struct SignedCharacter.
*/
enum class State : bool {
  lose,  ///< The paired character is lost
  gain   ///< The paired character is gained
};

/**
  @brief Struct used to represent a signed character

  Each character c+ and c− is called a signed character.
*/
struct SignedCharacter {
  std::string character{};    ///< Character name
  State state = State::gain;  ///< Character state
};

//=============================================================================
// Bundled properties

/**
  @brief Struct used to represent the properties of an edge (Hasse diagram)

  The character c is gained in the edge (x, y) such that y is a child of x and
  c has state 0 in x and state 1 in y.
  In this case the edge (x, y) is labeled by c+.
  Conversely, c is lost in the edge (x, y) if y is a child of x and character c
  has state 1 in x and state 0 in y.
  In the latter case the edge (x, y) is labeled by c−.
  For each character c, we allow at most one edge labeled by c−.
*/
struct HDEdgeProperties {
  std::list<SignedCharacter> signedcharacters{};  ///< List of SignedCharacters
                                                  ///< that label the edge
};

/**
  @brief Struct used to represent the properties of a vertex (Hasse diagram)

  Given s a species, by C(s) we denote the set of characters of s.
  Let GM be a maximal reducible graph.
  Then the diagram P for GM is the Hasse diagram for the poset (Ps , ≤) of all
  species of GM ordered by the relation ≤, where s1 ≤ s2 if C(s1) ⊆ C(s2).
*/
struct HDVertexProperties {
  std::list<std::string> species{};  ///< List of species that label the vertex
  std::list<std::string> characters{};  ///< List of characters of the species
};

/**
  @brief Struct used to represent the properties of a Hasse diagram
*/
struct HDGraphProperties {
  const RBGraph* g{};   ///< Original red-black graph
  const RBGraph* gm{};  ///< Original maximal reducible graph
  size_t num_v; ///< Number of vertices
};

//=============================================================================
// Typedefs used for readabily

// Graph

/**
  Hasse diagram
*/
typedef boost::adjacency_list<boost::setS,            // OutEdgeList
                              boost::listS,            // VertexList
                              boost::bidirectionalS,  // Directed
                              HDVertexProperties,     // VertexProperties
                              HDEdgeProperties,       // EdgeProperties
                              HDGraphProperties       // GraphProperties
                              >
    HDGraph;

// Descriptors

/**
  Edge of a Hasse diagram
*/
typedef boost::graph_traits<HDGraph>::edge_descriptor HDEdge;

/**
  Vertex of a Hasse diagram
*/
typedef boost::graph_traits<HDGraph>::vertex_descriptor HDVertex;

// Iterators

/**
  Iterator of vertices (Hasse diagram)
*/
typedef boost::graph_traits<HDGraph>::vertex_iterator HDVertexIter;

/**
  Iterator of incoming edges (Hasse diagram)
*/
typedef boost::graph_traits<HDGraph>::in_edge_iterator HDInEdgeIter;

/**
  Iterator of outgoing edges (Hasse diagram)
*/
typedef boost::graph_traits<HDGraph>::out_edge_iterator HDOutEdgeIter;

/**
  Iterator (const) of a list of signed characters
*/
typedef std::list<SignedCharacter>::const_iterator SignedCharacterIter;

// Size types

/**
  Size type of vertices (Hasse diagram)
*/
typedef boost::graph_traits<HDGraph>::vertices_size_type HDVertexSize;

// Maps

/**
  Map of vertex indexes (Hasse diagram)
*/
typedef std::map<HDVertex, HDVertexSize> HDVertexIMap;

/**
  Associative property map of vertex indexes (Hasse diagram)
*/
typedef boost::associative_property_map<HDVertexIMap> HDVertexIAssocMap;

typedef std::map<HDVertex, size_t> HDVertexIndexMap;
typedef boost::associative_property_map<HDVertexIndexMap> HDVertexIndexAssocMap;

//=============================================================================
// Enum / Struct operator overloads

/**
  @brief Overloading of operator<< for State

  @param[in] os Output stream
  @param[in] s  State

  @return Updated output stream
*/
inline std::ostream& operator<<(std::ostream& os, const State s) {
  const bool sign = (s == State::lose);

  return os << (sign ? "-" : "+");
}

/**
  @brief Overloading of operator<< for SignedCharacter

  @param[in] os Output stream
  @param[in] sc SignedCharacter

  @return Updated output stream
*/
inline std::ostream& operator<<(std::ostream& os, const SignedCharacter sc) {
  return os << sc.character << sc.state;
}

/**
  @brief Overloading of operator== for a pair of signed characters

  @param[in] a SignedCharacter
  @param[in] b SignedCharacter

  @return True if a is equal to b
*/
inline bool operator==(const SignedCharacter& a, const SignedCharacter b) {
  return (a.character == b.character && a.state == b.state);
}

//=============================================================================
// Boost functions (overloading)

/**
  @brief Add vertex with \e species and \e characters to \e hasse

  @param[in]     species    List of species names
  @param[in]     characters List of character names
  @param[in,out] hasse      Hasse diagram graph

  @return Vertex descriptor for the new vertex
*/
HDVertex add_vertex(const std::list<std::string>& species,
                    const std::list<std::string>& characters, HDGraph& hasse);

/**
  @brief Add vertex with \e species and \e characters to \e hasse

  @param[in]     species    Species name
  @param[in]     characters List of character names
  @param[in,out] hasse      Hasse diagram graph

  @return Vertex descriptor for the new vertex
*/
inline HDVertex add_vertex(const std::string& species,
                           const std::list<std::string>& characters,
                           HDGraph& hasse) {
  return add_vertex(std::list<std::string>{species}, characters, hasse);
}

/**
  @brief Add edge between \e u and \e v with a list of signed characters to
         \e hasse

  @param[in]     u                Source Vertex
  @param[in]     v                Target Vertex
  @param[in]     signedcharacters List of signed characters
  @param[in,out] hasse            Hasse diagram graph

  @return Edge descriptor for the new edge.
          If the edge is already in the graph then a duplicate will not be
          added and the bool flag will be false.
          When the flag is false, the returned edge descriptor points to the
          already existing edge
*/
std::pair<HDEdge, bool> add_edge(
    const HDVertex u, const HDVertex v,
    const std::list<SignedCharacter>& signedcharacters, HDGraph& hasse);

//=============================================================================
// General functions

/**
  @brief Return a pointer to the original red-black graph of \e hasse

  @param[in] hasse Hasse diagram graph

  @return Pointer to the the original red-black graph of \e hasse
*/
inline const RBGraph* const orig_g(const HDGraph& hasse) {
  return hasse[boost::graph_bundle].g;
}

/**
  @brief Return a pointer to the original maximal reducible graph of \e hasse

  @param[in] hasse Hasse diagram graph

  @return Pointer to the the original maximal reducible graph of \e hasse
*/
inline const RBGraph* const orig_gm(const HDGraph& hasse) {
  return hasse[boost::graph_bundle].gm;
}

/**
  @brief Overloading of operator<< for HDGraph

  @param[in] os    Output stream
  @param[in] hasse Hasse diagram graph

  @return Updated output stream
*/
std::ostream& operator<<(std::ostream& os, const HDGraph& hasse);

//=============================================================================
// Algorithm functions

/**
  @brief Returns True if \e a is included in \e b

  @param[in] a List of character names (strings)
  @param[in] b List of character names (strings)

  @return True if \e a is included in \e b, False otherwise
*/
bool is_included(const std::list<std::string>& a,
                 const std::list<std::string>& b);

/**
  @brief Build the Hasse diagram of \e gm

  Let GM be a maximal reducible graph.
  Then the diagram P for GM is the Hasse diagram for the poset (Ps, ≤) of all
  species of GM ordered by the relation ≤, where s1 ≤ s2 if C(s1) ⊆ C(s2).
  Given (Ps, ≤) the poset of all species of a red-black graph, we
  consider the representation of the poset (Ps, ≤) by its Hasse diagram,
  represented by a directed acyclic graph P.
  More precisely, two species s1 and s2 are connected by the arc (s1, s2) if
  s1 < s2 and there does not exist a species s3 such that s1 < s3 < s2.

  @param[out] hasse Hasse diagram graph
  @param[in]  g     Red-black graph
  @param[in]  gm    Maximal reducible red-black graph
  @param[in]  components Vector of red-black connected subgraphs
  @param[in]  c_assocmap Connected Components map
*/
void hasse_diagram(HDGraph& hasse, const RBGraph& g, const RBGraph& gm, const RBGraphVector& components, const RBVertexIMap& c_map);

/**
  @brief Removes active species from an hasse diagram

  A reduced HDGraph is the one with active species eliminated.
  A specie is active if it has red edges incident to it.

  @param [in] hasse Hasse diagram graph
  @param [in] g     Red-black graph
  @param[in]  components Vector of red-black connected subgraphs
  @param[in]  c_assocmap Connected Components map

  @return Reduced Hasse diagram graph
*/
void reduce_diagram(HDGraph& hasse, const RBGraph& gm);

void transitive_reduction(HDGraph& hasse);
void remove_vertex(HDVertex& v, HDGraph& p);
inline size_t num_vertices(const HDGraph& hasse) {
  return hasse[boost::graph_bundle].num_v;
}

#endif  // HDGRAPH_HPP


// Copyright 2005 The Trustees of Indiana University.

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

//  Authors: Douglas Gregor
//           Andrew Lumsdaine
#include "graph_types.hpp"
#include "exports.hpp"
#include <boost/graph/python/generators.hpp>
#include <boost/python.hpp>
#include <string>
#include <boost/algorithm/string/replace.hpp>

namespace boost { namespace graph { namespace python {

const char* erdos_renyi_doc =
"erdos_renyi_graph(num_vertices, probability, allow_self_loops = False,\n"
"                  random_seed = 1) -> GRAPH\n\n"
"Constructs a new Erdos-Renyi random graph with num_vertices vertices and\n"
"a uniform probability of having an edge (u, v) in the graph for any\n"
"vertices u and v. Expect a graph with probability*num_vertices^2 edges.\n\n"
"Parameters:\n"
"  num_vertices      The number of vertices in the graph. \n"
"  probability       The probability of having an edge (u, v).\n"
"  allow_self_loops  Whether self-loops (u, u) will be generated.\n"
"  random_seed       Nonzero seed value for the random number generator.\n\n"
"Complete C++ documentation is available at:\n"
"  http://www.boost.org/libs/graph/doc/erdos_renyi_generator.html\n"

  ;

const char* plod_doc =
"plod_graph(num_vertices, alpha, beta, allow_self_loops = False, \n"
"           random_seed = 1) -> GRAPH\n\n"
"Constructs a new power law graph with num_vertices vertices. The number\n"
"of connections to a given vertex is beta*x^(-alpha), where alpha and\n"
"beta are parameters to the algorithm and x is a random variable between\n"
"0 and num_vertices - 1.\n\n"
"Parameters:\n"
"  num_vertices      The number of vertices in the graph.\n"
"  alpha             The exponential fall-off in the degree distribution.\n"
"  beta              Controls how many edges will occur in the graph.\n"
"  allow_self_loops  Whether self-loops (u, u) will be generated.\n"
"  random_seed       Nonzero seed value for the random number generator.\n\n"
"Complete C++ documentation is available at:\n"
"  http://www.boost.org/libs/graph/doc/plod_generator.html\n"
  ;

const char* small_world_doc =
"small_world_graph(num_vertices, num_neighbors, rewire_probability,\n"
"           allow_self_loops = False, random_seed = 1) -> GRAPH\n\n"
"Constructs a new small-world graph with num_vertices vertices, each\n"
"adjacent to its num_neighbors closest neighbors (assume that the \n"
"vertcices were arranged in a circle). With probability\n"
"rewire_probability, an edge will be rewired randomly.\n\n"
"Parameters:\n"
"  num_vertices          The number of vertices in the graph.\n"
"  num_neighbors         The number of neighbors each vertex starts with.\n"
"  rewire_probability    Probability of rewiring any given edge.\n"
"  allow_self_loops      Whether self-loops (u, u) will be generated.\n"
"  allow_multiple_edges  Whether multiple edges between vertices are generated.\n"
"  random_seed         Nonzero seed for the random number generator.\n\n"
"Complete C++ documentation is available at:\n"
"  http://www.boost.org/libs/graph/doc/small_world_generator.html\n"
  ;

template<typename Graph>
void export_generators(BGL_GRAPH_CLASS_(Graph)& graph, const char* name)
{
  using boost::python::arg;
  using boost::python::def;
  using boost::python::manage_new_object;
  using boost::python::return_value_policy;

  std::string my_erdos_renyi_doc(erdos_renyi_doc);
  algorithm::replace_all(my_erdos_renyi_doc, std::string("GRAPH"),
                         std::string(name));
  std::string my_plod_doc(plod_doc);
  algorithm::replace_all(my_plod_doc, std::string("GRAPH"), std::string(name));
  std::string my_small_world_doc(small_world_doc);
  algorithm::replace_all(my_small_world_doc,
                         std::string("GRAPH"), std::string(name));

  // Generators
  graph
    .def("erdos_renyi_graph",
         &boost::graph::python::erdos_renyi_graph<Graph>,
         (arg("num_vertices"), arg("probability"),
          arg("allow_self_loops") = false, arg("random_seed") = 1),
         my_erdos_renyi_doc.c_str())
    .staticmethod("erdos_renyi_graph")
    .def("plod_graph",
         &boost::graph::python::plod_graph<Graph>,
         (arg("num_vertices"), arg("alpha"), arg("beta"),
          arg("allow_self_loops") = false, arg("random_seed") = 1),
         my_plod_doc.c_str())
    .staticmethod("plod_graph")
    .def("small_world_graph",
         &boost::graph::python::small_world_graph<Graph>,
         (arg("num_vertices"), arg("num_neighbors"), arg("rewire_probability"),
          arg("allow_self_loops") = false, arg("allow_multiple_edges") = true,
	  arg("random_seed") = 1),
         my_small_world_doc.c_str())
    .staticmethod("small_world_graph");
}

#define UNDIRECTED_GRAPH(Name,Type) \
  template void export_generators<Type>(BGL_GRAPH_CLASS_(Type)&, \
                                        const char*);
#include "graphs.hpp"

} } } // end namespace boost::graph::python


#ifndef WAVE_ODOMETRY_TYPES_HPP
#define WAVE_ODOMETRY_TYPES_HPP

#include <chrono>
#include <Eigen/Core>
#include "wave/geometry_og/transformation.hpp"
#include "wave/utils/math.hpp"

namespace wave {

template <int size>
using Earr = Eigen::array<long int, size>;
using ar1 = Earr<1>;
using ar2 = Earr<2>;
using ar3 = Earr<3>;

enum AssociationStatus { CORRESPONDED, UNCORRESPONDED };
enum ResidualType { PointToLine, PointToPlane };
enum SelectionPolicy { HIGH_POS, HIGH_NEG, NEAR_ZERO };
enum Kernel { LOAM, LOG, FOG, VAR };
enum Signal { RANGE, INTENSITY };

// true uses transform class with some approximations for exp map, etc
// false uses full analytical expressions wherever possible
using T_TYPE = Transformation<Eigen::Matrix<double, 3, 4>, true>;

using TimeType = std::chrono::steady_clock::time_point;

struct PoseVel {
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    PoseVel() {
        vel.setZero();
    }
    T_TYPE pose;
    Vec6 vel;
};

struct PoseVelStamped {
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    T_TYPE pose;
    Vec6 vel;
    Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> covar;
    TimeType stamp;
};

struct PoseStamped {
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    T_TYPE pose;
    TimeType stamp;
};

struct PoseVelDiff {
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    Vec12 hat_multiplier, candle_multiplier;
};

struct Criteria {
    Signal signal;
    Kernel kernel;
    SelectionPolicy sel_pol;
    float *threshold;
};

struct FeatureDefinition {
    // First item defines sort, rest are logical.
    std::vector<Criteria> criteria;
    int *n_limit;
};

}

#endif  // WAVE_ODOMETRY_TYPES_HPP