/*!
@file
Defines `boost::hana::Functor::laws`.

@copyright Louis Dionne 2014
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_HANA_FUNCTOR_LAWS_HPP
#define BOOST_HANA_FUNCTOR_LAWS_HPP

#include <boost/hana/functor/functor.hpp>

#include <boost/hana/comparable/comparable.hpp>
#include <boost/hana/functional.hpp>
#include <boost/hana/logical/logical.hpp>
#include <boost/hana/searchable/searchable.hpp>

namespace boost { namespace hana {
    struct Functor::laws {
        template <typename FunctorsOnX, typename FunctionsFromXtoY, typename FunctionsFromYtoZ>
        static constexpr auto check(FunctorsOnX xs, FunctionsFromXtoY fs, FunctionsFromYtoZ gs) {
            return and_(
                all(xs, [=](auto x) {
                    return equal(fmap(id, x), x);
                }),
                all(xs, [=](auto x) {
                    return all(fs, [=](auto f) {
                        return all(gs, [=](auto g) {
                            return equal(
                                fmap(compose(f, g), x),
                                fmap(f, fmap(g, x))
                            );
                        });
                    });
                })
            );
        }
    };
}} // end namespace boost::hana

#endif // !BOOST_HANA_FUNCTOR_LAWS_HPP


#include "EdgeOperations2d.h"

#include <wmtk/TriMesh.h>
#include <wmtk/utils/VectorUtils.h>
#include <wmtk/ExecutionScheduler.hpp>
#include <wmtk/utils/TupleUtils.hpp>

#include <Eigen/Core>
#include <Eigen/Geometry>

using namespace wmtk;
using namespace Edge2d;

bool Edge2d::EdgeOperations2d::swap_edge_after(const TriMesh::Tuple& t)
{
    std::vector<TriMesh::Tuple> tris;
    tris.push_back(t);
    tris.push_back(t.switch_edge(*this));
    return true;
}

bool Edge2d::EdgeOperations2d::collapse_edge_after(const TriMesh::Tuple& t)
{
    const Eigen::Vector3d p = (position_cache.local().v1p + position_cache.local().v2p) / 2.0;
    auto vid = t.vid(*this);
    vertex_attrs[vid].pos = p;

    return true;
}

bool Edge2d::EdgeOperations2d::split_edge_after(const TriMesh::Tuple& t)
{
    const Eigen::Vector3d p = (position_cache.local().v1p + position_cache.local().v2p) / 2.0;
    auto vid = t.vid(*this);
    vertex_attrs[vid].pos = p;

    return true;
}

std::vector<TriMesh::Tuple> Edge2d::EdgeOperations2d::new_edges_after(
    const std::vector<TriMesh::Tuple>& tris) const
{
    std::vector<TriMesh::Tuple> new_edges;
    std::vector<size_t> one_ring_fid;

    for (auto t : tris) {
        for (auto j = 0; j < 3; j++) {
            new_edges.push_back(tuple_from_edge(t.fid(*this), j));
        }
    }
    wmtk::unique_edge_tuples(*this, new_edges);
    return new_edges;
}

bool Edge2d::EdgeOperations2d::collapse_shortest(int target_operation_count)
{
    size_t initial_size = get_vertices().size();
    auto collect_all_ops = std::vector<std::pair<std::string, Tuple>>();
    for (auto& loc : get_edges()) collect_all_ops.emplace_back("edge_collapse", loc);

    auto renew = [](auto& m, auto op, auto& tris) {
        auto edges = m.new_edges_after(tris);
        auto optup = std::vector<std::pair<std::string, Tuple>>();
        for (auto& e : edges) optup.emplace_back("edge_collapse", e);
        return optup;
    };
    auto measure_len2 = [](auto& m, auto op, const Tuple& new_e) {
        auto len2 =
            (m.vertex_attrs[new_e.vid(m)].pos - m.vertex_attrs[new_e.switch_vertex(m).vid(m)].pos)
                .squaredNorm();
        return -len2;
    };
    auto setup_and_execute = [&](auto executor) {
        executor.num_threads = NUM_THREADS;
        executor.renew_neighbor_tuples = renew;
        executor.priority = measure_len2;
        executor.lock_vertices = [](auto& m, const auto& e, int task_id) -> bool {
            return m.try_set_edge_mutex_two_ring(e, task_id);
        };
        executor.stopping_criterion_checking_frequency = target_operation_count > 0
                                                             ? target_operation_count + 1
                                                             : std::numeric_limits<int>::max();
        executor.stopping_criterion = [](auto& m) { return true; };
        executor(*this, collect_all_ops);
    };

    if (NUM_THREADS > 0) {
        auto executor = wmtk::ExecutePass<EdgeOperations2d, ExecutionPolicy::kSeq>();
        setup_and_execute(executor);
    } else {
        auto executor = wmtk::ExecutePass<EdgeOperations2d, ExecutionPolicy::kPartition>();
        setup_and_execute(executor);
    }
    return true;
}


/*
 Copyright (C) 2021 Quaternion Risk Management Ltd
 All rights reserved.

 This file is part of ORE, a free-software/open-source library
 for transparent pricing and risk analysis - http://opensourcerisk.org

 ORE is free software: you can redistribute it and/or modify it
 under the terms of the Modified BSD License.  You should have received a
 copy of the license along with this program.
 The license is also available online at <http://opensourcerisk.org>

 This program is distributed on the basis that it will form a useful
 contribution to risk analytics and model standardisation, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
 FITNESS FOR A PARTICULAR PURPOSE. See the license for more details.
*/

/*! \file portfolio/builders/quantovanillaoption.hpp
    \brief Abstract engine builder for Quanto European Options
    \ingroup builders
*/

#pragma once

#include <boost/make_shared.hpp>
#include <ored/portfolio/builders/vanillaoption.hpp>
#include <qle/termstructures/flatcorrelation.hpp>
#include <ql/pricingengines/quanto/quantoengine.hpp>

namespace ore {
namespace data{

//! Abstract Engine Builder for Quanto Vanilla Options
/*! Pricing engines are cached by asset/currency

    \ingroup builders
*/
class QuantoVanillaOptionEngineBuilder
    : public CachingOptionEngineBuilder<string, const string&, const Currency&, const Currency&, const AssetClass&,
                                        const Date&> {
public:
    QuantoVanillaOptionEngineBuilder(const string& model, const string& engine, const set<string>& tradeTypes,
                                     const AssetClass& assetClass, const Date& expiryDate)
        : CachingOptionEngineBuilder(model, engine, tradeTypes, assetClass), expiryDate_(expiryDate) {}

    boost::shared_ptr<PricingEngine> engine(const string& assetName, const Currency& underlyingCcy,
                                            const Currency& payCcy, const Date& expiryDate) {
        return CachingPricingEngineBuilder<string, const string&, const Currency&, const Currency&, const AssetClass&,
                                           const Date&>::engine(assetName, underlyingCcy, payCcy, assetClass_,
                                                                expiryDate);
    }

protected:
    virtual string keyImpl(const string& assetName, const Currency& underlyingCcy, const Currency& payCcy,
                           const AssetClass& assetClassUnderlying, const Date& expiryDate) override {
        return assetName + "/" + underlyingCcy.code() + "/" + payCcy.code() + "/" + to_string(expiryDate);
    }

    Date expiryDate_;
};

//! Abstract Engine Builder for Quanto European Vanilla Options
/*! Pricing engines are cached by asset/currency

    \ingroup builders
*/
class QuantoEuropeanOptionEngineBuilder : public QuantoVanillaOptionEngineBuilder {
public:
    QuantoEuropeanOptionEngineBuilder(const string& model, const set<string>& tradeTypes, const AssetClass& assetClass)
        : QuantoVanillaOptionEngineBuilder(model, "AnalyticEuropeanEngine", tradeTypes, assetClass, Date()) {}

protected:
    virtual boost::shared_ptr<PricingEngine> engineImpl(const string& assetName, const Currency& underlyingCcy,
                                                        const Currency& payCcy, const AssetClass& assetClassUnderlying,
                                                        const Date& expiryDate) override {
        boost::shared_ptr<QuantLib::GeneralizedBlackScholesProcess> gbsp =
            getBlackScholesProcess(assetName, underlyingCcy, assetClassUnderlying);

        Handle<YieldTermStructure> discountCurve =
            market_->discountCurve(underlyingCcy.code(), configuration(MarketContext::pricing));

        Handle<BlackVolTermStructure> fxVolatility =
            market_->fxVol(underlyingCcy.code() + payCcy.code(), configuration(MarketContext::pricing));

        std::string fxSource = modelParameters_.at("FXSource");
        std::string fxIndex = "FX-" + fxSource + "-" + underlyingCcy.code() + "-" + payCcy.code();
        std::string underlyingIndex;
        switch (assetClassUnderlying) {
            case AssetClass::EQ:
                underlyingIndex = "EQ-" + assetName;
                break;
            case AssetClass::COM:
                underlyingIndex = "COMM-" + assetName;
                break;
            default:
                QL_FAIL("Asset class " << assetClassUnderlying << " not supported for quanto vanilla option.");
        }

    QuantLib::Handle<QuantExt::CorrelationTermStructure> corrCurve(
        boost::make_shared<QuantExt::FlatCorrelation>(0, NullCalendar(), 0.0, Actual365Fixed()));
    try {
        corrCurve = market_->correlationCurve(fxIndex, underlyingIndex, configuration(MarketContext::pricing));
    } catch (...) {
        WLOG("no correlation curve for " << fxIndex << ", " << underlyingIndex
                                         << " found, fall back to zero correlation");
    }

    return boost::make_shared<QuantLib::QuantoEngine<VanillaOption, AnalyticEuropeanEngine>>(
        gbsp, discountCurve, fxVolatility,
        Handle<Quote>(
            boost::make_shared<QuantExt::CorrelationValue>(corrCurve, corrCurve->timeFromReference(expiryDate))));
    }
};

} // namespace data
} //namespace ore


#define BIORBD_API_EXPORTS
#include "Utils/Quaternion.h"

#include <rbdl/rbdl_math.h>
#include <Eigen/Dense>
#include "Utils/Vector3d.h"
#include "Utils/Vector.h"
#include "Utils/RotoTrans.h"
#include "Utils/Error.h"
#include "Utils/Rotation.h"

biorbd::utils::Quaternion::Quaternion (
        double kStabilizer) :
    Eigen::Vector4d (1, 0, 0, 0),
    m_Kstab(kStabilizer) {

}

biorbd::utils::Quaternion::Quaternion(const biorbd::utils::Quaternion &other) :
    Eigen::Vector4d (other),
    m_Kstab(other.m_Kstab) {

}

biorbd::utils::Quaternion::Quaternion (
        double w,
        double x,
        double y,
        double z,
        double kStabilizer) :
    Eigen::Vector4d(w, x, y, z),
    m_Kstab(kStabilizer) {

}

biorbd::utils::Quaternion::Quaternion (
        double w,
        const biorbd::utils::Vector3d &vec3,
        double kStabilizer) :
    Eigen::Vector4d(w, vec3[0], vec3[1], vec3[2]),
    m_Kstab(kStabilizer) {

}

biorbd::utils::Quaternion::Quaternion(
        const biorbd::utils::Vector &vec,
        double kStabilizer):
    Eigen::Vector4d (vec),
    m_Kstab(kStabilizer) {

}

double biorbd::utils::Quaternion::w() const
{
    return (*this)(0);
}
double biorbd::utils::Quaternion::x() const
{
    return (*this)(1);
}
double biorbd::utils::Quaternion::y() const
{
    return (*this)(2);
}
double biorbd::utils::Quaternion::z() const
{
    return (*this)(3);
}

void biorbd::utils::Quaternion::setKStab(double newKStab)
{
    m_Kstab = newKStab;
}

double biorbd::utils::Quaternion::kStab() const
{
    return m_Kstab;
}

biorbd::utils::Quaternion biorbd::utils::Quaternion::operator*(
        const biorbd::utils::Quaternion& q) const
{
    return biorbd::utils::Quaternion (
        (*this)[0] * q[0] - (*this)[1] * q[1] - (*this)[2] * q[2] - (*this)[3] * q[3],
        (*this)[0] * q[1] + (*this)[1] * q[0] + (*this)[2] * q[3] - (*this)[3] * q[2],
        (*this)[0] * q[2] + (*this)[2] * q[0] + (*this)[3] * q[1] - (*this)[1] * q[3],
        (*this)[0] * q[3] + (*this)[3] * q[0] + (*this)[1] * q[2] - (*this)[2] * q[1],
            (this->m_Kstab + q.m_Kstab) / 2);
}

biorbd::utils::Quaternion biorbd::utils::Quaternion::operator*(
        double scalar) const
{
    return biorbd::utils::Quaternion (
                this->Eigen::Vector4d::operator*(scalar), this->m_Kstab);
}

biorbd::utils::Quaternion biorbd::utils::Quaternion::operator*(
        float scalar) const
{
    return biorbd::utils::Quaternion (
                this->Eigen::Vector4d::operator*(scalar), this->m_Kstab);
}

biorbd::utils::Quaternion biorbd::utils::Quaternion::operator+(
        const biorbd::utils::Quaternion& other) const
{
    return biorbd::utils::Quaternion(this->Eigen::Vector4d::operator+(other),
                                     (this->m_Kstab + other.m_Kstab) / 2);
}

biorbd::utils::Quaternion biorbd::utils::Quaternion::operator-(
        const biorbd::utils::Quaternion& other) const
{
    return biorbd::utils::Quaternion(this->Eigen::Vector4d::operator-(other),
                                     (this->m_Kstab + other.m_Kstab) / 2);
}

biorbd::utils::Quaternion biorbd::utils::Quaternion::fromGLRotate(
        double angle,
        double x,
        double y,
        double z,
        double kStab) {
    double st = std::sin (angle * M_PI / 360.);
    return biorbd::utils::Quaternion (
                std::cos (angle * M_PI / 360.), st * x, st * y, st * z, kStab);
}

biorbd::utils::Quaternion biorbd::utils::Quaternion::fromAxisAngle(
        double angle,
        const biorbd::utils::Vector3d &axis,
        double kStab) {
    double d = axis.norm();
    double s2 = std::sin (angle * 0.5) / d;
    return biorbd::utils::Quaternion (
                std::cos(angle * 0.5),
                axis[0] * s2, axis[1] * s2, axis[2] * s2, kStab
            );
}

biorbd::utils::Quaternion biorbd::utils::Quaternion::fromMatrix(
        const biorbd::utils::RotoTrans &rt,
        double kStab) {
    return fromMatrix(rt.rot(), kStab);
}

biorbd::utils::Quaternion biorbd::utils::Quaternion::fromMatrix(
        const biorbd::utils::Rotation &mat,
        double kStab) {
    double w = std::sqrt (1. + mat(0,0) + mat(1,1) + mat(2,2)) * 0.5;
    return Quaternion (
                w,
                (mat(2,1) - mat(1,2)) / (w * 4.),
                (mat(0,2) - mat(2,0)) / (w * 4.),
                (mat(1,0) - mat(0,1)) / (w * 4.),
                kStab);
}

biorbd::utils::Quaternion biorbd::utils::Quaternion::fromZYXAngles(
        const biorbd::utils::Vector3d &zyx_angles,
        double kStab) {
    return fromAxisAngle (zyx_angles[2], biorbd::utils::Vector3d (0., 0., 1.), kStab)
            * fromAxisAngle (zyx_angles[1], biorbd::utils::Vector3d (0., 1., 0.), kStab)
            * fromAxisAngle (zyx_angles[0], biorbd::utils::Vector3d (1., 0., 0.), kStab);
}

biorbd::utils::Quaternion biorbd::utils::Quaternion::fromYXZAngles(
        const biorbd::utils::Vector3d &yxz_angles,
        double kStab) {
    return fromAxisAngle (yxz_angles[1], biorbd::utils::Vector3d (0., 1., 0.), kStab)
            * fromAxisAngle (yxz_angles[0], biorbd::utils::Vector3d (1., 0., 0.), kStab)
            * fromAxisAngle (yxz_angles[2], biorbd::utils::Vector3d (0., 0., 1.), kStab);
}

biorbd::utils::Quaternion biorbd::utils::Quaternion::fromXYZAngles(
        const biorbd::utils::Vector3d &xyz_angles, double kStab) {
    return fromAxisAngle (xyz_angles[0], biorbd::utils::Vector3d (1., 0., 0.), kStab)
            * fromAxisAngle (xyz_angles[1], biorbd::utils::Vector3d (0., 1., 0.), kStab)
            * fromAxisAngle (xyz_angles[2], biorbd::utils::Vector3d (0., 0., 1.), kStab);
}

biorbd::utils::Rotation biorbd::utils::Quaternion::toMatrix(
        bool skipAsserts) const {
    if (!skipAsserts) {
        biorbd::utils::Error::check(fabs(this->squaredNorm() - 1.) < 1e-10,
                                    "The Quaternion norm is not equal to one");
    }

    double w = (*this)[0];
    double x = (*this)[1];
    double y = (*this)[2];
    double z = (*this)[3];
    biorbd::utils::Rotation out;
    out <<
        1 - 2*y*y - 2*z*z,  2*x*y - 2*w*z,      2*x*z + 2*w*y,
        2*x*y + 2*w*z,      1 - 2*x*x - 2*z*z,  2*y*z - 2*w*x,
        2*x*z - 2*w*y,      2*y*z + 2*w*x,      1 - 2*x*x - 2*y*y;
    return out;
}

biorbd::utils::Quaternion biorbd::utils::Quaternion::slerp(
        double alpha,
        const biorbd::utils::Quaternion &quat) const {
    // check whether one of the two has 0 length
    double s = std::sqrt (squaredNorm() * quat.squaredNorm());

    // division by 0.f is unhealthy!
    assert (s != 0.);

    double angle = acos (dot(quat) / s);
    if (angle == 0. || std::isnan(angle)) {
        return *this;
    }
    assert(!std::isnan(angle));

    double d = 1. / std::sin (angle);
    double p0 = std::sin ((1. - alpha) * angle);
    double p1 = std::sin (alpha * angle);

    if (dot (quat) < 0.) {
        return Quaternion( ((*this) * p0 - quat * p1) * d, this->m_Kstab);
    }
    return Quaternion( ((*this) * p0 + quat * p1) * d,
                       (this->m_Kstab + quat.m_Kstab) / 2);
}

biorbd::utils::Quaternion biorbd::utils::Quaternion::conjugate() const {
    return biorbd::utils::Quaternion (
                (*this)[0],
                -(*this)[1],-(*this)[2],-(*this)[3],
                this->kStab()
            );
}

biorbd::utils::Quaternion biorbd::utils::Quaternion::timeStep(
        const biorbd::utils::Vector3d &omega,
        double dt) {
    double omega_norm = omega.norm();
    return fromAxisAngle (
                dt * omega_norm, omega / omega_norm, this->m_Kstab) * (*this);
}

biorbd::utils::Vector3d biorbd::utils::Quaternion::rotate(
        const biorbd::utils::Vector3d &vec) const {
    biorbd::utils::Quaternion vec_quat (0., vec);

    biorbd::utils::Quaternion res_quat(vec_quat * (*this));
    res_quat = conjugate() * res_quat;

    return biorbd::utils::Vector3d(res_quat[1], res_quat[2], res_quat[3]);
}

#include <iostream>
biorbd::utils::Quaternion biorbd::utils::Quaternion::omegaToQDot(
        const biorbd::utils::Vector3d &omega) const {
    Eigen::MatrixXd m(4, 3);
    m(0, 0) = -(*this)[1];   m(0, 1) = -(*this)[2];   m(0, 2) = -(*this)[3];
    m(1, 0) =  (*this)[0];   m(1, 1) = -(*this)[3];   m(1, 2) =  (*this)[2];
    m(2, 0) =  (*this)[3];   m(2, 1) =  (*this)[0];   m(2, 2) = -(*this)[1];
    m(3, 0) = -(*this)[2];   m(3, 1) =  (*this)[1];   m(3, 2) =  (*this)[0];
    return biorbd::utils::Quaternion(0.5 * m * omega, this->m_Kstab);
}

biorbd::utils::Vector3d  biorbd::utils::Quaternion::eulerDotToOmega(
            const biorbd::utils::Vector3d &eulerDot,
            const biorbd::utils::Vector3d &euler,
            const biorbd::utils::String& seq) {

    biorbd::utils::Vector3d w;
    double dph, dth, dps, ph, th, ps;
    dph = eulerDot[0]; dth = eulerDot[1]; dps = eulerDot[2];
    ph = euler[0]; th = euler[1]; ps = euler[2];
    if (!seq.compare("xyz")) {          // xyz
        w[0] = dph*std::cos(th)*std::cos(ps) + dth*std::sin(ps);
        w[1] = dth*std::cos(ps) - dph*std::cos(th)*std::sin(ps);
        w[2] = dph*std::sin(th) + dps;
    } else {
        biorbd::utils::Error::raise("Angle sequence is either nor implemented or not recognized");
    }
    return w;
}

void biorbd::utils::Quaternion::derivate(
        const biorbd::utils::Vector &w)
{
    // Création du quaternion de "préproduit vectoriel"
    double& qw = (*this)(0);
    double& qx = (*this)(1);
    double& qy = (*this)(2);
    double& qz = (*this)(3);
    Eigen::Matrix4d Q;
    Q <<    qw, -qx, -qy, -qz,
            qx,  qw, -qz,  qy,
            qy,  qz,  qw, -qx,
            qz, -qy,  qx,  qw;

    // Ajout du paramètre de stabilisation
    Eigen::Vector4d w_tp (m_Kstab*w.norm()*(1-this->norm()), w(0), w(1), w(2));
    Eigen::Vector4d newQuat(0.5 * Q * w_tp);

    // Assigning is slightly faster than create a new Quaternion
    qw = newQuat[0];
    qx = newQuat[1];
    qy = newQuat[2];
    qz = newQuat[3];

}

void biorbd::utils::Quaternion::normalize()
{
    *this = *this / this->norm();
}


// includes
#include "predikct/motion_candidate_node.h"
#include "predikct/motion_state.h"
#include "predikct/user_prediction_node.h"
#include "predikct/reward_calculator.h"
#include <random>
#include <kdl/frames.hpp>
#include <Eigen/Eigenvalues>

namespace predikct
{

struct normal_random_variable
{
    normal_random_variable(Eigen::MatrixXd const& covar)
        : normal_random_variable(Eigen::VectorXd::Zero(covar.rows()), covar)
    {}

    normal_random_variable(Eigen::VectorXd const& mean, Eigen::MatrixXd const& covar)
        : mean(mean)
    {
        Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> eigenSolver(covar);
        transform = eigenSolver.eigenvectors() * eigenSolver.eigenvalues().cwiseSqrt().asDiagonal();
    }

    Eigen::VectorXd mean;
    Eigen::MatrixXd transform;

    Eigen::VectorXd operator()() const
    {
        static std::mt19937 gen{ std::random_device{}() };
        static std::normal_distribution<> dist;

        return mean + transform * Eigen::VectorXd{ mean.size() }.unaryExpr([&](double x) { return dist(gen); });
    }
};

MotionCandidateNode::MotionCandidateNode(TreeNode* parent, RobotModel* robot_model, boost::shared_ptr<MotionState> state, int tree_depth,
    TreeSpec* tree_spec, RewardCalculator* reward_calculator, UserModel* user_model, std::vector<double>* desired_velocity)
    : TreeNode(parent, robot_model, state, tree_depth, tree_spec, reward_calculator, user_model)
{
    desired_velocity_ = Eigen::Map<Eigen::VectorXd>(&((*desired_velocity)[0]), desired_velocity->size());
    state->CalculateJacobian(robot_model);
    candidates_generated_ = false;
    desired_velocity_raw_ = std::vector<double>(desired_velocity->size());
    desired_movement_ = std::vector<double>(desired_velocity->size());
    for(int i = 0; i < desired_velocity_raw_.size(); i++)
    {
        desired_velocity_raw_[i] = (*desired_velocity)[i];
        desired_movement_[i] = ((*desired_velocity)[i])*tree_spec_->time_window;
    }
    is_leaf_ = tree_depth >= tree_spec->tree_depth;
}

// Generate a set of set of joint motion candidates,
//  each of which leads to a User Prediction node
std::vector<boost::shared_ptr<TreeNode>> MotionCandidateNode::GenerateChildren()
{
    children_.clear();
    motion_candidates_.clear();

    //Calculate movement that satisfies desired velocity using pseudo-inverse
    Eigen::VectorXd primary_movement = state_->pseudo_inverse * desired_velocity_;

    //Generate possible movement commands for using redundant DOF within null space of pseudo-inverse
    GetNullSpace();
    //Generate null space movement around following the same null movement goals from previous timesteps with added noise sampled from a multivariate distribution with mean of previous null space motion and covariance = identity matrix
    Eigen::VectorXd base_null_eigen(state_->joint_positions.size());
    Eigen::VectorXd target_null_eigen(state_->joint_positions.size());
    Eigen::VectorXd mean_null_eigen(state_->joint_positions.size());
    bool all_zeros = true;
    for(int j = 0; j < state_->joint_positions.size(); j++)
    {
        base_null_eigen(j) = state_->null_goals[j];
        if(base_null_eigen(j) != 0.0)
        {
            all_zeros = false;
        }
    }
    // Find the mean of the last null vector and the current gradient of the manipulability index
    Eigen::Matrix<double,Eigen::Dynamic, Eigen::Dynamic> covariance = Eigen::Matrix<double,Eigen::Dynamic, Eigen::Dynamic>::Identity(state_->joint_positions.size(),state_->joint_positions.size());

    // CALCULATION OF GRADIENT OF MANIPULABILITY INDEX
    // Calculation of derivative source: https://arxiv.org/pdf/1908.02963.pdf (page 3, left column)
    // Step 1: Calculate the manipulability index
    state_->CalculateManipulability(robot_model_);
    Eigen::Matrix<double,6,Eigen::Dynamic> jac_dot;
    double k_gradient_weight = 25;
    for(int j = 0; j < state_->joint_positions.size(); j++)
    {
        // Step 2: Find the partial derivative of the Jacobian with respect to the current joint
        robot_model_->GetJacobianDot(state_->joint_positions, j, &jac_dot);
        // Steps 3 & 4: Find the trace of partial derivative of Jacobian times pseudoinverse, then multiply answer from Step 3 by Step 1 to get gradient
        target_null_eigen(j) = k_gradient_weight * state_->manipulability * ((jac_dot * state_->pseudo_inverse).trace());

        if(all_zeros)
        {
            mean_null_eigen(j) = target_null_eigen(j);
            covariance(j,j) = 0.1;
        }
        else
        {
            mean_null_eigen(j) = (base_null_eigen(j) + target_null_eigen(j)) / 2.0;
            covariance(j,j) = pow(std::abs(base_null_eigen(j) - target_null_eigen(j)), 2) / 2.0;
        }
    }

    normal_random_variable noise_sample { mean_null_eigen, covariance };

    //Generate number of children according to branching factor
    Eigen::VectorXd total_movement;
    for(int i = 0; i < tree_spec_->motion_candidate_branching_factor; i++)
    {
        Eigen::VectorXd null_movement_eigen(state_->joint_positions.size());
        if(i == 0){
            null_movement_eigen = base_null_eigen;
        } else {
            null_movement_eigen = noise_sample();
        }

        total_movement = primary_movement + null_space_*null_movement_eigen;

        //Create new motion candidate
        std::vector<double> motion_candidate;
        motion_candidate.resize(total_movement.size());
        Eigen::VectorXd::Map(&motion_candidate[0], total_movement.size()) = total_movement;
        std::vector<double> null_goals;
        null_goals.resize(null_movement_eigen.size());
        Eigen::VectorXd::Map(&null_goals[0], null_movement_eigen.size()) = null_movement_eigen;
        boost::shared_ptr<MotionState> new_motion_candidate(new MotionState(&(state_->joint_positions), &(state_->joint_velocities), &motion_candidate, &null_goals, tree_spec_->time_window, robot_model_, node_depth_ * tree_spec_->time_window));
        motion_candidates_.push_back(new_motion_candidate);
        if(!is_leaf_)
        {
            //Create new child node
            children_.push_back(boost::shared_ptr<TreeNode> (new UserPredictionNode(this, robot_model_, new_motion_candidate,
                node_depth_, tree_spec_, reward_calc_, user_model_, &desired_velocity_raw_)));
        }
    }

    candidates_generated_ = true;
    return children_;
}

double MotionCandidateNode::CalculateReward()
{
    if(!candidates_generated_)
    {
        GenerateChildren();
    }

    //Calculate ideal position after applying desired velocity
    state_->CalculatePosition(robot_model_);
    KDL::Frame ideal_position( KDL::Rotation::RPY(desired_movement_[3], desired_movement_[4], desired_movement_[5]) * state_->position.M,
        state_->position.p + KDL::Vector(desired_movement_[0], desired_movement_[1], desired_movement_[2]));

    double x, y, z, w;
    state_->position.M.GetQuaternion(x, y, z, w);
    ideal_position.M.GetQuaternion(x, y, z, w);
    double max_reward = reward_calc_->EvaluateMotionCandidate(robot_model_, state_, motion_candidates_[0], &ideal_position);
    if(!is_leaf_){
        max_reward += children_[0]->GetReward();
    }
    best_candidate_index_ = 0;
    for(int i = 1; i < motion_candidates_.size(); i++)
    {
        double new_reward = reward_calc_->EvaluateMotionCandidate(robot_model_, state_, motion_candidates_[i], &ideal_position);
        if(!is_leaf_)
        {
            new_reward += children_[i]->GetReward();
        }
        if(new_reward > max_reward)
        {
            max_reward = new_reward;
            best_candidate_index_ = i;
        }
    }
    reward_ = max_reward;

    reward_calculated_ = true;
    return reward_;
}

void MotionCandidateNode::ChooseMotionCandidate(std::vector<double>* joint_velocities, std::vector<double>* null_vector)
{
    joint_velocities->clear();
    null_vector->clear();
    if(!reward_calculated_)
    {
        CalculateReward();
    }

    for(int i = 0; i < (*motion_candidates_[best_candidate_index_]).commanded_velocities.size(); ++i)
    {
        joint_velocities->push_back((*motion_candidates_[best_candidate_index_]).commanded_velocities[i]);
        null_vector->push_back((*motion_candidates_[best_candidate_index_]).null_goals[i]);
    }
}

void MotionCandidateNode::GetBaseline(std::vector<double>* joint_velocities)
{
    joint_velocities->clear();
    state_->CalculateJacobian(robot_model_);
    Eigen::VectorXd primary_movement = state_->pseudo_inverse * desired_velocity_;

    for(int i = 0; i < primary_movement.size(); ++i)
    {
        joint_velocities->push_back(primary_movement(i));
    }
}

//--------------------------------------------------------------------------------------------------
// Linear algebra functions
void MotionCandidateNode::GetNullSpace()
{
    Eigen::Matrix<double,Eigen::Dynamic,Eigen::Dynamic> ident = Eigen::Matrix<double,Eigen::Dynamic, Eigen::Dynamic>::Identity(state_->pseudo_inverse.rows(),state_->jacobian.cols());
    null_space_ = ident - state_->pseudo_inverse * state_->jacobian;
}

}


#include "gtest/gtest.h"
#include <Eigen/Core>
#include <vector>
#include "MeshIntersection.h"
#include "IntersectionFace.h"
#include "MeshFactory.h"
#include "Face.h"

namespace {
    using namespace Geotree;

    class IntersectionFaceTest : public ::testing::Test {
    };

    TEST_F(IntersectionFaceTest, basic)
    {
        //Mesh mesh;

        Verticies V(3,3);
        Faces F(1,3);

        V << 0,0,0, 1,0,0, 0,1,0;
        F << 0,1,2;

        //mesh.V = V;
        //mesh.F = F;

        Mesh mesh(V,F);

        IntersectionFace face = mesh.getFaceT(0);

        PathX<FacePoint> path(mesh);
        path.add(FacePoint(mesh, Vector(0.5, 0, 0), SEGMENT, 0));
        path.add(FacePoint(mesh, Vector(0, 0.5, 0), SEGMENT, 2));
        face.cut(path);

        EXPECT_EQ(face.numberOfPaths(), 0);
        EXPECT_EQ(face.numberOfEdgePoints(), 5);

        face.completePaths();

        EXPECT_EQ(face.numberOfPaths(), 2);

        EXPECT_TRUE(face.pathSize(0) == 4 || face.pathSize(0) == 3);
        EXPECT_TRUE(face.pathSize(1) == 4 || face.pathSize(1) == 3);

        PathT p1(mesh);
        p1.addExisting(3);
        p1.addExisting(4);
        p1.addExisting(0);

        PathT p2(mesh);
        p2.addExisting(3);
        p2.addExisting(4);
        p2.addExisting(2);
        p2.addExisting(1);

        std::cout << face.getPath(0) << std::endl;
        std::cout << face.getPath(1) << std::endl;

        EXPECT_TRUE(face.getPath(0) == p1 || face.getPath(0) == p2);
        EXPECT_TRUE(face.getPath(1) == p1 || face.getPath(1) == p2);

        face.triangulate();

        for (int j=0; j<2; j++) {
            std::set<int> area = face.getArea(j);

            std::cout << "area: ";
            for (int i : area) {
                std::cout << i << ", ";
            }
            std::cout << std::endl;
        }

    }

    TEST_F(IntersectionFaceTest, twoCuts)
    {
        //Mesh mesh;

        Verticies V(3,3);
        Faces F(1,3);

        V << 0,0,0, 1,0,0, 0,1,0;
        F << 0,1,2;

        Mesh mesh(V,F);
        //mesh.V = V;
        //mesh.F = F;

        IntersectionFace face = mesh.getFaceT(0);

        PathX<FacePoint> path(mesh);
        PathX<FacePoint> path2(mesh);

        path.add(FacePoint(mesh, Vector(0.5, 0, 0), SEGMENT, 0));
        path2.add(FacePoint(mesh, Vector(0.7, 0, 0), SEGMENT, 0));

        path.add(FacePoint(mesh, Vector(0, 0.5, 0), SEGMENT, 2));
        path2.add(FacePoint(mesh, Vector(0, 0.7, 0), SEGMENT, 2));

        face.cut(path);
        face.cut(path2);

        EXPECT_EQ(face.numberOfPaths(), 0);
        EXPECT_EQ(face.numberOfEdgePoints(), 7);

        face.completePaths();

        EXPECT_EQ(face.numberOfPaths(), 3);

        std::cout << face.getPath(0) << std::endl;
        std::cout << face.getPath(1) << std::endl;

        EXPECT_TRUE(face.pathSize(0) == 4 || face.pathSize(0) == 3);
        EXPECT_TRUE(face.pathSize(1) == 4 || face.pathSize(1) == 3);
        EXPECT_TRUE(face.pathSize(2) == 4 || face.pathSize(2) == 3);

        PathT p1(mesh);
        p1.addExisting(3);
        p1.addExisting(5);
        p1.addExisting(0);

        PathT p2(mesh);
        p2.addExisting(3);
        p2.addExisting(5);
        p2.addExisting(6);
        p2.addExisting(4);

        EXPECT_TRUE(face.getPath(0) == p1 || face.getPath(0) == p2);
        EXPECT_TRUE(face.getPath(1) == p1 || face.getPath(1) == p2);

        face.triangulate();

        for (int j=0; j<3; j++) {
            std::set<int> area = face.getArea(j);

            std::cout << "area: ";
            for (int i : area) {
                std::cout << i << ", ";
            }
            std::cout << std::endl;
        }
    }

    TEST_F(IntersectionFaceTest, longer)
    {
        Verticies V(3,3);
        Faces F(1,3);

        V << 0,0,0, 1,0,0, 0,1,0;
        F << 0,1,2;

        Mesh mesh(V,F);
        IntersectionFace face = mesh.getFaceT(0);

        PathX<FacePoint> path(mesh);

        path.add(FacePoint(mesh, Vector(0.5, 0, 0), SEGMENT, 0));
        path.add(FacePoint(mesh, Vector(0.5, 0.1, 0), FACE, 0));
        path.add(FacePoint(mesh, Vector(0.5, 0.2, 0), FACE, 0));
        path.add(FacePoint(mesh, Vector(0.5, 0.3, 0), FACE, 0));
        path.add(FacePoint(mesh, Vector(0.0, 0.5, 0), SEGMENT, 2));

        face.cut(path);

        EXPECT_EQ(face.numberOfPaths(), 0);
        EXPECT_EQ(face.numberOfEdgePoints(), 5);

        face.completePaths();

        EXPECT_EQ(face.numberOfPaths(), 2);

        std::cout << face.getPath(0) << std::endl;
        std::cout << face.getPath(1) << std::endl;

        EXPECT_TRUE(face.pathSize(0) == 6 || face.pathSize(0) == 7);
        EXPECT_TRUE(face.pathSize(1) == 6 || face.pathSize(1) == 7);

        face.triangulate();

        for (int j=0; j<2; j++) {
            std::set<int> area = face.getArea(j);

            std::cout << "area: ";
            for (int i : area) {
                std::cout << i << ", ";
            }
            std::cout << std::endl;
        }
    }

    TEST_F(IntersectionFaceTest, noEdge) {

        Verticies V(3, 3);
        Faces F(1, 3);

        V << 0, 0, 0, 1, 0, 0, 0, 1, 0;
        F << 0, 1, 2;

        Mesh mesh(V,F);

        IntersectionFace face = mesh.getFaceT(0);

        PathX<FacePoint> path(mesh);

        path.add(FacePoint(mesh, Vector(0.1, 0.1, 0), FACE, 0));
        path.add(FacePoint(mesh, Vector(0.5, 0.1, 0), FACE, 0));
        path.add(FacePoint(mesh, Vector(0.1, 0.5, 0), FACE, 0));

        face.cut(path);

        EXPECT_EQ(face.numberOfPaths(), 0);
        EXPECT_EQ(face.numberOfEdgePoints(), 3);

        face.completePaths();

        EXPECT_EQ(face.numberOfPaths(), 2);

        face.triangulate();

        for (int j=0; j<2; j++) {
            std::set<int> area = face.getArea(j);

            std::cout << "area: ";
            for (int i : area) {
                std::cout << i << ", ";
            }
            std::cout << std::endl;
        }
    }

    }


/*
 * Copyright (C) 2020 Newlogic Pte. Ltd.
 *
 * 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.
 */

#ifdef __cplusplus

#include <dlib/clustering.h>
#include <dlib/dnn.h>
#include <dlib/image_io.h>
#include <dlib/image_processing/frontal_face_detector.h>
#include <dlib/string.h>
#include <sodium.h>
#include <sstream>

#ifdef ANDROID
#include <android/log.h>

#define LOGI(...)               \
    ((void)__android_log_print( \
        ANDROID_LOG_INFO, "idpassapi::dlib", __VA_ARGS__))
#else
#define LOGI(...)
#endif

namespace dlib_api
{
class InputStream : public std::istream
{
public:
    InputStream(const unsigned char* p, size_t l)
        : std::istream(&_buffer), _buffer((char*)p, l)
    {
        rdbuf(&_buffer);
    }

private:
    class membuf : public std::basic_streambuf<char>
    {
    public:
        membuf(char* p, size_t l)
        {
            // setg((char*)p, (char*)p, (char*)p + l);
            setg(p, p, p + l);
        }
    };

    membuf _buffer;
};

template<template<int, template<typename> class, int, typename> class block,
         int N,
         template<typename>
         class BN,
         typename SUBNET>
using residual = dlib::add_prev1<block<N, BN, 1, dlib::tag1<SUBNET>>>;

template<template<int, template<typename> class, int, typename> class block,
         int N,
         template<typename>
         class BN,
         typename SUBNET>
using residual_down = dlib::add_prev2<dlib::avg_pool<
    2,
    2,
    2,
    2,
    dlib::skip1<dlib::tag2<block<N, BN, 2, dlib::tag1<SUBNET>>>>>>;

template<int N, template<typename> class BN, int stride, typename SUBNET>
using block
    = BN<dlib::con<N,
                   3,
                   3,
                   1,
                   1,
                   dlib::relu<BN<dlib::con<N, 3, 3, stride, stride, SUBNET>>>>>;

template<int N, typename SUBNET>
using ares = dlib::relu<residual<block, N, dlib::affine, SUBNET>>;
template<int N, typename SUBNET>
using ares_down = dlib::relu<residual_down<block, N, dlib::affine, SUBNET>>;

template<typename SUBNET>
using alevel0 = ares_down<256, SUBNET>;
template<typename SUBNET>
using alevel1 = ares<256, ares<256, ares_down<256, SUBNET>>>;
template<typename SUBNET>
using alevel2 = ares<128, ares<128, ares_down<128, SUBNET>>>;
template<typename SUBNET>
using alevel3 = ares<64, ares<64, ares<64, ares_down<64, SUBNET>>>>;
template<typename SUBNET>
using alevel4 = ares<32, ares<32, ares<32, SUBNET>>>;

using anet_type = dlib::loss_metric<dlib::fc_no_bias<
    128,
    dlib::avg_pool_everything<alevel0<alevel1<alevel2<alevel3<
        alevel4<dlib::max_pool<3,
                               3,
                               2,
                               2,
                               dlib::relu<dlib::affine<dlib::con<
                                   32,
                                   7,
                                   7,
                                   2,
                                   2,
                                   dlib::input_rgb_image_sized<150>>>>>>>>>>>>>;

#ifdef EMBED_MODELS
// These externs are coming from the Dlib dat files inside  models/ folder.
// The dat binary files are converted into C array using `xxd -i`.
// Github does not allow any file greater than 50MB size.
extern "C" unsigned char shape_predictor_5_face_landmarks_dat[];
extern "C" unsigned int shape_predictor_5_face_landmarks_dat_len;

extern "C" unsigned char dlib_face_recognition_resnet_model_v1_dat[];
extern "C" unsigned int dlib_face_recognition_resnet_model_v1_dat_len;
#endif

// Supported: BMP, JPEG, DNG
int load2matrix(const char* img,
                int img_len,
                dlib::matrix<dlib::rgb_pixel>& image)
{
    char buffer[9];
    std::memcpy(buffer, img, 8);
    buffer[8] = 0;

    InputStream input((unsigned char*)img, img_len);

    try {
        if (buffer[0] == '\xff' && buffer[1] == '\xd8' && buffer[2] == '\xff') {
            dlib::load_jpeg(image, img, img_len);
        } else if (buffer[0] == 'B' && buffer[1] == 'M') {
            dlib::load_bmp(image, input);
        } else if (buffer[0] == 'D' && buffer[1] == 'N' && buffer[2] == 'G') {
            dlib::load_dng(image, input);
        } else {
            LOGI("load2matrix: fail");
            return 1;
        }
    } catch (...) {
        return 2;
    }

    return 0;
}

int computeface128d(const char* photo, int photo_len, float* f128d)
{
    if (photo_len <= 0 || photo == nullptr || f128d == nullptr) {
        return 0;
    }

    // feed photo to Dlib (no file system)
    // pass f128d to Dlib API to fill-in
    // returns the count of faces detected

    dlib::frontal_face_detector detector = dlib::get_frontal_face_detector();

    dlib::shape_predictor sp;
    anet_type net;

#ifdef EMBED_MODELS
    InputStream landmark_dat(shape_predictor_5_face_landmarks_dat,
                             shape_predictor_5_face_landmarks_dat_len);

    InputStream resnet_dat(dlib_face_recognition_resnet_model_v1_dat,
                           dlib_face_recognition_resnet_model_v1_dat_len);

    dlib::deserialize(sp, landmark_dat);
    dlib::deserialize(net, resnet_dat);
#else
    dlib::deserialize(getenv("SHAPEPREDICTIONDATA")) >> sp;
    dlib::deserialize(getenv("FACERECOGNITIONDATA")) >> net;
#endif

    dlib::matrix<dlib::rgb_pixel> img;
    int status = load2matrix(photo, photo_len, img);

    if (status != 0) {
        // cannot load to matrix as image format
        // is not supported
        LOGI("\ncomputeface128: cannot load to matrix as image format ***\n");
        return -1;
    }

    std::vector<dlib::matrix<dlib::rgb_pixel>> faces;
    for (auto face : detector(img)) {
        auto shape = sp(img, face);
        dlib::matrix<dlib::rgb_pixel> face_chip;
        extract_image_chip(
            img, get_face_chip_details(shape, 150, 0.25), face_chip);
        faces.push_back(std::move(face_chip));
    }

    if (faces.size() == 1) {
        std::vector<dlib::matrix<float, 0, 1>> face_descriptors = net(faces);

        int i = 0;
        for (float fval : face_descriptors[0]) {
            if (i < 128) {
                f128d[i] = fval;
            }
            i++;
        }
        if (i != 128) {
            std::string logmsg
                = "computeface128d anomaly = " + std::to_string(i);
            LOGI("computeface128d anomaly");
            return -2;
        }
    } else if (faces.size() == 0) {
        LOGI("computeface128: No faces found in image!");
    } else if (faces.size() != 1) {
        LOGI("computeface128: many faces found");
    }

    return faces.size();
}

} // nampespace dlib_api
#endif // __cplusplus


#ifndef BOOST_THREAD_PTHREAD_TIMESPEC_HPP
#define BOOST_THREAD_PTHREAD_TIMESPEC_HPP
//  (C) Copyright 2007 Anthony Williams
//
//  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)

#include <boost/thread/thread_time.hpp>
#include <boost/date_time/posix_time/conversion.hpp>

namespace boost
{
    namespace detail
    {
        inline struct timespec get_timespec(boost::system_time const& abs_time)
        {
            struct timespec timeout={0};
            boost::posix_time::time_duration const time_since_epoch=abs_time-boost::posix_time::from_time_t(0);

            timeout.tv_sec=time_since_epoch.total_seconds();
            timeout.tv_nsec=time_since_epoch.fractional_seconds()*(1000000000/time_since_epoch.ticks_per_second());
            return timeout;
        }
    }
}

#endif


/*===================================================================

The Medical Imaging Interaction Toolkit (MITK)

Copyright (c) German Cancer Research Center,
Division of Medical and Biological Informatics.
All rights reserved.

This software is distributed WITHOUT ANY WARRANTY; without
even the implied warranty of MERCHANTABILITY or FITNESS FOR
A PARTICULAR PURPOSE.

See LICENSE.txt or http://www.mitk.org for details.

===================================================================*/

#include <mitkBaseData.h>
#include <mitkImageCast.h>
#include <mitkImageToItk.h>
#include <metaCommand.h>
#include <mitkCommandLineParser.h>
#include <usAny.h>
#include <mitkIOUtil.h>
#include <itksys/SystemTools.hxx>
#include <itkDirectory.h>
#include <mitkFiberBundle.h>
#include <mitkPreferenceListReaderOptionsFunctor.h>
#include <itkImageFileWriter.h>
#include <mitkPeakImage.h>
#include <itkFitFibersToImageFilter.h>
#include <boost/lexical_cast.hpp>
#include <itkTractDensityImageFilter.h>

typedef itksys::SystemTools ist;
typedef itk::Point<float, 4> PointType4;
typedef itk::Image< float, 4 >  PeakImgType;
typedef itk::Image< unsigned char, 3 >  ItkUcharImageType;

std::vector< std::string > get_file_list(const std::string& path, std::vector< std::string > extensions={".fib", ".trk"})
{
  std::vector< std::string > file_list;
  itk::Directory::Pointer dir = itk::Directory::New();

  if (dir->Load(path.c_str()))
  {
    int n = dir->GetNumberOfFiles();
    for (int r = 0; r < n; r++)
    {
      const char *filename = dir->GetFile(r);
      std::string ext = ist::GetFilenameExtension(filename);
      for (auto e : extensions)
      {
        if (ext==e)
        {
          file_list.push_back(path + '/' + filename);
          break;
        }
      }
    }
  }
  return file_list;
}

std::vector< mitk::FiberBundle::Pointer > CombineTractograms(std::vector< mitk::FiberBundle::Pointer > reference, std::vector< mitk::FiberBundle::Pointer > candidates, int skip=-1)
{
  std::vector< mitk::FiberBundle::Pointer > fib;
  for (auto f : reference)
    fib.push_back(f);

  int c = 0;
  for (auto f : candidates)
  {
    if (c!=skip)
      fib.push_back(f);
    ++c;
  }

  return fib;
}

/*!
\brief Fits the tractogram to the input peak image by assigning a weight to each fiber (similar to https://doi.org/10.1016/j.neuroimage.2015.06.092).
*/
int main(int argc, char* argv[])
{
  mitkCommandLineParser parser;

  parser.setTitle("Anchor Based Scoring");
  parser.setCategory("Fiber Tracking Evaluation");
  parser.setDescription("");
  parser.setContributor("MIC");

  parser.setArgumentPrefix("--", "-");
  parser.addArgument("", "i1", mitkCommandLineParser::InputFile, "Anchor tractogram:", "anchor tracts in one tractogram file", us::Any(), false);
  parser.addArgument("", "i2", mitkCommandLineParser::InputFile, "Input peaks:", "input peak image", us::Any(), false);
  parser.addArgument("", "i3", mitkCommandLineParser::InputFile, "Candidate folder:", "folder containing all candidate tracts (separate files)", us::Any(), false);
  parser.addArgument("", "o", mitkCommandLineParser::OutputDirectory, "Output folder:", "output folder", us::Any(), false);

  parser.addArgument("reference_mask_folder", "", mitkCommandLineParser::String, "Reference Mask Folder:", "reference tract masks for accuracy evaluation");
  parser.addArgument("mask", "", mitkCommandLineParser::InputFile, "Mask image:", "scoring is only performed inside the mask image");
  parser.addArgument("greedy_add", "", mitkCommandLineParser::Bool, "Greedy:", "if enabled, the candidate tracts are not jointly fitted to the residual image but one after the other employing a greedy scheme", false);
  parser.addArgument("lambda", "", mitkCommandLineParser::Float, "Lambda:", "modifier for regularization", 0.1);
  parser.addArgument("dont_filter_outliers", "", mitkCommandLineParser::Bool, "Don't filter outliers:", "don't perform second optimization run with an upper weight bound based on the first weight estimation (95% quantile)", false);

  std::map<std::string, us::Any> parsedArgs = parser.parseArguments(argc, argv);
  if (parsedArgs.size()==0)
    return EXIT_FAILURE;

  std::string anchors_file = us::any_cast<std::string>(parsedArgs["i1"]);
  std::string peak_file_name = us::any_cast<std::string>(parsedArgs["i2"]);
  std::string candidate_folder = us::any_cast<std::string>(parsedArgs["i3"]);
  std::string out_folder = us::any_cast<std::string>(parsedArgs["o"]);

  bool greedy_add = false;
  if (parsedArgs.count("greedy_add"))
    greedy_add = us::any_cast<bool>(parsedArgs["greedy_add"]);

  float lambda = 0.1;
  if (parsedArgs.count("lambda"))
    lambda = us::any_cast<float>(parsedArgs["lambda"]);

  bool filter_outliers = true;
  if (parsedArgs.count("dont_filter_outliers"))
    filter_outliers = !us::any_cast<bool>(parsedArgs["dont_filter_outliers"]);

  std::string mask_file = "";
  if (parsedArgs.count("mask"))
    mask_file = us::any_cast<std::string>(parsedArgs["mask"]);

  std::string reference_mask_folder = "";
  if (parsedArgs.count("reference_mask_folder"))
    reference_mask_folder = us::any_cast<std::string>(parsedArgs["reference_mask_folder"]);

  try
  {
    itk::TimeProbe clock;
    clock.Start();

    MITK_INFO << "Creating directory structure";
    if (ist::PathExists(out_folder))
      ist::RemoveADirectory(out_folder);
    ist::MakeDirectory(out_folder);

    MITK_INFO << "Loading data";

    std::streambuf *old = cout.rdbuf(); // <-- save
    std::stringstream ss;
    std::cout.rdbuf (ss.rdbuf());       // <-- redirect

    ofstream logfile;
    logfile.open (out_folder + "log.txt");

    itk::ImageFileWriter< PeakImgType >::Pointer peak_image_writer = itk::ImageFileWriter< PeakImgType >::New();
    mitk::PreferenceListReaderOptionsFunctor functor = mitk::PreferenceListReaderOptionsFunctor({"Peak Image", "Fiberbundles"}, {});
    mitk::Image::Pointer inputImage = dynamic_cast<mitk::PeakImage*>(mitk::IOUtil::Load(peak_file_name, &functor)[0].GetPointer());

    float minSpacing = 1;
    if(inputImage->GetGeometry()->GetSpacing()[0]<inputImage->GetGeometry()->GetSpacing()[1] && inputImage->GetGeometry()->GetSpacing()[0]<inputImage->GetGeometry()->GetSpacing()[2])
      minSpacing = inputImage->GetGeometry()->GetSpacing()[0];
    else if (inputImage->GetGeometry()->GetSpacing()[1] < inputImage->GetGeometry()->GetSpacing()[2])
      minSpacing = inputImage->GetGeometry()->GetSpacing()[1];
    else
      minSpacing = inputImage->GetGeometry()->GetSpacing()[2];

    // Load mask file. Fit is only performed inside the mask
    itk::FitFibersToImageFilter::UcharImgType::Pointer mask = nullptr;
    if (mask_file.compare("")!=0)
    {
      mitk::Image::Pointer mitk_mask = dynamic_cast<mitk::Image*>(mitk::IOUtil::Load(mask_file)[0].GetPointer());
      mitk::CastToItkImage(mitk_mask, mask);
    }

    // Load masks covering the true positives for evaluation purposes
    std::vector< std::string > reference_mask_filenames;
    std::vector< itk::FitFibersToImageFilter::UcharImgType::Pointer > reference_masks;
    if (reference_mask_folder.compare("")!=0)
    {
      reference_mask_filenames = get_file_list(reference_mask_folder, {".nii", ".nii.gz", ".nrrd"});
      for (auto filename : reference_mask_filenames)
      {
        itk::FitFibersToImageFilter::UcharImgType::Pointer ref_mask = nullptr;
        mitk::Image::Pointer ref_mitk_mask = dynamic_cast<mitk::Image*>(mitk::IOUtil::Load(filename)[0].GetPointer());
        mitk::CastToItkImage(ref_mitk_mask, ref_mask);
        reference_masks.push_back(ref_mask);
      }
    }

    // Load peak image
    typedef mitk::ImageToItk< PeakImgType > CasterType;
    CasterType::Pointer caster = CasterType::New();
    caster->SetInput(inputImage);
    caster->Update();
    PeakImgType::Pointer peak_image = caster->GetOutput();

    // Load all candidate tracts
    std::vector< mitk::FiberBundle::Pointer > input_candidates;
    std::vector< std::string > candidate_tract_files = get_file_list(candidate_folder);
    for (std::string f : candidate_tract_files)
    {
      mitk::FiberBundle::Pointer fib = dynamic_cast<mitk::FiberBundle*>(mitk::IOUtil::Load(f)[0].GetPointer());
      if (fib.IsNull())
        continue;
      if (fib->GetNumFibers()<=0)
        return 0;
      fib->ResampleLinear(minSpacing/10.0);
      input_candidates.push_back(fib);
    }
    std::cout.rdbuf (old);              // <-- restore

    double rmse = 0.0;
    int iteration = 0;
    std::string name = "NOANCHOR";
    // Load reference tractogram consisting of all known tracts
    std::vector< mitk::FiberBundle::Pointer > input_reference;
    mitk::FiberBundle::Pointer anchor_tractogram = dynamic_cast<mitk::FiberBundle*>(mitk::IOUtil::Load(anchors_file)[0].GetPointer());
    if ( !(anchor_tractogram.IsNull() || anchor_tractogram->GetNumFibers()==0) )
    {
      std::streambuf *old = cout.rdbuf(); // <-- save
      std::stringstream ss;
      std::cout.rdbuf (ss.rdbuf());       // <-- redirect
      anchor_tractogram->ResampleLinear(minSpacing/10.0);
      std::cout.rdbuf (old);              // <-- restore
      input_reference.push_back(anchor_tractogram);

      // Fit known tracts to peak image to obtain underexplained image
      MITK_INFO << "Fit anchor tracts";
      itk::FitFibersToImageFilter::Pointer fitter = itk::FitFibersToImageFilter::New();
      fitter->SetTractograms(input_reference);
      fitter->SetLambda(lambda);
      fitter->SetFilterOutliers(filter_outliers);
      fitter->SetPeakImage(peak_image);
      fitter->SetVerbose(true);
      fitter->SetResampleFibers(false);
      fitter->SetMaskImage(mask);
      fitter->Update();
      rmse = fitter->GetRMSE();

      name = ist::GetFilenameWithoutExtension(anchors_file);
      mitk::FiberBundle::Pointer anchor_tracts = fitter->GetTractograms().at(0);
      anchor_tracts->SetFiberColors(255,255,255);
      mitk::IOUtil::Save(anchor_tracts, out_folder + "0_" + name + ".fib");

      peak_image = fitter->GetUnderexplainedImage();
      peak_image_writer->SetInput(peak_image);
      peak_image_writer->SetFileName(out_folder + boost::lexical_cast<std::string>(iteration) + "_" + name + ".nrrd");
      peak_image_writer->Update();
    }

    if (!greedy_add)
    {
      MITK_INFO << "Fit candidate tracts";
      itk::FitFibersToImageFilter::Pointer fitter = itk::FitFibersToImageFilter::New();
      fitter->SetLambda(lambda);
      fitter->SetFilterOutliers(filter_outliers);
      fitter->SetVerbose(true);
      fitter->SetPeakImage(peak_image);
      fitter->SetResampleFibers(false);
      fitter->SetMaskImage(mask);
      fitter->SetTractograms(input_candidates);
      fitter->Update();
      vnl_vector<double> rms_diff = fitter->GetRmsDiffPerBundle();

      vnl_vector<double> log_rms_diff = rms_diff-rms_diff.min_value() + 1;
      log_rms_diff = log_rms_diff.apply(std::log);
      log_rms_diff /= log_rms_diff.max_value();
      int c = 0;
      for (auto fib : input_candidates)
      {
        fib->SetFiberWeights( log_rms_diff[c] );
        fib->ColorFibersByFiberWeights(false, true);

        std::string bundle_name = ist::GetFilenameWithoutExtension(candidate_tract_files.at(c));

        std::streambuf *old = cout.rdbuf(); // <-- save
        std::stringstream ss;
        std::cout.rdbuf (ss.rdbuf());       // <-- redirect
        mitk::IOUtil::Save(fib, out_folder + boost::lexical_cast<std::string>((int)(10000*rms_diff[c])) + "_" + bundle_name + ".fib");

        float best_overlap = 0;
        int best_overlap_index = -1;
        int m_idx = 0;
        for (auto ref_mask : reference_masks)
        {
          float overlap = fib->GetOverlap(ref_mask, false);
          if (overlap>best_overlap)
          {
            best_overlap = overlap;
            best_overlap_index = m_idx;
          }
          ++m_idx;
        }

        unsigned int num_voxels = 0;
        {
          itk::TractDensityImageFilter< ItkUcharImageType >::Pointer masks_filter = itk::TractDensityImageFilter< ItkUcharImageType >::New();
          masks_filter->SetInputImage(mask);
          masks_filter->SetBinaryOutput(true);
          masks_filter->SetFiberBundle(fib);
          masks_filter->SetUseImageGeometry(true);
          masks_filter->Update();
          num_voxels = masks_filter->GetNumCoveredVoxels();
        }

        std::cout.rdbuf (old);              // <-- restore

        logfile << "RMS_DIFF: " << setprecision(5) << rms_diff[c] << " " << bundle_name << " " << num_voxels << "\n";
        if (best_overlap_index>=0)
          logfile << "Best_overlap: " << setprecision(5) << best_overlap << " " << ist::GetFilenameWithoutExtension(reference_mask_filenames.at(best_overlap_index)) << "\n";
        else
          logfile << "No_overlap\n";

        ++c;
      }

      mitk::FiberBundle::Pointer out_fib = mitk::FiberBundle::New();
      out_fib = out_fib->AddBundles(input_candidates);
      out_fib->ColorFibersByFiberWeights(false, true);
      mitk::IOUtil::Save(out_fib, out_folder + "AllCandidates.fib");
    }
    else
    {
      MITK_INFO << "RMSE: " << setprecision(5) << rmse;
      //    fitter->SetPeakImage(peak_image);

      // Iteratively add candidate bundles in a greedy manner
      while (!input_candidates.empty())
      {
        double next_rmse = rmse;
        double num_peaks = 0;
        mitk::FiberBundle::Pointer best_candidate = nullptr;
        PeakImgType::Pointer best_candidate_peak_image = nullptr;

        for (int i=0; i<(int)input_candidates.size(); ++i)
        {
          // WHY NECESSARY AGAIN??
          itk::FitFibersToImageFilter::Pointer fitter = itk::FitFibersToImageFilter::New();
          fitter->SetLambda(lambda);
          fitter->SetFilterOutliers(filter_outliers);
          fitter->SetVerbose(false);
          fitter->SetPeakImage(peak_image);
          fitter->SetResampleFibers(false);
          fitter->SetMaskImage(mask);
          // ******************************
          fitter->SetTractograms({input_candidates.at(i)});

          std::streambuf *old = cout.rdbuf(); // <-- save
          std::stringstream ss;
          std::cout.rdbuf (ss.rdbuf());       // <-- redirect
          fitter->Update();
          std::cout.rdbuf (old);              // <-- restore

          double candidate_rmse = fitter->GetRMSE();

          if (candidate_rmse<next_rmse)
          {
            next_rmse = candidate_rmse;
            num_peaks = fitter->GetNumCoveredDirections();
            best_candidate = fitter->GetTractograms().at(0);
            best_candidate_peak_image = fitter->GetUnderexplainedImage();
          }
        }

        if (best_candidate.IsNull())
          break;

        //      fitter->SetPeakImage(peak_image);
        peak_image = best_candidate_peak_image;

        int i=0;
        std::vector< mitk::FiberBundle::Pointer > remaining_candidates;
        std::vector< std::string > remaining_candidate_files;
        for (auto fib : input_candidates)
        {
          if (fib!=best_candidate)
          {
            remaining_candidates.push_back(fib);
            remaining_candidate_files.push_back(candidate_tract_files.at(i));
          }
          else
            name = ist::GetFilenameWithoutExtension(candidate_tract_files.at(i));
          ++i;
        }
        input_candidates = remaining_candidates;
        candidate_tract_files = remaining_candidate_files;

        iteration++;
        std::streambuf *old = cout.rdbuf(); // <-- save
        std::stringstream ss;
        std::cout.rdbuf (ss.rdbuf());       // <-- redirect
        // Save winning candidate
        mitk::IOUtil::Save(best_candidate, out_folder + boost::lexical_cast<std::string>(iteration) + "_" + name + ".fib");
        peak_image_writer->SetInput(peak_image);
        peak_image_writer->SetFileName(out_folder + boost::lexical_cast<std::string>(iteration) + "_" + name + ".nrrd");
        peak_image_writer->Update();

        // Calculate best overlap with reference masks for evaluation purposes
        float best_overlap = 0;
        int best_overlap_index = -1;
        i = 0;
        for (auto ref_mask : reference_masks)
        {
          float overlap = best_candidate->GetOverlap(ref_mask, false);
          if (overlap>best_overlap)
          {
            best_overlap = overlap;
            best_overlap_index = i;
          }
          ++i;
        }
        std::cout.rdbuf (old);              // <-- restore

        logfile << "RMSE: " << setprecision(5) << rmse << " " << name << " " << num_peaks << "\n";
        if (best_overlap_index>=0)
          logfile << "Best_overlap: " << setprecision(5) << best_overlap << " " << ist::GetFilenameWithoutExtension(reference_mask_filenames.at(best_overlap_index)) << "\n";
        else
          logfile << "No_overlap\n";
      }
    }

    clock.Stop();
    int h = clock.GetTotal()/3600;
    int m = ((int)clock.GetTotal()%3600)/60;
    int s = (int)clock.GetTotal()%60;
    MITK_INFO << "Plausibility estimation took " << h << "h, " << m << "m and " << s << "s";
    logfile.close();
  }
  catch (itk::ExceptionObject e)
  {
    std::cout << e;
    return EXIT_FAILURE;
  }
  catch (std::exception e)
  {
    std::cout << e.what();
    return EXIT_FAILURE;
  }
  catch (...)
  {
    std::cout << "ERROR!?!";
    return EXIT_FAILURE;
  }
  return EXIT_SUCCESS;
}


/*
@copyright Louis Dionne 2015
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/hana/detail/std/integer_sequence.hpp>

#include <type_traits>
using namespace boost::hana;

static_assert(std::is_same<
    detail::std::make_integer_sequence<int, 0>,
    detail::std::integer_sequence<int>
>::value, "");

static_assert(std::is_same<
    detail::std::make_integer_sequence<int, 1>,
    detail::std::integer_sequence<int, 0>
>::value, "");

static_assert(std::is_same<
    detail::std::make_integer_sequence<int, 2>,
    detail::std::integer_sequence<int, 0, 1>
>::value, "");

static_assert(std::is_same<
    detail::std::make_integer_sequence<int, 3>,
    detail::std::integer_sequence<int, 0, 1, 2>
>::value, "");

static_assert(std::is_same<
    detail::std::make_integer_sequence<int, 4>,
    detail::std::integer_sequence<int, 0, 1, 2, 3>
>::value, "");

static_assert(std::is_same<
    detail::std::make_integer_sequence<int, 5>,
    detail::std::integer_sequence<int, 0, 1, 2, 3, 4>
>::value, "");

static_assert(std::is_same<
    detail::std::make_integer_sequence<int, 6>,
    detail::std::integer_sequence<int, 0, 1, 2, 3, 4, 5>
>::value, "");

int main() { }


// Copyright David Abrahams 2002.
// 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)
#include <boost/python/operators.hpp>
#include <boost/python/class.hpp>
#include <boost/python/module.hpp>
#include <boost/python/def.hpp>
#include "test_class.hpp"
#include <boost/python/module.hpp>
#include <boost/python/class.hpp>
#include <boost/python/operators.hpp>
#include <boost/operators.hpp>
//#include <boost/python/str.hpp>
// Just use math.h here; trying to use std::pow() causes too much
// trouble for non-conforming compilers and libraries.
#include <math.h>

#if __GNUC__ != 2
# include <ostream>
#else
# include <ostream.h>
#endif

using namespace boost::python;

using namespace boost::python;

struct X : test_class<>
{
    typedef test_class<> base_t;

    X(int x) : base_t(x) {}
    X const operator+(X const& r) const { return X(value() + r.value()); }

//    typedef int (X::*safe_bool)() const;
//    operator safe_bool() const { return value() != 0 ? &X::value : 0; }
};

X operator-(X const& l, X const& r) { return X(l.value() - r.value()); }
X operator-(int l, X const& r) { return X(l - r.value()); }
X operator-(X const& l, int r) { return X(l.value() - r); }

X operator-(X const& x) { return X(-x.value()); }

X& operator-=(X& l, X const& r) { l.set(l.value() - r.value()); return l; }

bool operator<(X const& x, X const& y) { return x.value() < y.value(); }
bool operator<(X const& x, int y) { return x.value() < y; }
bool operator<(int x, X const& y) { return x < y.value(); }

X abs(X x) { return X(x.value() < 0 ? -x.value() : x.value()); }

X pow(X x, int y)
{
    return X(int(pow(double(x.value()), double(y))));
}

X pow(X x, X y)
{
    return X(int(pow(double(x.value()), double(y.value()))));
}

int pow(int x, X y)
{
    return int(pow(double(x), double(y.value())));
}

std::ostream& operator<<(std::ostream& s, X const& x)
{
    return s << x.value();
}

struct number
  : boost::integer_arithmetic<number>
{
    explicit number(long x_) : x(x_) {}
    operator long() const { return x; }

    template <class T>
    number& operator+=(T const& rhs)
    { x += rhs; return *this; }

    template <class T>
    number& operator-=(T const& rhs)
    { x -= rhs; return *this; }

    template <class T>
    number& operator*=(T const& rhs)
    { x *= rhs; return *this; }

    template <class T>
    number& operator/=(T const& rhs)
    { x /= rhs; return *this; }

    template <class T>
    number& operator%=(T const& rhs)
    { x %= rhs; return *this; }

   long x;
};

BOOST_PYTHON_MODULE(operators_ext)
{
    class_<X>("X", init<int>())
        .def("value", &X::value)
        .def(self + self)
        .def(self - self)
        .def(self - int())
        .def(other<int>() - self)
        .def(-self)
        .def(self < other<int>())
        .def(self < self)
        .def(1 < self)
        .def(self -= self)

        .def(abs(self))
        .def(str(self))

        .def(pow(self,self))
        .def(pow(self,int()))
        .def(pow(int(),self))
        .def(
            !self
            // "not self" is legal here but causes friction on a few
            // nonconforming compilers; it's cute because it looks
            // like python, but doing it here doesn't prove much and
            // just causes tests to fail or complicated workarounds to
            // be enacted.
        )
        ;

    class_<number>("number", init<long>())
      // interoperate with self
      .def(self += self)
      .def(self + self)
      .def(self -= self)
      .def(self - self)
      .def(self *= self)
      .def(self * self)
      .def(self /= self)
      .def(self / self)
      .def(self %= self)
      .def(self % self)

      // Convert to Python int
      .def(int_(self))

      // interoperate with long
      .def(self += long())
      .def(self + long())
      .def(long() + self)
      .def(self -= long())
      .def(self - long())
      .def(long() - self)
      .def(self *= long())
      .def(self * long())
      .def(long() * self)
      .def(self /= long())
      .def(self / long())
      .def(long() / self)
      .def(self %= long())
      .def(self % long())
      .def(long() % self)
      ;

    class_<test_class<1> >("Z", init<int>())
        .def(int_(self))
        .def(float_(self))
        .def(complex_(self))
        ;
}

#include "module_tail.cpp"


#ifndef STAN_MATH_PRIM_MAT_FUN_DIVIDE_HPP
#define STAN_MATH_PRIM_MAT_FUN_DIVIDE_HPP

#include <boost/type_traits/is_arithmetic.hpp>
#include <boost/utility/enable_if.hpp>
#include <stan/math/prim/mat/fun/Eigen.hpp>

namespace stan {
  namespace math {

    /**
     * Return specified matrix divided by specified scalar.
     * @tparam R Row type for matrix.
     * @tparam C Column type for matrix.
     * @param m Matrix.
     * @param c Scalar.
     * @return Matrix divided by scalar.
     */
    template <int R, int C, typename T>
    inline
    typename boost::enable_if_c<boost::is_arithmetic<T>::value,
                                Eigen::Matrix<double, R, C> >::type
    divide(const Eigen::Matrix<double, R, C>& m,
           T c) {
      return m / c;
    }

  }
}
#endif


#include <boost/unordered_map.hpp>
#include <iostream>
#include <openssl/sha.h>
#include "momentum.h"
#include "hashblock.h"
namespace bts
{
	#define MAX_MOMENTUM_NONCE  (1<<22)
	#define SEARCH_SPACE_BITS 42
	#define BIRTHDAYS_PER_HASH 8

	bool momentum_search( uint256 midHash ,std::vector< std::pair<uint32_t,uint32_t> > &results)
  {
			semiOrderedMap somap;
			if(!somap.allocate(4))
			{
				return false;
			}

      char  hash_tmp[sizeof(midHash)+4];
      memcpy((char*)&hash_tmp[4], (char*)&midHash, sizeof(midHash) );
      uint32_t* index = (uint32_t*)hash_tmp;

      for( uint32_t i = 0; i < MAX_MOMENTUM_NONCE;  )
      {
	      if(i%1048576==0){
		boost::this_thread::interruption_point();
	      }
          *index = i;
          uint64_t  result_hash[8];
          HashGroestl((unsigned char*)hash_tmp, sizeof(hash_tmp), (unsigned char*)result_hash);

          for( uint32_t x = 0; x < 8; ++x )
          {
              uint64_t birthday = result_hash[x] >> (64-SEARCH_SPACE_BITS);
              uint32_t nonce = i+x;
              //boost::unordered_map<uint64_t,uint32_t>::const_iterator itr = found.find( birthday );

              uint64_t foundMatch=somap.checkAdd( birthday, nonce );
              if( foundMatch != 0 ){
                  results.push_back( std::make_pair( foundMatch, nonce ) );
              }
          }
          i += BIRTHDAYS_PER_HASH;
      }
      //for( auto itr = results.begin(); itr != results.end(); ++itr )
      //{
      //   assert( momentum_verify( midHash, itr->first, itr->second ) );
     // }
      //somap.destroy();
      return true;
   }

	uint64_t getBirthdayHash(const uint256& midHash, uint32_t a)
  {
    uint32_t index = a - (a%8);
    char  hash_tmp[sizeof(midHash)+4];
  //  std::cerr<<"midHash size:" <<sizeof(midHash)<<"\n";
    memcpy(&hash_tmp[4], (char*)&midHash, sizeof(midHash) );
    memcpy(&hash_tmp[0], (char*)&index, sizeof(index) );

    uint64_t  result_hash[8];
//      for( uint32_t i = 0; i < sizeof(hash_tmp); ++i )
 //     {
  //       std::cerr<<" "<<uint16_t((((unsigned char*)hash_tmp)[i]));
   //   }
//      std::cerr<<"\n";
		HashGroestl((unsigned char*)hash_tmp, sizeof(hash_tmp), (unsigned char*)&result_hash);
//    std::cerr<<"result_hash "<<a<<"  "<<a%8<<"  --- ";
 //   for( uint32_t i = 0; i < 8; ++i ) std::cerr<<result_hash[i]<<" ";
  //  std::cerr<<"\n";

    uint64_t r = result_hash[a%BIRTHDAYS_PER_HASH]>>(64-SEARCH_SPACE_BITS);
  //  std::cerr<<"bdayresult: "<<r<<"\n";
    return r;
	}

	bool momentum_verify( uint256 head, uint32_t a, uint32_t b ){
 //   std::cerr<<"verify  "<<a<<"  and "<<b<<"  mid: "<<head.ToString()<<"\n";
 //   std::cerr<<"    "<<getBirthdayHash(head,a)<<"   "<<getBirthdayHash(head,b)<<"\n";
		if( a == b ) return false;
		if( a > MAX_MOMENTUM_NONCE ) return false;
		if( b > MAX_MOMENTUM_NONCE ) return false;

     bool r = (getBirthdayHash(head,a) == getBirthdayHash(head,b));
  //   std::cerr<< "####### Verified "<<int(r)<<"\n";
     return r;
	}

}


//
// Copyright (c) 2015-2019 CNRS INRIA
// Copyright (c) 2015-2016 Wandercraft, 86 rue de Paris 91400 Orsay, France.
//

#ifndef __pinocchio_joint_spherical_ZYX_hpp__
#define __pinocchio_joint_spherical_ZYX_hpp__

#include "pinocchio/macros.hpp"
#include "pinocchio/multibody/joint/joint-base.hpp"
#include "pinocchio/multibody/joint/joint-spherical.hpp"
#include "pinocchio/multibody/constraint.hpp"
#include "pinocchio/math/sincos.hpp"
#include "pinocchio/math/matrix.hpp"
#include "pinocchio/spatial/inertia.hpp"
#include "pinocchio/spatial/skew.hpp"

namespace pinocchio
{
  template<typename Scalar, int Options> struct ConstraintSphericalZYXTpl;

  template <typename _Scalar, int _Options>
  struct traits< ConstraintSphericalZYXTpl<_Scalar,_Options> >
  {
    typedef _Scalar Scalar;
    enum { Options = _Options };

    enum {
      LINEAR = 0,
      ANGULAR = 3
    };

    typedef MotionSphericalTpl<Scalar,Options> JointMotion;
    typedef Eigen::Matrix<Scalar,3,1,Options> JointForce;
    typedef Eigen::Matrix<Scalar,6,3,Options> DenseBase;

    typedef DenseBase MatrixReturnType;
    typedef const DenseBase ConstMatrixReturnType;
  }; // struct traits struct ConstraintRotationalSubspace

  template<typename _Scalar, int _Options>
  struct ConstraintSphericalZYXTpl
  : public ConstraintBase< ConstraintSphericalZYXTpl<_Scalar,_Options> >
  {
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    PINOCCHIO_CONSTRAINT_TYPEDEF_TPL(ConstraintSphericalZYXTpl)

    enum { NV = 3 };
    typedef Eigen::Matrix<Scalar,3,3,Options> Matrix3;

    ConstraintSphericalZYXTpl() {}

    template<typename Matrix3Like>
    ConstraintSphericalZYXTpl(const Eigen::MatrixBase<Matrix3Like> & subspace)
    : S_minimal(subspace)
    {  EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix3Like,3,3); }

    template<typename Vector3Like>
    JointMotion __mult__(const Eigen::MatrixBase<Vector3Like> & v) const
    {
      EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector3Like,3);
      return JointMotion(S_minimal * v);
    }

    Matrix3 & operator() () { return S_minimal; }
    const Matrix3 & operator() () const { return S_minimal; }

    int nv_impl() const { return NV; }

    struct ConstraintTranspose
    {
      const ConstraintSphericalZYXTpl & ref;
      ConstraintTranspose(const ConstraintSphericalZYXTpl & ref) : ref(ref) {}

      template<typename Derived>
      const typename MatrixMatrixProduct<
      Eigen::Transpose<const Matrix3>,
      Eigen::Block<const typename ForceDense<Derived>::Vector6,3,1>
      >::type
      operator* (const ForceDense<Derived> & phi) const
      {
        return ref.S_minimal.transpose() * phi.angular();
      }

      /* [CRBA]  MatrixBase operator* (Constraint::Transpose S, ForceSet::Block) */
      template<typename D>
      const typename MatrixMatrixProduct<
      typename Eigen::Transpose<const Matrix3>,
      typename Eigen::MatrixBase<const D>::template NRowsBlockXpr<3>::Type
      >::type
      operator* (const Eigen::MatrixBase<D> & F) const
      {
        EIGEN_STATIC_ASSERT(D::RowsAtCompileTime==6,THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE)
        return ref.S_minimal.transpose () * F.template middleRows<3>(ANGULAR);
      }
    }; // struct ConstraintTranspose

    ConstraintTranspose transpose () const { return ConstraintTranspose(*this); }

    DenseBase matrix_impl() const
    {
      DenseBase S;
      S.template middleRows<3>(LINEAR).setZero();
      S.template middleRows<3>(ANGULAR) = S_minimal;
      return S;
    }

    //      const typename Eigen::ProductReturnType<
    //      const ConstraintDense,
    //      const Matrix3
    //      >::Type
    template<typename S1, int O1>
    Eigen::Matrix<Scalar,6,3,Options>
    se3Action(const SE3Tpl<S1,O1> & m) const
    {
      //        Eigen::Matrix <Scalar,6,3,Options> X_subspace;
      //        X_subspace.template block <3,3> (Motion::LINEAR, 0) = skew (m.translation ()) * m.rotation ();
      //        X_subspace.template block <3,3> (Motion::ANGULAR, 0) = m.rotation ();
      //
      //        return (X_subspace * S_minimal).eval();

      Eigen::Matrix<Scalar,6,3,Options> result;

      // ANGULAR
      result.template middleRows<3>(ANGULAR).noalias() = m.rotation () * S_minimal;

      // LINEAR
      cross(m.translation(),
            result.template middleRows<3>(Motion::ANGULAR),
            result.template middleRows<3>(LINEAR));

      return result;
    }

    template<typename S1, int O1>
    Eigen::Matrix<Scalar,6,3,Options>
    se3ActionInverse(const SE3Tpl<S1,O1> & m) const
    {
      Eigen::Matrix<Scalar,6,3,Options> result;

      // LINEAR
      cross(m.translation(),
            S_minimal,
            result.template middleRows<3>(ANGULAR));
      result.template middleRows<3>(LINEAR).noalias() = -m.rotation().transpose() * result.template middleRows<3>(ANGULAR);

      // ANGULAR
      result.template middleRows<3>(ANGULAR).noalias() = m.rotation().transpose() * S_minimal;

      return result;
    }

    template<typename MotionDerived>
    DenseBase motionAction(const MotionDense<MotionDerived> & m) const
    {
      const typename MotionDerived::ConstLinearType v = m.linear();
      const typename MotionDerived::ConstAngularType w = m.angular();

      DenseBase res;
      cross(v,S_minimal,res.template middleRows<3>(LINEAR));
      cross(w,S_minimal,res.template middleRows<3>(ANGULAR));

      return res;
    }

    // data
    Matrix3 S_minimal;

  }; // struct ConstraintSphericalZYXTpl

  /* [CRBA] ForceSet operator* (Inertia Y,Constraint S) */
  template <typename S1, int O1, typename S2, int O2>
  Eigen::Matrix<S1,6,3,O1>
  operator*(const InertiaTpl<S1,O1> & Y,
            const ConstraintSphericalZYXTpl<S2,O2> & S)
  {
    typedef typename InertiaTpl<S1,O1>::Symmetric3 Symmetric3;
    typedef ConstraintSphericalZYXTpl<S2,O2> Constraint;
    Eigen::Matrix<S1,6,3,O1> M;
    alphaSkew (-Y.mass(),Y.lever(),M.template middleRows<3>(Constraint::LINEAR));
    M.template middleRows<3>(Constraint::ANGULAR) =  (Y.inertia () -
    typename Symmetric3::AlphaSkewSquare(Y.mass (), Y.lever ())).matrix();

    return (M * S.S_minimal).eval();
  }

  /* [ABA] Y*S operator (Inertia Y,Constraint S) */
  //  inline Eigen::Matrix<double,6,3>
  template<typename Matrix6Like, typename S2, int O2>
  const typename MatrixMatrixProduct<
  typename Eigen::internal::remove_const<typename SizeDepType<3>::ColsReturn<Matrix6Like>::ConstType>::type,
  typename ConstraintSphericalZYXTpl<S2,O2>::Matrix3
  >::type
  operator*(const Eigen::MatrixBase<Matrix6Like> & Y,
            const ConstraintSphericalZYXTpl<S2,O2> & S)
  {
    EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix6Like,6,6);
    return Y.derived().template middleCols<3>(Inertia::ANGULAR) * S.S_minimal;
  }

  template<typename S1, int O1>
  struct SE3GroupAction< ConstraintSphericalZYXTpl<S1,O1> >
  {
    //      typedef const typename Eigen::ProductReturnType<
    //      Eigen::Matrix <double,6,3,0>,
    //      Eigen::Matrix <double,3,3,0>
    //      >::Type Type;
    typedef Eigen::Matrix<S1,6,3,O1> ReturnType;
  };

  template<typename S1, int O1, typename MotionDerived>
  struct MotionAlgebraAction< ConstraintSphericalZYXTpl<S1,O1>, MotionDerived >
  {
    typedef Eigen::Matrix<S1,6,3,O1> ReturnType;
  };

  template<typename Scalar, int Options> struct JointSphericalZYXTpl;

  template<typename _Scalar, int _Options>
  struct traits< JointSphericalZYXTpl<_Scalar,_Options> >
  {
    enum {
      NQ = 3,
      NV = 3
    };
    typedef _Scalar Scalar;
    enum { Options = _Options };
    typedef JointDataSphericalZYXTpl<Scalar,Options> JointDataDerived;
    typedef JointModelSphericalZYXTpl<Scalar,Options> JointModelDerived;
    typedef ConstraintSphericalZYXTpl<Scalar,Options> Constraint_t;
    typedef SE3Tpl<Scalar,Options> Transformation_t;
    typedef MotionSphericalTpl<Scalar,Options> Motion_t;
    typedef MotionSphericalTpl<Scalar,Options> Bias_t;

    // [ABA]
    typedef Eigen::Matrix<Scalar,6,NV,Options> U_t;
    typedef Eigen::Matrix<Scalar,NV,NV,Options> D_t;
    typedef Eigen::Matrix<Scalar,6,NV,Options> UD_t;

    PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE

    typedef Eigen::Matrix<Scalar,NQ,1,Options> ConfigVector_t;
    typedef Eigen::Matrix<Scalar,NV,1,Options> TangentVector_t;
  };

  template<typename Scalar, int Options>
  struct traits< JointDataSphericalZYXTpl<Scalar,Options> >
  { typedef JointSphericalZYXTpl<Scalar,Options> JointDerived; };

  template<typename Scalar, int Options>
  struct traits< JointModelSphericalZYXTpl<Scalar,Options> >
  { typedef JointSphericalZYXTpl<Scalar,Options> JointDerived; };

  template<typename _Scalar, int _Options>
  struct JointDataSphericalZYXTpl : public JointDataBase< JointDataSphericalZYXTpl<_Scalar,_Options> >
  {
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    typedef JointSphericalZYXTpl<_Scalar,_Options> JointDerived;
    PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(JointDerived);
    PINOCCHIO_JOINT_DATA_BASE_DEFAULT_ACCESSOR

    Constraint_t S;
    Transformation_t M;
    Motion_t v;
    Bias_t c;

    // [ABA] specific data
    U_t U;
    D_t Dinv;
    UD_t UDinv;
    D_t StU;

    JointDataSphericalZYXTpl () : M(1), U(), Dinv(), UDinv() {}

    static std::string classname() { return std::string("JointDataSphericalZYX"); }
    std::string shortname() const { return classname(); }

  }; // strcut JointDataSphericalZYXTpl

  PINOCCHIO_JOINT_CAST_TYPE_SPECIALIZATION(JointModelSphericalZYXTpl);
  template<typename _Scalar, int _Options>
  struct JointModelSphericalZYXTpl
  : public JointModelBase< JointModelSphericalZYXTpl<_Scalar,_Options> >
  {
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    typedef JointSphericalZYXTpl<_Scalar,_Options> JointDerived;
    PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(JointDerived);

    typedef JointModelBase<JointModelSphericalZYXTpl> Base;
    using Base::id;
    using Base::idx_q;
    using Base::idx_v;
    using Base::setIndexes;

    JointDataDerived createData() const { return JointDataDerived(); }

    template<typename ConfigVector>
    void calc(JointDataDerived & data,
              const typename Eigen::MatrixBase<ConfigVector> & qs) const
    {
      typename ConfigVector::template ConstFixedSegmentReturnType<NQ>::Type & q = qs.template segment<NQ>(idx_q());

      typedef typename ConfigVector::Scalar S2;

      S2 c0,s0; SINCOS(q(0), &s0, &c0);
      S2 c1,s1; SINCOS(q(1), &s1, &c1);
      S2 c2,s2; SINCOS(q(2), &s2, &c2);

      data.M.rotation () << c0 * c1,
                c0 * s1 * s2 - s0 * c2,
                c0 * s1 * c2 + s0 * s2,
                s0 * c1,
                s0 * s1 * s2 + c0 * c2,
                s0 * s1 * c2 - c0 * s2,
                -s1,
                c1 * s2,
                c1 * c2;

      data.S.S_minimal
      <<  -s1, Scalar(0), Scalar(1),
      c1 * s2, c2, Scalar(0),
      c1 * c2, -s2, Scalar(0);
    }

    template<typename ConfigVector, typename TangentVector>
    void calc(JointDataDerived & data,
              const typename Eigen::MatrixBase<ConfigVector> & qs,
              const typename Eigen::MatrixBase<TangentVector> & vs) const
    {
      typename ConfigVector::template ConstFixedSegmentReturnType<NQ>::Type & q = qs.template segment<NQ>(idx_q());

      typedef typename ConfigVector::Scalar S2;

      S2 c0,s0; SINCOS(q(0), &s0, &c0);
      S2 c1,s1; SINCOS(q(1), &s1, &c1);
      S2 c2,s2; SINCOS(q(2), &s2, &c2);

      data.M.rotation () << c0 * c1,
          c0 * s1 * s2 - s0 * c2,
          c0 * s1 * c2 + s0 * s2,
          s0 * c1,
          s0 * s1 * s2 + c0 * c2,
          s0 * s1 * c2 - c0 * s2,
          -s1,
          c1 * s2,
          c1 * c2;

      data.S.S_minimal
      <<  -s1, Scalar(0), Scalar(1),
      c1 * s2, c2, Scalar(0),
      c1 * c2, -s2, Scalar(0);

      typename TangentVector::template ConstFixedSegmentReturnType<NV>::Type
      & q_dot = vs.template segment<NV>(idx_v());

      data.v().noalias() = data.S.S_minimal * q_dot;

      data.c()(0) = -c1 * q_dot(0) * q_dot(1);
      data.c()(1) = -s1 * s2 * q_dot(0) * q_dot(1) + c1 * c2 * q_dot(0) * q_dot(2) - s2 * q_dot(1) * q_dot(2);
      data.c()(2) = -s1 * c2 * q_dot(0) * q_dot(1) - c1 * s2 * q_dot(0) * q_dot(2) - c2 * q_dot(1) * q_dot(2);
    }

    template<typename Matrix6Like>
    void calc_aba(JointDataDerived & data,
                  const Eigen::MatrixBase<Matrix6Like> & I,
                  const bool update_I) const
    {
      data.U.noalias() = I.template middleCols<3>(Motion::ANGULAR) * data.S.S_minimal;
      data.StU.noalias() = data.S.S_minimal.transpose() * data.U.template middleRows<3>(Motion::ANGULAR);

      // compute inverse
//      data.Dinv.setIdentity();
//      data.StU.llt().solveInPlace(data.Dinv);
      internal::PerformStYSInversion<Scalar>::run(data.StU,data.Dinv);

      data.UDinv.noalias() = data.U * data.Dinv;

      if (update_I)
        PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like,I) -= data.UDinv * data.U.transpose();
    }

    static std::string classname() { return std::string("JointModelSphericalZYX"); }
    std::string shortname() const { return classname(); }

    /// \returns An expression of *this with the Scalar type casted to NewScalar.
    template<typename NewScalar>
    JointModelSphericalZYXTpl<NewScalar,Options> cast() const
    {
      typedef JointModelSphericalZYXTpl<NewScalar,Options> ReturnType;
      ReturnType res;
      res.setIndexes(id(),idx_q(),idx_v());
      return res;
    }

  }; // struct JointModelSphericalZYXTpl

} // namespace pinocchio

#include <boost/type_traits.hpp>

namespace boost
{
  template<typename Scalar, int Options>
  struct has_nothrow_constructor< ::pinocchio::JointModelSphericalZYXTpl<Scalar,Options> >
  : public integral_constant<bool,true> {};

  template<typename Scalar, int Options>
  struct has_nothrow_copy< ::pinocchio::JointModelSphericalZYXTpl<Scalar,Options> >
  : public integral_constant<bool,true> {};

  template<typename Scalar, int Options>
  struct has_nothrow_constructor< ::pinocchio::JointDataSphericalZYXTpl<Scalar,Options> >
  : public integral_constant<bool,true> {};

  template<typename Scalar, int Options>
  struct has_nothrow_copy< ::pinocchio::JointDataSphericalZYXTpl<Scalar,Options> >
  : public integral_constant<bool,true> {};
}

#endif // ifndef __pinocchio_joint_spherical_ZYX_hpp__


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

// Copyright (c) 2007-2012 Barend Gehrels, Amsterdam, the Netherlands.
// Copyright (c) 2017 Adam Wulkiewicz, Lodz, Poland.

// This file was modified by Oracle on 2017-2020.
// Modifications copyright (c) 2017-2020 Oracle and/or its affiliates.

// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

// 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_DETAIL_OVERLAY_SELF_TURN_POINTS_HPP
#define BOOST_GEOMETRY_ALGORITHMS_DETAIL_OVERLAY_SELF_TURN_POINTS_HPP

#include <cstddef>

#include <boost/geometry/algorithms/detail/disjoint/box_box.hpp>
#include <boost/geometry/algorithms/detail/partition.hpp>
#include <boost/geometry/algorithms/detail/overlay/do_reverse.hpp>
#include <boost/geometry/algorithms/detail/overlay/get_turns.hpp>
#include <boost/geometry/algorithms/detail/sections/section_box_policies.hpp>

#include <boost/geometry/core/access.hpp>
#include <boost/geometry/core/coordinate_dimension.hpp>
#include <boost/geometry/core/point_order.hpp>
#include <boost/geometry/core/tags.hpp>

#include <boost/geometry/geometries/box.hpp>
#include <boost/geometry/geometries/concepts/check.hpp>

#include <boost/geometry/util/condition.hpp>

namespace boost { namespace geometry
{

#ifndef DOXYGEN_NO_DETAIL
namespace detail { namespace self_get_turn_points
{

struct no_interrupt_policy
{
    static bool const enabled = false;
    static bool const has_intersections = false;

    template <typename Range>
    static inline bool apply(Range const&)
    {
        return false;
    }
};

template
<
    bool Reverse,
    typename Geometry,
    typename Turns,
    typename TurnPolicy,
    typename IntersectionStrategy,
    typename RobustPolicy,
    typename InterruptPolicy
>
struct self_section_visitor
{
    Geometry const& m_geometry;
    IntersectionStrategy const& m_intersection_strategy;
    RobustPolicy const& m_rescale_policy;
    Turns& m_turns;
    InterruptPolicy& m_interrupt_policy;
    int m_source_index;
    bool m_skip_adjacent;

    inline self_section_visitor(Geometry const& g,
                                IntersectionStrategy const& is,
                                RobustPolicy const& rp,
                                Turns& turns,
                                InterruptPolicy& ip,
                                int source_index,
                                bool skip_adjacent)
        : m_geometry(g)
        , m_intersection_strategy(is)
        , m_rescale_policy(rp)
        , m_turns(turns)
        , m_interrupt_policy(ip)
        , m_source_index(source_index)
        , m_skip_adjacent(skip_adjacent)
    {}

    template <typename Section>
    inline bool apply(Section const& sec1, Section const& sec2)
    {
        if (! detail::disjoint::disjoint_box_box(sec1.bounding_box,
                                                 sec2.bounding_box,
                                                 m_intersection_strategy.get_disjoint_box_box_strategy())
                && ! sec1.duplicate
                && ! sec2.duplicate)
        {
            // false if interrupted
            return detail::get_turns::get_turns_in_sections
                    <
                        Geometry, Geometry,
                        Reverse, Reverse,
                        Section, Section,
                        TurnPolicy
                    >::apply(m_source_index, m_geometry, sec1,
                             m_source_index, m_geometry, sec2,
                             false, m_skip_adjacent,
                             m_intersection_strategy,
                             m_rescale_policy,
                             m_turns, m_interrupt_policy);
        }

        return true;
    }

};

template <bool Reverse, typename TurnPolicy>
struct get_turns
{
    template <typename Geometry, typename IntersectionStrategy, typename RobustPolicy, typename Turns, typename InterruptPolicy>
    static inline bool apply(
            Geometry const& geometry,
            IntersectionStrategy const& intersection_strategy,
            RobustPolicy const& robust_policy,
            Turns& turns,
            InterruptPolicy& interrupt_policy,
            int source_index, bool skip_adjacent)
    {
        typedef model::box
            <
                typename geometry::robust_point_type
                <
                    typename geometry::point_type<Geometry>::type,
                    RobustPolicy
                >::type
            > box_type;

        // sectionalize in two dimensions to detect
        // all potential spikes correctly
        typedef geometry::sections<box_type, 2> sections_type;

        typedef std::integer_sequence<std::size_t, 0, 1> dimensions;

        sections_type sec;
        geometry::sectionalize<Reverse, dimensions>(geometry, robust_policy, sec,
                                                    intersection_strategy.get_envelope_strategy(),
                                                    intersection_strategy.get_expand_strategy());

        self_section_visitor
            <
                Reverse, Geometry,
                Turns, TurnPolicy, IntersectionStrategy, RobustPolicy, InterruptPolicy
            > visitor(geometry, intersection_strategy, robust_policy, turns, interrupt_policy, source_index, skip_adjacent);

        typedef detail::section::get_section_box
            <
                typename IntersectionStrategy::expand_box_strategy_type
            > get_section_box_type;
        typedef detail::section::overlaps_section_box
            <
                typename IntersectionStrategy::disjoint_box_box_strategy_type
            > overlaps_section_box_type;

        // false if interrupted
        geometry::partition
            <
                box_type
            >::apply(sec, visitor,
                     get_section_box_type(),
                     overlaps_section_box_type());

        return ! interrupt_policy.has_intersections;
    }
};

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

#ifndef DOXYGEN_NO_DISPATCH
namespace dispatch
{

template
<
    bool Reverse,
    typename GeometryTag,
    typename Geometry,
    typename TurnPolicy
>
struct self_get_turn_points
{
};

template
<
    bool Reverse,
    typename Ring,
    typename TurnPolicy
>
struct self_get_turn_points
    <
        Reverse, ring_tag, Ring,
        TurnPolicy
    >
    : detail::self_get_turn_points::get_turns<Reverse, TurnPolicy>
{};

template
<
    bool Reverse,
    typename Box,
    typename TurnPolicy
>
struct self_get_turn_points
    <
        Reverse, box_tag, Box,
        TurnPolicy
    >
{
    template <typename Strategy, typename RobustPolicy, typename Turns, typename InterruptPolicy>
    static inline bool apply(
            Box const& ,
            Strategy const& ,
            RobustPolicy const& ,
            Turns& ,
            InterruptPolicy& ,
            int /*source_index*/,
            bool /*skip_adjacent*/)
    {
        return true;
    }
};

template
<
    bool Reverse,
    typename Polygon,
    typename TurnPolicy
>
struct self_get_turn_points
    <
        Reverse, polygon_tag, Polygon,
        TurnPolicy
    >
    : detail::self_get_turn_points::get_turns<Reverse, TurnPolicy>
{};

template
<
    bool Reverse,
    typename MultiPolygon,
    typename TurnPolicy
>
struct self_get_turn_points
    <
        Reverse, multi_polygon_tag, MultiPolygon,
        TurnPolicy
    >
    : detail::self_get_turn_points::get_turns<Reverse, TurnPolicy>
{};

} // namespace dispatch
#endif // DOXYGEN_NO_DISPATCH

#ifndef DOXYGEN_NO_DETAIL
namespace detail { namespace self_get_turn_points
{

// Version where Reverse can be specified manually. TODO:
// can most probably be merged with self_get_turn_points::get_turn
template
<
    bool Reverse,
    typename AssignPolicy,
    typename Geometry,
    typename IntersectionStrategy,
    typename RobustPolicy,
    typename Turns,
    typename InterruptPolicy
>
inline void self_turns(Geometry const& geometry,
                       IntersectionStrategy const& strategy,
                       RobustPolicy const& robust_policy,
                       Turns& turns,
                       InterruptPolicy& interrupt_policy,
                       int source_index = 0,
                       bool skip_adjacent = false)
{
    concepts::check<Geometry const>();

    typedef detail::overlay::get_turn_info<detail::overlay::assign_null_policy> turn_policy;

    dispatch::self_get_turn_points
            <
                Reverse,
                typename tag<Geometry>::type,
                Geometry,
                turn_policy
            >::apply(geometry, strategy, robust_policy, turns, interrupt_policy,
                     source_index, skip_adjacent);
}

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

/*!
    \brief Calculate self intersections of a geometry
    \ingroup overlay
    \tparam Geometry geometry type
    \tparam Turns type of intersection container
                (e.g. vector of "intersection/turn point"'s)
    \param geometry geometry
    \param strategy strategy to be used
    \param robust_policy policy to handle robustness issues
    \param turns container which will contain intersection points
    \param interrupt_policy policy determining if process is stopped
        when intersection is found
    \param source_index source index for generated turns
    \param skip_adjacent indicates if adjacent turns should be skipped
 */
template
<
    typename AssignPolicy,
    typename Geometry,
    typename IntersectionStrategy,
    typename RobustPolicy,
    typename Turns,
    typename InterruptPolicy
>
inline void self_turns(Geometry const& geometry,
                       IntersectionStrategy const& strategy,
                       RobustPolicy const& robust_policy,
                       Turns& turns,
                       InterruptPolicy& interrupt_policy,
                       int source_index = 0,
                       bool skip_adjacent = false)
{
    concepts::check<Geometry const>();

    static bool const reverse =  detail::overlay::do_reverse
        <
            geometry::point_order<Geometry>::value
        >::value;

    detail::self_get_turn_points::self_turns
            <
                reverse,
                AssignPolicy
            >(geometry, strategy, robust_policy, turns, interrupt_policy,
              source_index, skip_adjacent);
}

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_ALGORITHMS_DETAIL_OVERLAY_SELF_TURN_POINTS_HPP