#include <type_traits>
#include <utility>
#include <cassert>
#include <queue>
#include <gtest/gtest.h>
#include <boost/graph/graph_concepts.hpp>
#include <graph/static_graph.hpp>
#include <graph/graph.hpp>
#include <graph/properties.hpp>
#include <graph/breadth_first_search.hpp>
#include <graph/dijkstra.hpp>
#include <graph/bidirectional_dijkstra.hpp>
#include <graph/io.hpp>
#include <generator.hpp>

using namespace std;
using namespace graph;

//BFS related properties
struct distance_t {};
struct color_t {};
struct edge_type_t {};

using BFSBundledVertexProperties = Properties<Property<distance_t, uint32_t>, Property<color_t, char>>;
using BFSBundledEdgeProperties = Properties<Property<edge_type_t, char>>;

TEST(GraphConcepts, GraphConcept) {
    using Graph = StaticGraph<>;
    BOOST_CONCEPT_ASSERT((boost::concepts::GraphConcept<Graph>));
};

TEST(GraphConcepts, VertexListGraphConcept) {
    using Graph = StaticGraph<>;
    BOOST_CONCEPT_ASSERT((boost::concepts::VertexListGraphConcept<Graph>));
};

TEST(GraphConcepts, AdjacencyGraphConcept) {
    using Graph = StaticGraph<>;
	BOOST_CONCEPT_ASSERT((boost::concepts::AdjacencyGraphConcept<Graph>));
};

TEST(GraphConcepts, IncidenceGraphConcept) {
    using Graph = StaticGraph<>;
    BOOST_CONCEPT_ASSERT((boost::concepts::IncidenceGraphConcept<Graph>));
};

TEST(GraphConcepts, BidirectionalGraphConcept) {
    using Graph = StaticGraph<BFSBundledVertexProperties, BFSBundledEdgeProperties>;
    BOOST_CONCEPT_ASSERT((boost::concepts::BidirectionalGraphConcept<Graph>));
};

TEST(PropertyGraph, InternalProperties) {
    using Graph = StaticGraph<BFSBundledVertexProperties, BFSBundledEdgeProperties>;

    BOOST_CONCEPT_ASSERT((boost::ReadWritePropertyMapConcept<
        graph::property_map<Graph, graph::vertex_bundle_t>::type,
        graph::property_map<Graph, graph::vertex_bundle_t>::type::key_type>));
    BOOST_CONCEPT_ASSERT((boost::ReadWritePropertyMapConcept<
        graph::property_map<Graph, graph::edge_bundle_t>::type,
        graph::property_map<Graph, graph::edge_bundle_t>::type::key_type>));
};

TEST(PropertyGraph, InternalColorEdgeTypeProperties) {
    using Graph = StaticGraph<BFSBundledVertexProperties, BFSBundledEdgeProperties>;

    BOOST_CONCEPT_ASSERT((boost::ReadWritePropertyMapConcept<
        graph::property_map<Graph, color_t>::type,
        graph::property_map<Graph, color_t>::type::key_type>));
    BOOST_CONCEPT_ASSERT((boost::ReadWritePropertyMapConcept<
        graph::property_map<Graph, edge_type_t>::type,
        graph::property_map<Graph, edge_type_t>::type::key_type>));
};

//TEST(PropertyGraph, VertexIndexProperty) {
//    using Graph = BFSGraph;
//    using Vertex = typename graph_traits<Graph>::vertex_descriptor;
//    using Map = typename property_map<Graph, graph::vertex_index_t>::type;
//    using Index = Graph::vertices_size_type;
//    Map m = get(vertex_index_t{}, g);
//    Index x = get(vertex_index_t{}, g, Vertex())
//};

TEST(GraphStructure, ListGraph) {
    using Graph = StaticGraph<>;

    using SizeT = size_t;
    using VecPair = vector<pair<SizeT, SizeT>>;
    const SizeT n = 1 << 2;
    VecPair input;
    input.reserve(n);
    back_insert_iterator<VecPair> backInserter(input);
    generate_list_graph(backInserter, n);
    Graph graph(input.begin(), input.end(), n);
    //count # of cycles using vector
    vector<SizeT> cycles;
    for (auto p = input.begin(); p != input.end(); ++p) {
        if (p->first != n) {
            p->first = n;
            SizeT length = 1;
            auto cycleIt = p;
            while ((cycleIt = input.begin() + cycleIt->second) != p) {
                cycleIt->first = n;
                ++length;
            }
            cycles.push_back(length);
        }
    }
    sort(cycles.begin(), cycles.end());

    //count # of cycles using graph data structure
    // assumes that the vertex_descriptor is an integral in [0,n)
    char* visited = new char[n];
    memset(visited, 0, sizeof(char)*n);
    graph_traits<Graph>::vertices_size_type numVertices = num_vertices(graph);
    EXPECT_EQ(n, numVertices);
    graph_traits<Graph>::edges_size_type numEdges = 0;
    vector<graph_traits<Graph>::vertices_size_type> graphCycles;
    auto vRange = vertices(graph);
    for (graph_traits<Graph>::vertex_iterator vIt = vRange.first; vIt != vRange.second; ++vIt) {
        if (!visited[*vIt]) {
            visited[*vIt] = true;
            graph_traits<Graph>::degree_size_type degree = out_degree(*vIt, graph);
            EXPECT_EQ(1, degree);
            numEdges += degree;
            graph_traits<Graph>::vertices_size_type length = 1;
            auto outRange = out_edges(*vIt, graph);
            graph_traits<Graph>::vertex_descriptor curVertex;
            while ( (curVertex = target(*outRange.first, graph)) != *vIt) {
                ++length;
                visited[curVertex] = true;
                outRange = out_edges(curVertex, graph);
                degree = out_degree(curVertex, graph);
                EXPECT_EQ(1, degree);
                numEdges += degree;
            }
            graphCycles.push_back(length);
        }
    }
    delete[] visited;
    sort(graphCycles.begin(), graphCycles.end());
    EXPECT_EQ(cycles.size(), graphCycles.size());
    for (SizeT i = 0; i < cycles.size(); ++i)
        EXPECT_EQ(cycles[i], graphCycles[i]);
    assert(numEdges == n);
    EXPECT_EQ(n, num_edges(graph));
};

int main(int argc, char **argv) {
    ::testing::InitGoogleTest(&argc, argv);
    return RUN_ALL_TESTS();
}


#include <ndt_calibration/ndt_calib.h>

#include <ndt_map/ndt_map.h>
#include <ndt_map/ndt_cell.h>
#include <ndt_map/pointcloud_utils.h>
#include <tf_conversions/tf_eigen.h>
#include <eigen_conversions/eigen_msg.h>
#include <cstdio>
#include <Eigen/Eigen>
#include <Eigen/Geometry>
#include <fstream>
#include <stdio.h>
#include <stdlib.h>
#include <dirent.h>
#include <algorithm>

#include <ros/ros.h>

#include <boost/program_options.hpp>

namespace po = boost::program_options;
using namespace std;

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

  po::options_description desc("Allowed options");
  desc.add_options()
    ("help", "produce help message")
    ;

    po::variables_map vm;
    po::store(po::parse_command_line(argc, argv, desc), vm);
    po::notify(vm);
    if (vm.count("help"))
    {
	cout << desc << "\n";
	return 1;
    }

    {
      Eigen::Affine3d T1 = getAsAffine3d(Eigen::Vector3d(1, 0, 0), Eigen::Vector3d(0, 0, 0));
      NDTCalibScan scan(T1, T1, 0.);
      std::cout << "Should give zeros";
      std::cout << "scan.getTs() : " << affine3dToString(scan.getTs()) << std::endl;
    }

    {
      Eigen::Affine3d T1 = getAsAffine3d(Eigen::Vector3d(1, 0, 0), Eigen::Vector3d(0, 0, 0));
      Eigen::Affine3d T2 = getAsAffine3d(Eigen::Vector3d(2, 0, 0), Eigen::Vector3d(0, 0, M_PI/2.));
      NDTCalibScan scan(T1, T2, 0.);
      std::cout << "Should give 1 0 0 0 0 M_PI/2..." << std::endl;
      std::cout << "scan.getTs() : " << affine3dToString(scan.getTs()) << std::endl;
    }

    {
      Eigen::Affine3d T1 = getAsAffine3d(Eigen::Vector3d(1, 0, 0), Eigen::Vector3d(0, 0, 0));
      Eigen::Affine3d T2 = getAsAffine3d(Eigen::Vector3d(2, 0, 0), Eigen::Vector3d(0, 0, M_PI/2.));
      Eigen::Affine3d Ts = getAsAffine3d(Eigen::Vector3d(3.,0, 1.2), Eigen::Vector3d(0,0,0));

      NDTCalibScan scan1(T1, T1*Ts, 0.);
      NDTCalibScan scan2(T2, T2*Ts, 0.);
      NDTCalibScanPair pair;
      pair.first = scan1;
      pair.second = scan2;

      std::cout << "Should give 1 0 0 0 0 M_PI/2..." << std::endl;
      std::cout << "pair.getRelativePose() : " << affine3dToString(pair.getRelativePose()) << std::endl;
      std::cout << "Should give -2 3 0 0 0 M_PI/2..." << std::endl;
      std::cout << "pair.getRelativeEstSensorPose() : " << affine3dToString(pair.getRelativeEstSensorPose()) << std::endl;

      std::cout << "Should give 1 0 0 0 0 M_PI/2..." << std::endl;
      Eigen::Affine3d Ts2 = getAsAffine3d(Eigen::Vector3d(0, 0, 0), Eigen::Vector3d(0, 0, 0));
      std::cout << "pair.getPredictedRelativeEstSensorPose(Ts2) : " << affine3dToString(pair.getPredictedRelativeEstSensorPose(Ts2)) << std::endl;

      std::cout << "Should give -2 3 0 0 0 M_PI/2..." << std::endl;
      std::cout << "pair.getPredictedRelativeEstSensorPose(Ts) : " << affine3dToString(pair.getPredictedRelativeEstSensorPose(Ts)) << std::endl;

      std::cout << "Shold give 0 0 0 0 0 0 0" << ::endl;
      std::cout << "pair.getDifference(Ts) : " << affine3dToString(pair.getDifference(Ts)) << std::endl;
    }

}


#ifndef RLSS_INTERNAL_UTIL_HPP
#define RLSS_INTERNAL_UTIL_HPP

#include <Eigen/Geometry>
#include <boost/functional/hash/hash_fwd.hpp>
#include <memory>
#include <Eigen/Dense>
#include <queue>
#include <functional>
#include <stdexcept>
#include <absl/strings/str_cat.h>
#include <iostream>
#include <fstream>
#include <qp_wrappers/cplex.hpp>
#include <qp_wrappers/gurobi.hpp>
#include <qp_wrappers/qpoases.hpp>
#include <qp_wrappers/osqp.hpp>
#include <splx/curve/PiecewiseCurve.hpp>
#include <lp_wrappers/cplex.hpp>
#include <lp_wrappers/gurobi.hpp>

#define RLSS_HARD_QP_SOLVER CPLEX
#define RLSS_SOFT_QP_SOLVER CPLEX
#define RLSS_SVM_QP_SOLVER qpOASES
#define RLSS_HP_PRUNING_LP_SOLVER GUROBI

namespace rlss {

#ifdef ENABLE_RLSS_DEBUG_MESSAGES
namespace internal {
    // methods for debugging
    template<typename T>
    void debug_message_internal(bool first, const T& message) {
        if(first) {
            std::cout << "[DEBUG] ";
        }
        std::cout << message << std::endl;
    }

    template<typename T, typename... Args>
    void debug_message_internal(bool first, const T& message, const Args&... args) {
        if(first) {
            std::cout << "[DEBUG] ";
        }
        std::cout << message;
        debug_message_internal(false, args...);
    }
} // namespace internal

    template<typename... Args>
    void debug_message(const Args&... args) {
        internal::debug_message_internal(true, args...);
    }
#else
    template<typename... Args>
    void debug_message(const Args&... args) {

    }
#endif

namespace internal {
    namespace debug {
        namespace colors {
            constexpr char RESET[] = "\033[0m";
            constexpr char RED[] = "\033[31m";
            constexpr char GREEN[] = "\033[32m";
        }
    }
}

namespace internal {

template<typename T, unsigned int DIM>
using AlignedBox = Eigen::AlignedBox<T, DIM>;

template<typename T, unsigned int DIM>
using Hyperplane = Eigen::Hyperplane<T, DIM>;

template<typename T, unsigned int DIM>
using VectorDIM = Eigen::Matrix<T, DIM, 1>;

template<typename T, unsigned int DIM>
using StdVectorVectorDIM = std::vector<VectorDIM<T,DIM>,
                            Eigen::aligned_allocator<VectorDIM<T,DIM>>>;

template<typename T>
using Vector = Eigen::Matrix<T, Eigen::Dynamic, 1>;

template<typename T>
using Row = Eigen::Matrix<T, 1, Eigen::Dynamic>;

template<typename T>
using Matrix = Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>;

template<typename T, unsigned int R, unsigned int C>
using MatrixRC = Eigen::Matrix<T, R, C>; // row column

// method to get corner points of the Eigen AlignedBox passed as argument
template<typename T, unsigned int DIM>
StdVectorVectorDIM<T, DIM> cornerPoints(const AlignedBox<T, DIM>& box) {
    StdVectorVectorDIM<T, DIM> pts(1<<DIM);
    for(unsigned int i = 0; i < (1<<DIM); i++) {
        for(unsigned int d = 0; d < DIM; d++) {
            pts[i](d) = (i & (1<<d)) ? box.min()(d) : box.max()(d);
        }
    }
    return pts;
}

// method to return hash value based on vector coordinates passed
template<typename T, unsigned int DIM>
class VectorDIMHasher {
public:
    std::size_t operator()(const VectorDIM<T, DIM>& vec) const {
        std::size_t seed = 0;
        for(unsigned int d = 0; d < DIM; d++) {
            boost::hash_combine(seed, vec(d));
        }
        return seed;
    }
};

// linear interpolation of two vector points. Returns a vector container of
// points based on number of interpolated points passed
template <typename T, unsigned int DIM>
StdVectorVectorDIM<T, DIM> linearInterpolate(
    const VectorDIM<T, DIM>& start,
    const VectorDIM<T, DIM>& end,
    std::size_t num_points
) {

    using StdVectorVectorDIM = StdVectorVectorDIM<T, DIM>;
    using VectorDIM = VectorDIM<T, DIM>;

    if(num_points < 2) {
        throw std::domain_error(
            absl::StrCat
            (
                "linear interpolate can't have less than 2 number of points",
                " given: ",
                num_points
            )
        );
    }

    StdVectorVectorDIM result(num_points);

    VectorDIM step_vec = (end - start) / (num_points - 1);

    result[0] = start;
    for(std::size_t step = 1; step < num_points - 1; step++) {
        result[step] = start + step * step_vec;
    }
    result.back() = end;

    return result;

}

// method to check if the discrete path search algorithm splits the path segments
// to the minimum required number of segments
template <typename T, unsigned int DIM>
StdVectorVectorDIM<T, DIM> bestSplitSegments(
    const StdVectorVectorDIM<T, DIM>& segments,
    std::size_t num_pieces
) {
    // if current number of segments is larger than the number of pieces, throws error
    if(num_pieces + 1 < segments.size()) {
        throw std::domain_error(
            absl::StrCat
            (
                "nothing to split since num_pieces=",
                num_pieces,
                "is less than current number of segments ",
                segments.size() - 1
            )
        );
    }

    using VectorDIM = VectorDIM<T, DIM>;
    using StdVectorVectorDIM = StdVectorVectorDIM<T, DIM>;

    // if segements is currently of size 1, return a vector of size num_pieces + 1 with
    // all of values of that segment
    if(segments.size() == 1) {
        return StdVectorVectorDIM(num_pieces + 1, segments[0]);
    }

    // define segment priority queue element
    struct SegmentPQElem {
        VectorDIM start;
        VectorDIM end;
        T length;
        unsigned int num_pieces;
        std::size_t segment_idx;

        SegmentPQElem(const VectorDIM& s, const VectorDIM& e, std::size_t i)
                :   start(s),
                    end(e),
                    length((s-e).norm()),
                    num_pieces(1),
                    segment_idx(i)
        {

        }

        // operator to compare the average length of the segment
        bool operator<(const SegmentPQElem& rhs) const {
            return this->length / this->num_pieces
                        < rhs.length / rhs.num_pieces;
        }
    };

    std::priority_queue
    <
        SegmentPQElem,
        std::vector<SegmentPQElem>
    > pq;

    // initialize and store segmentPQelements in PQ
    // store current segment point and next segment point and the index of that segment
    for(std::size_t i = 0; i < segments.size()-1; i++) {
        pq.push({segments[i], segments[i+1], i});
    }

    // enters iteration only if there are excess segments
    // remove excess segments, such that the segments vector container size equals to the
    // number of segments to be generated
    for(std::size_t p_count = segments.size() - 1;
            p_count < num_pieces; p_count++) {
        SegmentPQElem t = pq.top();
        pq.pop();
        t.num_pieces++;
        pq.push(t);
    }

    // iteratively process the priority queue to store into the segment pq elements vector
    std::vector<SegmentPQElem> segment_pqelems;
    while(!pq.empty()) {
        segment_pqelems.push_back(pq.top());
        pq.pop();
    }

    // sort the segments based on their segment length from shortest to longest
    std::sort(
            segment_pqelems.begin(),
            segment_pqelems.end(),
            [] (const SegmentPQElem& lhs, const SegmentPQElem& rhs) -> bool {
                return lhs.segment_idx < rhs.segment_idx;
            }
    );

    StdVectorVectorDIM result;

    // interpolate every segment and split them into the number of pieces that segment
    // is to be interpolated, and store the segment points into the vector
    for(const SegmentPQElem& pqelem: segment_pqelems) {
        StdVectorVectorDIM interpolate
            = linearInterpolate<T, DIM>(
                    pqelem.start,
                    pqelem.end,
                    pqelem.num_pieces + 1
        );

        // append to the back of results vector
        result.insert(result.end(), interpolate.begin(), interpolate.end() - 1);
    }

    result.push_back(segments.back());
    return  result;
}

template<typename T, unsigned int DIM>
StdVectorVectorDIM<T, DIM> firstSegmentFix(
    const StdVectorVectorDIM<T, DIM>& segments
) {
    assert(segments.size() > 1);

    StdVectorVectorDIM<T, DIM> result;
    result.push_back(segments[0]);
    result.push_back((segments[0] + segments[1]) / 2);
    for(std::size_t i = 1; i < segments.size(); i++) {
        result.push_back(segments[i]);
    }
    return result;
}
// shift hyperplane hp creating hyperplane shp
// such that whenever the center_of_mass of the robot
// is to the negative side of the shp,
// the collision shape box of the
// robot is to the negative side of the hyperplane hp.
// box should be the bounding box of the collision shape at given
// center_of_mass
template<typename T, unsigned int DIM>
Hyperplane<T, DIM> shiftHyperplane(
    const VectorDIM<T, DIM>& center_of_mass,
    const AlignedBox<T, DIM>& box,
    const Hyperplane<T, DIM>& hp
) {
    using Hyperplane = Hyperplane<T, DIM>;
    using StdVectorVectorDIM = StdVectorVectorDIM<T, DIM>;
    Hyperplane shp {hp.normal(), std::numeric_limits<T>::lowest()};

    StdVectorVectorDIM corner_points = cornerPoints<T, DIM>(box);

    for(const auto& pt : corner_points) {
        shp.offset()
                = std::max(shp.offset(),
                           hp.normal().dot(pt - center_of_mass) + hp.offset()
        );
    }

    return shp;
}

/*
* Buffer 'box' so that when center_of_mass is inside the buffered box
* com_box corresponding to robot with center of mass 'center_of_mass'
* is inside the 'box'
*/
template<typename T, unsigned int DIM>
AlignedBox<T, DIM> bufferAlignedBox(
    const VectorDIM<T, DIM>& center_of_mass,
    const AlignedBox<T, DIM>& com_box,
    const AlignedBox<T, DIM>& box
) {
    return AlignedBox<T, DIM>(box.min() + (center_of_mass - com_box.min()),
                      box.max() + (center_of_mass - com_box.max()));
}

/*
 * Prunes hyperplanes h in hps such that if the space S bounded by the bounding
 * box bbox and the hyperplane h contains the space S' bounded by the bounding
 * box bbox and an hyperplane h' in hps (that is S' is a subset of S).
 */
template<typename T, unsigned int DIM>
std::vector<Hyperplane<T, DIM>> pruneHyperplanes(
        const std::vector<Hyperplane<T,DIM>>& hps,
        const AlignedBox<T, DIM>& bbox) {

    using Hyperplane = Hyperplane<T, DIM>;
    using AlignedBox = AlignedBox<T, DIM>;
    using LP = LPWrappers::Problem<T>;
    using LPEngine = LPWrappers::RLSS_HP_PRUNING_LP_SOLVER::Engine<T>;
    using Vector = typename LPEngine::Vector;

    std::vector<Hyperplane> result = hps;
    LP lp(DIM, 2);
    LPEngine solver;

    for(unsigned int d = 0; d < DIM; d++) {
        lp.set_var_limits(d, bbox.min()(d), bbox.max()(d));
    }

    Vector c(DIM);
    for(unsigned int d = 0; d < DIM; d++) c(d) = 1;
    lp.add_c(c);

    for(std::size_t i = 0; i < result.size(); i++) {
        bool should_remove_i = false;

        lp.set_constraint(1, result[i].normal().transpose(),
                result[i].offset(), LP::infinity);

        for(std::size_t j = 0; j < result.size(); j++) {
            if(i == j) continue;

            lp.set_constraint(0, result[j].normal().transpose(),
                    LP::minus_infinity, result[j].offset());

            Vector soln;
            auto ret = solver.init(lp, soln);
            if(ret != LPWrappers::OptReturnType::Optimal) {
                should_remove_i = true;
                break;
            }
        }

        if(should_remove_i) {
            std::swap(result[i], result.back());
            result.pop_back();
            i--;
        }
    }

    return result;
}

} // namespace internal

// ellipsoid class
template<typename T, unsigned int DIM>
class Ellipsoid {
public:
    using VectorDIM = rlss::internal::VectorDIM<T, DIM>;
    using MatrixDIMDIM = rlss::internal::MatrixRC<T, DIM, DIM>;
    using AlignedBox = rlss::internal::AlignedBox<T, DIM>;
    using Index = Eigen::Index;

    Ellipsoid(const VectorDIM& cnt, const MatrixDIMDIM& m)
        : center(cnt), mtr(m) {
        mtrInvSq = mtr.inverse();
        mtrInvSq = mtrInvSq * mtrInvSq;

    }

    AlignedBox boundingBox() const {
        // https://members.loria.fr/samuel.hornus/ellipsoid-bbox.html
        VectorDIM delta;
        for(Index r = 0; r < DIM; r++) {
            delta(r) = mtr.row(r).norm();
        }

        debug_message(center - delta, center + delta);

        return AlignedBox(center - delta, center + delta);
    }

    bool intersects(const AlignedBox& box) const {
        if(box.contains(this->center)) {
            return true;
        }

        auto crnr = internal::cornerPoints<T, DIM>(box);
        for(const auto& pt: crnr) {
            VectorDIM d = pt - center;
            if(d.transpose() * mtrInvSq * d <= 1) {
                return true;
            }
        }
        return false;
    }

private:
    VectorDIM center;
    // affine transformation matrix that transforms unit sphere
    // at origin to the ellipsoid at origin
    MatrixDIMDIM mtr;
    MatrixDIMDIM mtrInvSq;
};

} // namespace rlss

#endif // RLSS_INTERNAL_UTIL_HPP


//==================================================================================================
/**
  Copyright 2016 NumScale SAS

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

#include <boost/simd/detail/overload.hpp>
#include <boost/simd/meta/as_logical.hpp>

#if !defined(_MM_PCOMCTRL_GE)
#define _MM_PCOMCTRL_GE 3
#define BOOST_SIMD_MISSING_MM_PCOMCTRL_GE
#endif

#if BOOST_HW_SIMD_X86_AMD_XOP
namespace boost { namespace simd { namespace ext
{
  namespace bd = boost::dispatch;
  namespace bs = boost::simd;

  BOOST_DISPATCH_OVERLOAD ( is_greater_equal_
                          , (typename A0)
                          , bs::avx_
                          , bs::pack_<bd::int8_<A0>, bs::sse_>
                          , bs::pack_<bd::int8_<A0>, bs::sse_>
                          )
  {
    BOOST_FORCEINLINE
    bs::as_logical_t<A0> operator()(const A0& a0, const A0& a1) const BOOST_NOEXCEPT
    {
      #if defined(__clang__)
      return _mm_com_epi8(a0,a1,_MM_PCOMCTRL_GE);
      #else
      return _mm_comge_epi8(a0,a1);
      #endif
    }
  };

  BOOST_DISPATCH_OVERLOAD ( is_greater_equal_
                          , (typename A0)
                          , bs::avx_
                          , bs::pack_<bd::int16_<A0>, bs::sse_>
                          , bs::pack_<bd::int16_<A0>, bs::sse_>
                          )
  {
    BOOST_FORCEINLINE
    bs::as_logical_t<A0> operator()(const A0& a0, const A0& a1) const BOOST_NOEXCEPT
    {
      #if defined(__clang__)
      return _mm_com_epi16(a0,a1,_MM_PCOMCTRL_GE);
      #else
      return _mm_comge_epi16(a0,a1);
      #endif
    }
  };

  BOOST_DISPATCH_OVERLOAD ( is_greater_equal_
                          , (typename A0)
                          , bs::avx_
                          , bs::pack_<bd::int32_<A0>, bs::sse_>
                          , bs::pack_<bd::int32_<A0>, bs::sse_>
                          )
  {
    BOOST_FORCEINLINE
    bs::as_logical_t<A0> operator()(const A0& a0, const A0& a1) const BOOST_NOEXCEPT
    {
      #if defined(__clang__)
      return _mm_com_epi32(a0,a1,_MM_PCOMCTRL_GE);
      #else
      return _mm_comge_epi32(a0,a1);
      #endif
    }
  };

  BOOST_DISPATCH_OVERLOAD ( is_greater_equal_
                          , (typename A0)
                          , bs::avx_
                          , bs::pack_<bd::int64_<A0>, bs::sse_>
                          , bs::pack_<bd::int64_<A0>, bs::sse_>
                          )
  {
    BOOST_FORCEINLINE
    bs::as_logical_t<A0> operator()(const A0& a0, const A0& a1) const BOOST_NOEXCEPT
    {
      #if defined(__clang__)
      return _mm_com_epi64(a0,a1,_MM_PCOMCTRL_GE);
      #else
      return _mm_comge_epi64(a0,a1);
      #endif
    }
  };

  BOOST_DISPATCH_OVERLOAD ( is_greater_equal_
                          , (typename A0)
                          , bs::avx_
                          , bs::pack_<bd::uint8_<A0>, bs::sse_>
                          , bs::pack_<bd::uint8_<A0>, bs::sse_>
                          )
  {
    BOOST_FORCEINLINE
    bs::as_logical_t<A0> operator()(const A0& a0, const A0& a1) const BOOST_NOEXCEPT
    {
      #if defined(__clang__)
      return _mm_com_epu8(a0,a1,_MM_PCOMCTRL_GE);
      #else
      return _mm_comge_epu8(a0,a1);
      #endif
    }
  };

  BOOST_DISPATCH_OVERLOAD ( is_greater_equal_
                          , (typename A0)
                          , bs::avx_
                          , bs::pack_<bd::uint16_<A0>, bs::sse_>
                          , bs::pack_<bd::uint16_<A0>, bs::sse_>
                          )
  {
    BOOST_FORCEINLINE
    bs::as_logical_t<A0> operator()(const A0& a0, const A0& a1) const BOOST_NOEXCEPT
    {
      #if defined(__clang__)
      return _mm_com_epu16(a0,a1,_MM_PCOMCTRL_GE);
      #else
      return _mm_comge_epu16(a0,a1);
      #endif
    }
  };

  BOOST_DISPATCH_OVERLOAD ( is_greater_equal_
                          , (typename A0)
                          , bs::avx_
                          , bs::pack_<bd::uint32_<A0>, bs::sse_>
                          , bs::pack_<bd::uint32_<A0>, bs::sse_>
                          )
  {
    BOOST_FORCEINLINE
    bs::as_logical_t<A0> operator()(const A0& a0, const A0& a1) const BOOST_NOEXCEPT
    {
      #if defined(__clang__)
      return _mm_com_epu32(a0,a1,_MM_PCOMCTRL_GE);
      #else
      return _mm_comge_epu32(a0,a1);
      #endif
    }
  };

  BOOST_DISPATCH_OVERLOAD ( is_greater_equal_
                          , (typename A0)
                          , bs::avx_
                          , bs::pack_<bd::uint64_<A0>, bs::sse_>
                          , bs::pack_<bd::uint64_<A0>, bs::sse_>
                          )
  {
    BOOST_FORCEINLINE
    bs::as_logical_t<A0> operator()(const A0& a0, const A0& a1) const BOOST_NOEXCEPT
    {
      #if defined(__clang__)
      return _mm_com_epu64(a0,a1,_MM_PCOMCTRL_GE);
      #else
      return _mm_comge_epu64(a0,a1);
      #endif
    }
  };
} } }
#endif

#if defined(BOOST_SIMD_MISSING_MM_PCOMCTRL_GE)
#undef _MM_PCOMCTRL_GE
#undef BOOST_SIMD_MISSING_MM_PCOMCTRL_GE
#endif

#endif


#include "terminal/TerminalUI.h"

#include "terminal/stdout_helper.h"
#include "types/Units.h"

#include <algorithm>
#include <array>
#include <cmath>
#include <iomanip>
#include <iostream>
#include <sstream>

#include <boost/format.hpp>

using namespace std::chrono;

const static std::string ForegroundColorWhite = "\u001b[37m";

static std::string
pretty_print_large_number(double large_number)
{
  constexpr std::array<const char*, 5> suffixes = { "", "K", "M", "B", "T" };

  const auto order_of_magnitude = [=](double val) {
    double cur_val = val;
    for (size_t order = 0; order < 4; ++order) {
      if (cur_val < 1000)
        return order;
      cur_val = std::round(cur_val / 1000);
    }
    return size_t{ 4 };
  }(large_number);

  const auto suffix = suffixes[order_of_magnitude];
  const auto number_trimmed = large_number / std::pow(1000, order_of_magnitude);

  std::stringstream ss;
  ss << std::fixed << std::setprecision(1) << number_trimmed << suffix;
  return ss.str();
}

static std::string
format_progress_counter(const ProgressCounter_t& progress_counter)
{
  std::stringstream ss;
  std::visit(
    [&ss](const auto& counter) {
      const auto cur_progress = counter.get_current_progress();
      const auto max_progress = counter.get_max_progress();
      const auto progress_percentage = 100 * (static_cast<double>(cur_progress) / max_progress);

      ss << " " << std::fixed << std::setprecision(2) << progress_percentage << "% ["
         << pretty_print_large_number(cur_progress) << "/"
         << pretty_print_large_number(max_progress) << "]";
    },
    progress_counter);
  return ss.str();
}

/**
 * Returns a nice glyph for the given progress value in [0;1]
 */
static std::string
get_progress_glyph(float progress)
{
  constexpr static std::array<const char*, 9> Glyphs = { "\u2588", "\u2589", "\u258A",
                                                         "\u258B", "\u258C", "\u258D",
                                                         "\u258E", "\u258F", " " };
  const auto step = std::max(0, std::min(8, static_cast<int>((1.f - progress) * 8)));
  return Glyphs[step];
}

/**
 * Format a progress name (e.g. 'loading') into a right-justified format
 * with a colon at the end (e.g. 'loading    :')
 */
static std::string
format_progress_name(const std::string& name, size_t max_name_length)
{
  std::stringstream ss;
  ss << name;
  for (size_t fill = 0; fill < (max_name_length - name.size()); ++fill) {
    ss << ' ';
  }
  ss << ':';
  return ss.str();
}

static std::string
format_time_as_hh_mm_ss(std::chrono::seconds seconds)
{
  const auto hours = seconds.count() / 3600;
  const auto remaining_minutes = (seconds.count() / 60) % 60;
  const auto remaining_seconds = (seconds.count() % 60);

  if (hours < 100) {
    return (boost::format("%02i:%02i:%02i") % hours % remaining_minutes % remaining_seconds).str();
  }

  return (boost::format("%i:%02i:%02i") % hours % remaining_minutes % remaining_seconds).str();
}

ProgressReporter&
UIState::get_progress_reporter()
{
  return _progress_reporter;
}

TerminalUIElement::~TerminalUIElement() {}

TerminalLayoutType
TerminalUIElement::get_layout_type() const
{
  return _layout_type;
}

TerminalLabel::TerminalLabel()
{
  _layout_type = TerminalLayoutType::FixedWidth;
}

TerminalLabel::~TerminalLabel() {}

void
TerminalLabel::render(std::ostream& stream) const
{
  if (util::terminal_is_tty()) {
    stream << _color;
  }
  stream << _content;
}

const std::string&
TerminalLabel::get_content() const
{
  return _content;
}

const std::string&
TerminalLabel::get_color() const
{
  return _color;
}

void
TerminalLabel::set_content(const std::string& content)
{
  _content = content;
}

void
TerminalLabel::set_color(const std::string& color)
{
  _color = color;
}

TerminalMultilineLabel::TerminalMultilineLabel()
  : _line_width(80)
{}

TerminalMultilineLabel::~TerminalMultilineLabel() {}

void
TerminalMultilineLabel::render(std::ostream& stream) const
{

  const auto chunk_string = [](const std::string& str,
                               uint32_t chunk_length) -> std::vector<std::string> {
    const auto num_chunks =
      static_cast<size_t>(std::ceil(str.size() / static_cast<double>(chunk_length)));
    std::vector<std::string> chunks;
    chunks.reserve(num_chunks);
    for (size_t chunk = 0; chunk < num_chunks; ++chunk) {
      const auto start_iter = str.begin() + (chunk * chunk_length);
      const auto end_iter = ((chunk + 1) * chunk_length >= str.size())
                              ? str.end()
                              : str.begin() + ((chunk + 1) * chunk_length);
      chunks.emplace_back(start_iter, end_iter);
    }

    return chunks;
  };

  const auto lines = chunk_string(get_content(), _line_width);

  for (auto& line : lines) {
    if (util::terminal_is_tty()) {
      stream << get_color();
    }
    stream << line << "\n";
  }
}

void
TerminalMultilineLabel::set_line_width(uint32_t line_width)
{
  _line_width = line_width;
}

TerminalProgressBar::TerminalProgressBar()
  : _progress(0)
  , _allowed_width(0)
{
  _layout_type = TerminalLayoutType::Flexible;
}

TerminalProgressBar::~TerminalProgressBar() {}

void
TerminalProgressBar::render(std::ostream& stream) const
{
  if (_allowed_width < 3)
    return;

  const auto steps = _allowed_width - 2;
  const auto enabled_steps = steps * _progress;

  // TODO Colors

  stream << '[';
  for (uint32_t idx = 0; idx < steps; ++idx) {
    const auto glyph = get_progress_glyph(enabled_steps - static_cast<float>(idx));
    stream << glyph;
  }
  stream << ']';
}

float
TerminalProgressBar::get_progress() const
{
  return _progress;
}

void
TerminalProgressBar::set_progress(float progress)
{
  _progress = progress;
}

void
TerminalProgressBar::set_allowed_width(uint32_t allowed_width)
{
  _allowed_width = allowed_width;
}

TerminalUI::TerminalUI(UIState* state)
  : _state(state)
{
  _redraw_interval = (util::terminal_is_tty() ? TERMINAL_REDRAW_INTERVAL_WITH_TTY
                                              : TERMINAL_REDRAW_INTERVAL_WITHOUT_TTY);
  _t_start = std::chrono::high_resolution_clock::now();
}

TerminalUI::~TerminalUI()
{
  if (!util::terminal_is_tty())
    return;

  // Clear the draw buffer so that potential error messages get printed correctly
  const auto draw_buffer_lines = _ui_elements.size();
  for (size_t idx = 0; idx < draw_buffer_lines; ++idx) {
    std::cout << "\n";
  }
  std::cout << std::flush;
}

void
TerminalUI::redraw()
{
  const auto time_since_last_redraw =
    duration_cast<milliseconds>(high_resolution_clock::now() - _last_redraw_time);
  if (time_since_last_redraw < _redraw_interval)
    return;
  _last_redraw_time = high_resolution_clock::now();

  // TODO Don't rebuild the UI completely, instead just update what has changed
  if (util::terminal_is_tty()) {
    rebuild_progress_ui_with_tty();
  } else {
    rebuild_progress_ui_without_tty();
  }

  if (_ui_elements.empty())
    return;

  // Redraw

  util::print_lock().lock();

  if (util::terminal_is_tty()) {
    redraw_with_tty();
  } else {
    redraw_without_tty();
  }

  util::print_lock().unlock();
}

void
TerminalUI::redraw_with_tty() const
{
  for (auto& ui_line : _ui_elements) {
    std::cout << "\u001b[2K";
    for (auto& ui_element : ui_line) {
      ui_element->render(std::cout);
    }
    std::cout << "\n";
  }

  // Move cursor to beginning of draw buffer
  const auto ui_height_lines = _ui_elements.size();
  std::cout << "\u001b[1000D"
            << "\u001b[" << ui_height_lines << "A";

  std::cout.flush();
}

void
TerminalUI::redraw_without_tty() const
{
  for (auto& ui_line : _ui_elements) {
    for (auto& ui_element : ui_line) {
      ui_element->render(std::cout);
    }
    std::cout << "\n";
  }
}

void
TerminalUI::rebuild_progress_ui_with_tty()
{
  _ui_elements.clear();

  auto& progress_reporter = _state->get_progress_reporter();
  const auto& progress_counters = progress_reporter.get_progress_counters();

  if (progress_counters.empty())
    return;

  const auto longest_name =
    std::max_element(std::begin(progress_counters),
                     std::end(progress_counters),
                     [](const auto& l, const auto& r) { return l.first.size() < r.first.size(); });
  const auto max_name_length = longest_name->first.size();

  for (auto& kv : progress_reporter.get_progress_counters()) {
    const auto& progress_name = kv.first;
    const auto& progress_counter = *kv.second;
    const auto progress = std::visit(
      [](const auto& counter) -> double {
        const auto cur_progress = counter.get_current_progress();
        const auto max_progress = counter.get_max_progress();
        return static_cast<double>(cur_progress) / static_cast<double>(max_progress);
      },
      progress_counter);

    // Each progress counter has a name, a progress bar and a numerical
    // indicator of the progress

    auto progress_name_label = std::make_unique<TerminalLabel>();
    progress_name_label->set_content(format_progress_name(progress_name, max_name_length));
    progress_name_label->set_color(ForegroundColorWhite);

    auto progress_bar = std::make_unique<TerminalProgressBar>();
    progress_bar->set_progress(static_cast<float>(progress));
    progress_bar->set_allowed_width(40);

    auto progress_detail = std::make_unique<TerminalLabel>();
    progress_detail->set_content(format_progress_counter(progress_counter));
    progress_detail->set_color(ForegroundColorWhite);

    std::vector<std::unique_ptr<TerminalUIElement>> ui_for_progress_counter;
    ui_for_progress_counter.push_back(std::move(progress_name_label));
    ui_for_progress_counter.push_back(std::move(progress_bar));
    ui_for_progress_counter.push_back(std::move(progress_detail));

    _ui_elements.push_back(std::move(ui_for_progress_counter));
  }
}

void
TerminalUI::rebuild_progress_ui_without_tty()
{
  _ui_elements.clear();

  auto& progress_reporter = _state->get_progress_reporter();
  const auto& progress_counters = progress_reporter.get_progress_counters();

  if (progress_counters.empty())
    return;

  std::vector<std::unique_ptr<TerminalUIElement>> ui_elements;

  const auto delta_time = std::chrono::high_resolution_clock::now() - _t_start;
  const auto delta_time_seconds = std::chrono::duration_cast<std::chrono::seconds>(delta_time);

  auto time_label = std::make_unique<TerminalLabel>();
  time_label->set_content(
    (boost::format("[%1%] ") % format_time_as_hh_mm_ss(delta_time_seconds)).str());
  time_label->set_color(ForegroundColorWhite);
  ui_elements.push_back(std::move(time_label));

  for (auto& kv : progress_reporter.get_progress_counters()) {
    const auto& progress_name = kv.first;
    const auto& progress_counter = *kv.second;
    const auto current_progress = std::visit(
      [](const auto& counter) -> double {
        return static_cast<double>(counter.get_current_progress());
      },
      progress_counter);
    const auto max_progress = std::visit(
      [](const auto& counter) -> double { return static_cast<double>(counter.get_max_progress()); },
      progress_counter);

    // Format each progress indicator as: "NAME: PROGRESS / MAX_PROGRESS "

    const auto progress_text = (boost::format("%1%: %2% / %3% ") % progress_name %
                                unit::format_with_metric_prefix(current_progress) %
                                unit::format_with_metric_prefix(max_progress))
                                 .str();

    auto progress_label = std::make_unique<TerminalLabel>();
    progress_label->set_content(progress_text);
    progress_label->set_color(ForegroundColorWhite);

    ui_elements.push_back(std::move(progress_label));
  }

  _ui_elements.push_back(std::move(ui_elements));
}

TerminalUIAsyncRenderer::TerminalUIAsyncRenderer(TerminalUI& ui)
  : _ui(ui)
{
  _do_render = true;
  _render_thread = std::thread([this]() {
    while (_do_render) {
      _ui.redraw();
      std::this_thread::sleep_for(std::chrono::milliseconds(50));
    }
  });
}

TerminalUIAsyncRenderer::~TerminalUIAsyncRenderer()
{
  _do_render = false;
  _render_thread.join();
}


/* ----------------------------------------------------------------------------

 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

 * See LICENSE for the license information

 * -------------------------------------------------------------------------- */

/**
 * @brief wraps GenericProjectionFactor for several values to python
 * @author Ellon Paiva Mendes (LAAS-CNRS)
 **/

#include <boost/python.hpp>

#define NO_IMPORT_ARRAY
#include <numpy_eigen/NumpyEigenConverter.hpp>

#include "gtsam/slam/ProjectionFactor.h"
#include "gtsam/geometry/Pose3.h"
#include "gtsam/geometry/Point3.h"
#include "gtsam/geometry/Cal3_S2.h"

using namespace boost::python;
using namespace gtsam;

using namespace std;

typedef GenericProjectionFactor<Pose3, Point3, Cal3_S2> GenericProjectionFactorCal3_S2;

BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(print_overloads, GenericProjectionFactorCal3_S2::print, 0, 1)
BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(equals_overloads, GenericProjectionFactorCal3_S2::equals, 1, 2)
BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(evaluateError_overloads, GenericProjectionFactorCal3_S2::evaluateError, 2, 4)

void exportGenericProjectionFactor()
{

  class_<GenericProjectionFactorCal3_S2, bases<NonlinearFactor> >("GenericProjectionFactorCal3_S2", init<>())
    .def(init<const Point2 &, SharedNoiseModel, Key, Key, const std::shared_ptr<Cal3_S2> &, optional<Pose3> >())
    .def(init<const Point2 &, SharedNoiseModel, Key, Key, const std::shared_ptr<Cal3_S2> &, bool, bool, optional<Pose3> >())
    .def("print", &GenericProjectionFactorCal3_S2::print, print_overloads(args("s")))
    .def("equals", &GenericProjectionFactorCal3_S2::equals, equals_overloads(args("q","tol")))
    .def("evaluate_error", &GenericProjectionFactorCal3_S2::evaluateError, evaluateError_overloads())
    .def("measured", &GenericProjectionFactorCal3_S2::measured, return_value_policy<copy_const_reference>())
    // TODO(Ellon): Find the right return policy when returning a 'const shared_ptr<...> &'
    // .def("calibration", &GenericProjectionFactorCal3_S2::calibration, return_value_policy<copy_const_reference>())
    .def("verbose_cheirality", &GenericProjectionFactorCal3_S2::verboseCheirality)
    .def("throw_cheirality", &GenericProjectionFactorCal3_S2::throwCheirality)
  ;

}


/* soma_sorting_swc_plugin.cpp
 * This is a test plugin, you can use it as a demo.
 * 2016-1-28 : by Zhi Zhou
 */

#include "v3d_message.h"
#include <vector>
#include "soma_sorting_swc_plugin.h"
#include <iostream>
#include <boost/lexical_cast.hpp>
#include "../../../released_plugins/v3d_plugins/resample_swc/resampling.h"
#include "../../../released_plugins/v3d_plugins/sort_neuron_swc/sort_swc.h"
#include "../../../released_plugins/v3d_plugins/neurontracing_vn2/app2/my_surf_objs.h"

using namespace std;
#define getParent(n,nt) ((nt).listNeuron.at(n).pn<0)?(1000000000):((nt).hashNeuron.value((nt).listNeuron.at(n).pn))
template <class T> T pow2(T a)
{
    return a*a;

}
Q_EXPORT_PLUGIN2(soma_sorting_swc, soma_sorting);

bool export_list2file_retypeRoot(QList<NeuronSWC> & lN, QString fileSaveName, int soma_ID)
{
    QFile file(fileSaveName);
    if (!file.open(QIODevice::WriteOnly|QIODevice::Text))
        return false;
    QTextStream myfile(&file);

    for (V3DLONG i=0;i<lN.size();i++)
    {
        if(i == soma_ID)
            myfile << lN.at(i).n <<" " << "1" << " "<< lN.at(i).x <<" "<<lN.at(i).y << " "<< lN.at(i).z << " "<< lN.at(i).r << " " <<lN.at(i).pn << "\n";
        else
            myfile << lN.at(i).n <<" " << lN.at(i).type << " "<< lN.at(i).x <<" "<<lN.at(i).y << " "<< lN.at(i).z << " "<< lN.at(i).r << " " <<lN.at(i).pn << "\n";
    }
    file.close();
    cout<<"swc file "<<fileSaveName.toStdString()<<" has been generated, size: "<<lN.size()<<endl;
    return true;
};

QStringList soma_sorting::menulist() const
{
	return QStringList()
		<<tr("soma_sorting")
		<<tr("about");
}

QStringList soma_sorting::funclist() const
{
	return QStringList()
		<<tr("soma_sorting")
		<<tr("help");
}

void soma_sorting::domenu(const QString &menu_name, V3DPluginCallback2 &callback, QWidget *parent)
{
	if (menu_name == tr("soma_sorting"))
	{
        v3d_msg("Only works on dofunc for now.");
	}
	else
	{
        v3d_msg(tr("Developed by Zhi Zhou, 2016-1-28"));
	}
}

bool soma_sorting::dofunc(const QString & func_name, const V3DPluginArgList & input, V3DPluginArgList & output, V3DPluginCallback2 & callback,  QWidget * parent)
{
	vector<char*> infiles, inparas, outfiles;
	if(input.size() >= 1) infiles = *((vector<char*> *)input.at(0).p);
	if(input.size() >= 2) inparas = *((vector<char*> *)input.at(1).p);
	if(output.size() >= 1) outfiles = *((vector<char*> *)output.at(0).p);

	if (func_name == tr("soma_sorting"))
	{
        cout<<"This is soma sorting plugin"<<endl;
        if(infiles.size() <2)
        {
            cerr<<"Need  both the gold standard swc file and the input swc file"<<endl;
            return false;
        }

        QString  gsswc_file =  infiles[0];
        QString  inputswc_file = infiles[1];
        if(inputswc_file.isEmpty() || gsswc_file.isEmpty())
        {
            cerr<<"please check both input swc files"<<endl;
            return false;
        }

        if (inparas.size()!=1)
        {
            printf("Please specify only one parameter - the step size for resampling.\n");
            return false;
        }

        double dis_th = atof(inparas.at(0));
        double sort_th = dis_th * 2; //the parameter in sort_neuron_swc plugin, specifying the length threshold to bridge the gap

        QString  outswc_file =  outfiles[0];

        cout<<"gsswc_file = "<<gsswc_file.toStdString().c_str()<<endl;
        cout<<"inswc_file = "<<inputswc_file.toStdString().c_str()<<endl;
        cout<<"distance threshold = "<<dis_th<<endl;
        cout<<"outswc_file = "<<outswc_file.toStdString().c_str()<<endl;

        NeuronTree nt_input_rs = readSWC_file(inputswc_file);
        cout<<" Soma sorting: 1) resample"<<endl;
//        NeuronTree nt_input_rs = resample(nt_input, dis_th);

//        NeuronTree nt_input_rs_sort;
//        if (!SortSWC(nt_input_rs.listNeuron, nt_input_rs_sort.listNeuron ,VOID, sort_th))
//        {
//            v3d_msg("fail to call swc sorting function.",0);
//            return false;
//        }
//        QHash <int, int>  hashNeuron;
//        hashNeuron.clear();
//        //set node
//        NeuronSWC S;
//        QList<NeuronSWC> list_neurons = nt_input_rs_sort.listNeuron;
//        for (V3DLONG i=0;i<list_neurons.size();i++)
//        {
//            NeuronSWC curr = list_neurons.at(i);
//            S.n 	= curr.n;
//            S.type 	= curr.type;
//            S.x 	= curr.x;
//            S.y 	= curr.y;
//            S.z 	= curr.z;
//            S.r 	= curr.r;
//            S.pn 	= curr.pn;
//            hashNeuron.insert(S.n, i);
//        }
//        nt_input_rs_sort.hashNeuron = hashNeuron;

//        NeuronTree nt_gs = readSWC_file(gsswc_file);
        double soma_x, soma_y, soma_z, soma_r;
        QList <ImageMarker> file_inmarkers;
        file_inmarkers = readMarker_file(gsswc_file);
        soma_x = file_inmarkers.at(0).x;
        soma_y = file_inmarkers.at(0).y;
        soma_z = file_inmarkers.at(0).z;
        soma_r = 3;

//        for (V3DLONG i = 0; i < nt_gs.listNeuron.size(); i++)
//        {
//            if(nt_gs.listNeuron[i].pn<0)
//            {
//                soma_x = nt_gs.listNeuron.at(i).x;
//                soma_y = nt_gs.listNeuron.at(i).y;
//                soma_z = nt_gs.listNeuron.at(i).z;
//                soma_r = nt_gs.listNeuron.at(i).r;
//                break;
//            }
//        }

        V3DLONG neuronNum = nt_input_rs.listNeuron.size();
        QVector<QVector<V3DLONG> > children_list = QVector< QVector<V3DLONG> >(neuronNum, QVector<V3DLONG>() );

        for (V3DLONG i = 0; i < neuronNum; i++)
        {
            V3DLONG parent = nt_input_rs.listNeuron[i].pn;
            if (parent < 0)
                continue;
            children_list[nt_input_rs.hashNeuron.value(parent)].push_back(i);
        }

//        QVector<QVector<V3DLONG> > childs;

//        V3DLONG neuronNum = nt.listNeuron.size();
//        childs = QVector< QVector<V3DLONG> >(neuronNum, QVector<V3DLONG>() );
//        V3DLONG *flag = new V3DLONG[neuronNum];

//        for (V3DLONG i=0;i<neuronNum;i++)
//        {
//            flag[i] = 1;
//            V3DLONG par = nt.listNeuron[i].pn;
//            if (par<0) continue;
//            childs[nt.hashNeuron.value(par)].push_back(i);
//        }

//        QList<NeuronSWC> list = nt.listNeuron;
//        for (V3DLONG i=0;i<list.size();i++)
//        {
//            if (childs[i].size()==0)
//            {
//                int parent_tip = getParent(i,nt);
//                if(parent_tip == 1000000000)
//                    flag[i] = -1;
//                else if(getParent(parent_tip,nt) == 1000000000 && childs[parent_tip].size() ==1)
//                {
//                    flag[i] = -1;
//                    flag[parent_tip] = -1;
//                }
//            }
//        }

//        //Prune small segments
//        cout<<" Soma sorting: 2) prune small segments"<<endl;
//        NeuronTree nt_pruned;
//        QList <NeuronSWC> listNeuron;
//        QHash <int, int>  hashNeuron;
//        listNeuron.clear();
//        hashNeuron.clear();
//        //set node
//        NeuronSWC S;
//        for (V3DLONG i=0;i<list.size();i++)
//        {
//            if(flag[i] == 1)
//            {
//                NeuronSWC curr = list.at(i);
//                S.n 	= curr.n;
//                S.type 	= curr.type;
//                S.x 	= curr.x;
//                S.y 	= curr.y;
//                S.z 	= curr.z;
//                S.r 	= curr.r;
//                S.pn 	= curr.pn;
//                listNeuron.append(S);
//                hashNeuron.insert(S.n, listNeuron.size()-1);
//            }
//        }
//        nt_pruned.n = -1;
//        nt_pruned.on = true;
//        nt_pruned.listNeuron = listNeuron;
//        nt_pruned.hashNeuron = hashNeuron;
//        if(flag) {delete[] flag; flag = 0;}

//        QVector<QVector<V3DLONG> > children_list;

//        V3DLONG neuronNum_prunned = nt_pruned.listNeuron.size();
//        children_list = QVector< QVector<V3DLONG> >(neuronNum, QVector<V3DLONG>() );

//        for (V3DLONG i=0;i<neuronNum_prunned;i++)
//        {
//            V3DLONG par = nt_pruned.listNeuron[i].pn;
//            if (par<0) continue;
//            children_list[nt_pruned.hashNeuron.value(par)].push_back(i);
//        }

        double Dist = 10000000000;
        double Dist_inrange = 10000000000;
        V3DLONG soma_ID = -1;
        V3DLONG dist_ID = -1;
        int child_num = 0;

//        if (list_prunned.size() == 0){
//                cout << "empty tree after prunning!" <<endl;
//                return false;
//        }

        cout<<" Soma sorting: 2) matching soma roots"<<endl;
       // set the distance threshold to searching for matching soma node

        QList<NeuronSWC> list_neurons = nt_input_rs.listNeuron;
        double search_distance_th = soma_r*5 ;
        if (search_distance_th < sort_th *5)
         {
             search_distance_th = sort_th *5;
         }

        for (V3DLONG i=0;i<list_neurons.size();i++)
        {
            NeuronSWC curr = list_neurons.at(i);
            double nodedist = sqrt(pow2(curr.x - soma_x) + pow2(curr.y - soma_y) + pow2(curr.z - soma_z));
            if(nodedist <= search_distance_th && curr.pn <0)
            {
                soma_ID = curr.n;
                child_num = 1;
                break;
            }

            if(nodedist <= search_distance_th  && children_list[i].size() > child_num)
            {
                soma_ID = curr.n;
                child_num = children_list[i].size();
                Dist_inrange = nodedist;
            }

            if(nodedist <= search_distance_th && children_list[i].size() == child_num && nodedist < Dist_inrange)
            {
                soma_ID = curr.n;
                Dist_inrange = nodedist;
            }

            if(nodedist < Dist)
            {
                dist_ID = curr.n;
                Dist = nodedist;
            }
        }

        if(child_num < 1 || soma_ID == -1) soma_ID = dist_ID;

        cout<<" Soma sorting: 3) sort"<<endl;
        QList<NeuronSWC> result;
        if (!SortSWC(nt_input_rs.listNeuron, result ,soma_ID, sort_th))
        {
            v3d_msg("fail to call swc sorting function.",0);
            return false;
        }

        //assign soma type
        cout<<" Soma sorting: 4) assign some type and save the result"<<endl;

        export_list2file_retypeRoot(result,outswc_file,0);
	}
	else if (func_name == tr("help"))
	{
        cout << "This super-plugin (calls resample, and sort plugin)  is used to post-processing auto reconstructions for comparisons.  It will identify the soma for each input reconstruction" <<endl;
        cout << "based on the gold standard SWC file, and resample them according to specified stepsize and sort SWC nodes based on the soma root while bridging" <<endl;
        cout << " all disconneted components when the gap is less then 2*stepsize. The search range for the maching soma  is  max(5* soma_radius, 10* stepsize)." <<endl;
        cout<<"Usage : <vaa3d> -x soma_sorting_swc -f soma_sorting -i <gsswc_file> <inswc_file> -o <outswc_file> -p <step_size>"<<endl;

        cout<<endl;
	}
	else return false;

	return true;
}


//---------------------------------------------------------------------------//
// Copyright (c) 2018-2020 Mikhail Komarov <nemo@nil.foundation>
// Copyright (c) 2021-2022 Aleksei Moskvin <alalmoskvin@gmail.com>
// Copyright (c) 2021 Ilias Khairullin <ilias@nil.foundation>
//
// MIT License
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in all
// copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
//---------------------------------------------------------------------------//

#define BOOST_TEST_MODULE containter_merkletree_test

#include <nil/crypto3/hash/sha2.hpp>
#include <nil/crypto3/hash/md5.hpp>
#include <nil/crypto3/hash/blake2b.hpp>
#include <nil/crypto3/hash/pedersen.hpp>

#include <nil/crypto3/container/merkle/tree.hpp>
#include <nil/crypto3/container/merkle/proof.hpp>

#include <boost/test/unit_test.hpp>
#include <boost/test/data/test_case.hpp>
#include <boost/test/data/monomorphic.hpp>

#include <cstdio>
#include <limits>
#include <type_traits>

using namespace nil::crypto3;
using namespace nil::crypto3::containers;

template<typename ValueType, std::size_t N>
typename std::enable_if<std::is_unsigned<ValueType>::value, std::vector<std::array<ValueType, N>>>::type
    generate_random_data(std::size_t leaf_number) {
    std::vector<std::array<ValueType, N>> v;
    for (std::size_t i = 0; i < leaf_number; ++i) {
        std::array<ValueType, N> leaf {};
        std::generate(std::begin(leaf), std::end(leaf),
                      [&]() { return std::rand() % (std::numeric_limits<ValueType>::max() + 1); });
        v.emplace_back(leaf);
    }
    return v;
}

template<typename Hash, size_t Arity, typename ValueType, std::size_t N>
void testing_validate_template_random_data(std::size_t leaf_number) {
    std::array<ValueType, N> data_not_in_tree = {0};
    auto data = generate_random_data<ValueType, N>(leaf_number);
    auto tree = make_merkle_tree<Hash, Arity>(data.begin(), data.end());

    std::size_t proof_idx = std::rand() % leaf_number;
    merkle_proof<Hash, Arity> proof(tree, proof_idx);
    bool good_validate = proof.validate(data[proof_idx]);
    bool wrong_leaf_validate = proof.validate(data[(proof_idx + 1) % leaf_number]);
    bool wrong_data_validate = proof.validate(data_not_in_tree);
    BOOST_CHECK(good_validate);
    BOOST_CHECK(!wrong_leaf_validate);
    BOOST_CHECK(!wrong_data_validate);
}

template<typename Hash, size_t Arity, typename Element>
void testing_validate_template(std::vector<Element> data) {
    std::array<uint8_t, 7> data_not_in_tree = {'\x6d', '\x65', '\x73', '\x73', '\x61', '\x67', '\x65'};
    merkle_tree<Hash, Arity> tree = make_merkle_tree<Hash, Arity>(data.begin(), data.end());
    merkle_proof<Hash, Arity> proof(tree, 0);
    bool good_validate = proof.validate(data[0]);
    bool wrong_leaf_validate = proof.validate(data[1]);
    bool wrong_data_validate = proof.validate(data_not_in_tree);
    BOOST_CHECK(true == good_validate);
    BOOST_CHECK(false == wrong_leaf_validate);
    BOOST_CHECK(false == wrong_data_validate);
}

template<typename Hash, size_t Arity, typename Element>
void testing_hash_template(std::vector<Element> data, std::string result) {
    merkle_tree<Hash, Arity> tree = make_merkle_tree<Hash, Arity>(data.begin(), data.end());
    BOOST_CHECK(result == std::to_string(tree.root()));
}

BOOST_AUTO_TEST_SUITE(containers_merkltree_test)

BOOST_AUTO_TEST_CASE(merkletree_validate_test_1) {
    std::vector<std::array<char, 1>> v = {{'0'}, {'1'}, {'2'}, {'3'}, {'4'}, {'5'}, {'6'}, {'7'}};
    testing_validate_template<hashes::sha2<256>, 2>(v);
    testing_validate_template<hashes::md5, 2>(v);
    testing_validate_template<hashes::blake2b<224>, 2>(v);

    std::size_t leaf_number = 8;
    testing_validate_template_random_data<hashes::sha2<256>, 2, std::uint8_t, 1>(leaf_number);
    testing_validate_template_random_data<hashes::md5, 2, std::uint8_t, 1>(leaf_number);
    testing_validate_template_random_data<hashes::blake2b<224>, 2, std::uint8_t, 1>(leaf_number);
}

BOOST_AUTO_TEST_CASE(merkletree_validate_test_2) {
    std::vector<std::array<char, 1>> v = {{'0'}, {'1'}, {'2'}, {'3'}, {'4'}, {'5'}, {'6'}, {'7'}, {'8'}};
    testing_validate_template<hashes::sha2<256>, 3>(v);
    testing_validate_template<hashes::md5, 3>(v);
    testing_validate_template<hashes::blake2b<224>, 3>(v);

    std::size_t leaf_number = 9;
    testing_validate_template_random_data<hashes::sha2<256>, 3, std::uint8_t, 1>(leaf_number);
    testing_validate_template_random_data<hashes::md5, 3, std::uint8_t, 1>(leaf_number);
    testing_validate_template_random_data<hashes::blake2b<224>, 3, std::uint8_t, 1>(leaf_number);
}

BOOST_AUTO_TEST_CASE(merkletree_validate_test_3) {
    using hash_type = hashes::pedersen<
        hashes::find_group_hash_default_params, hashes::sha2<256>,
        algebra::curves::jubjub::template g1_type<nil::crypto3::algebra::curves::coordinates::affine,
                                                  nil::crypto3::algebra::curves::forms::twisted_edwards>>;
    std::size_t leaf_number = 8;
    testing_validate_template_random_data<hash_type, 2, bool, hash_type::digest_bits>(leaf_number);
}

BOOST_AUTO_TEST_CASE(merkletree_hash_test_1) {
    std::vector<std::array<char, 1>> v = {{'0'}, {'1'}, {'2'}, {'3'}, {'4'}, {'5'}, {'6'}, {'7'}};
    testing_hash_template<hashes::sha2<256>, 2>(v, "3b828c4f4b48c5d4cb5562a474ec9e2fd8d5546fae40e90732ef635892e42720");
    testing_hash_template<hashes::md5, 2>(v, "11ee8b50825ce6f816a1ae06d4aa0045");
    testing_hash_template<hashes::blake2b<224>, 2>(v, "0ed2a2145cae554ca57f08420d6cb58629ca1e89dc92f819c6c1d13d");
}

BOOST_AUTO_TEST_CASE(merkletree_hash_test_2) {
    std::vector<std::array<char, 1>> v = {{'0'}, {'1'}, {'2'}, {'3'}, {'4'}, {'5'}, {'6'}, {'7'}, {'8'}};
    testing_hash_template<hashes::sha2<256>, 3>(v, "6831d4d32538bedaa7a51970ac10474d5884701c840781f0a434e5b6868d4b73");
    testing_hash_template<hashes::md5, 3>(v, "0733c4cd580b1523cfbb9751f42e9420");
    testing_hash_template<hashes::blake2b<224>, 3>(v, "d9d0ff26d10aaac2882c08eb2b55e78690c949d1a73b1cfc0eb322ee");
}

BOOST_AUTO_TEST_SUITE_END()


//
// Copyright (c) 2016-2019 CNRS INRIA
//

#ifndef __pinocchio_joint_revolute_unbounded_hpp__
#define __pinocchio_joint_revolute_unbounded_hpp__

#include "pinocchio/math/fwd.hpp"
#include "pinocchio/math/sincos.hpp"
#include "pinocchio/spatial/inertia.hpp"
#include "pinocchio/multibody/joint/joint-base.hpp"
#include "pinocchio/multibody/joint/joint-revolute.hpp"

namespace pinocchio
{

  template<typename Scalar, int Options, int axis> struct JointRevoluteUnboundedTpl;

  template<typename _Scalar, int _Options, int axis>
  struct traits< JointRevoluteUnboundedTpl<_Scalar,_Options,axis> >
  {
    enum {
      NQ = 2,
      NV = 1
    };
    typedef _Scalar Scalar;
    enum { Options = _Options };
    typedef JointDataRevoluteUnboundedTpl<Scalar,Options,axis> JointDataDerived;
    typedef JointModelRevoluteUnboundedTpl<Scalar,Options,axis> JointModelDerived;
    typedef ConstraintRevoluteTpl<Scalar,Options,axis> Constraint_t;
    typedef TransformRevoluteTpl<Scalar,Options,axis> Transformation_t;
    typedef MotionRevoluteTpl<Scalar,Options,axis> Motion_t;
    typedef MotionZeroTpl<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, int axis>
  struct traits< JointDataRevoluteUnboundedTpl<Scalar,Options,axis> >
  { typedef JointRevoluteUnboundedTpl<Scalar,Options,axis> JointDerived; };

  template<typename Scalar, int Options, int axis>
  struct traits< JointModelRevoluteUnboundedTpl<Scalar,Options,axis> >
  { typedef JointRevoluteUnboundedTpl<Scalar,Options,axis> JointDerived; };

  template<typename _Scalar, int _Options, int axis>
  struct JointDataRevoluteUnboundedTpl : public JointDataBase< JointDataRevoluteUnboundedTpl<_Scalar,_Options,axis> >
  {
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    typedef JointRevoluteUnboundedTpl<_Scalar,_Options,axis> 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;

    JointDataRevoluteUnboundedTpl()
    : M((Scalar)0,(Scalar)1)
    , v((Scalar)0)
    , U(U_t::Zero())
    , Dinv(D_t::Zero())
    , UDinv(UD_t::Zero())
    {}

    static std::string classname()
    {
      return std::string("JointDataRUB") + axisLabel<axis>();
    }
    std::string shortname() const { return classname(); }

  }; // struct JointDataRevoluteUnbounded

  template<typename NewScalar, typename Scalar, int Options, int axis>
  struct CastType< NewScalar, JointModelRevoluteUnboundedTpl<Scalar,Options,axis> >
  {
    typedef JointModelRevoluteUnboundedTpl<NewScalar,Options,axis> type;
  };

  template<typename _Scalar, int _Options, int axis>
  struct JointModelRevoluteUnboundedTpl
  : public JointModelBase< JointModelRevoluteUnboundedTpl<_Scalar,_Options,axis> >
  {
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    typedef JointRevoluteUnboundedTpl<_Scalar,_Options,axis> JointDerived;
    PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(JointDerived);
    typedef JointRevoluteTpl<Scalar,_Options,axis> JointDerivedBase;

    typedef JointModelBase<JointModelRevoluteUnboundedTpl> 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
    {
      typedef typename ConfigVector::Scalar OtherScalar;
      typename ConfigVector::template ConstFixedSegmentReturnType<NQ>::Type
      & q = qs.template segment<NQ> (idx_q());

      const OtherScalar & ca = q(0);
      const OtherScalar & sa = q(1);

      data.M.setValues(sa,ca);
    }

    template<typename ConfigVector, typename TangentVector>
    void calc(JointDataDerived & data,
              const typename Eigen::MatrixBase<ConfigVector> & qs,
              const typename Eigen::MatrixBase<TangentVector> & vs) const
    {
      calc(data,qs.derived());

      data.v.angularRate() = static_cast<Scalar>(vs[idx_v()]);
    }

    template<typename Matrix6Like>
    void calc_aba(JointDataDerived & data, const Eigen::MatrixBase<Matrix6Like> & I, const bool update_I) const
    {
      data.U = I.col(Inertia::ANGULAR + axis);
      data.Dinv[0] = (Scalar)(1)/I(Inertia::ANGULAR + axis,Inertia::ANGULAR + axis);
      data.UDinv.noalias() = data.U * data.Dinv[0];

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

    static std::string classname()
    {
      return std::string("JointModelRUB") + axisLabel<axis>();
    }
    std::string shortname() const { return classname(); }

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

  }; // struct JointModelRevoluteUnboundedTpl

  struct UnboundedRevoluteAffineTransform
  {
    template<typename ConfigVectorIn, typename Scalar, typename ConfigVectorOut>
    static void run(const Eigen::MatrixBase<ConfigVectorIn> & q,
                    const Scalar & scaling,
                    const Scalar & offset,
                    const Eigen::MatrixBase<ConfigVectorOut> & dest)
    {
      EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(ConfigVectorIn,2);
      EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(ConfigVectorOut,2);

      const typename ConfigVectorIn::Scalar & ca = q(0);
      const typename ConfigVectorIn::Scalar & sa = q(1);

      const typename ConfigVectorIn::Scalar & theta = math::atan2(sa,ca);
      const typename ConfigVectorIn::Scalar & theta_transform = scaling * theta + offset;

      ConfigVectorOut & dest_ = PINOCCHIO_EIGEN_CONST_CAST(ConfigVectorOut,dest);
      SINCOS(theta_transform,&dest_.coeffRef(1),&dest_.coeffRef(0));
    }
  };

  template<typename Scalar, int Options, int axis>
  struct ConfigVectorAffineTransform< JointRevoluteUnboundedTpl<Scalar,Options,axis> >
  {
    typedef UnboundedRevoluteAffineTransform Type;
  };

  typedef JointRevoluteUnboundedTpl<double,0,0> JointRUBX;
  typedef JointDataRevoluteUnboundedTpl<double,0,0> JointDataRUBX;
  typedef JointModelRevoluteUnboundedTpl<double,0,0> JointModelRUBX;

  typedef JointRevoluteUnboundedTpl<double,0,1> JointRUBY;
  typedef JointDataRevoluteUnboundedTpl<double,0,1> JointDataRUBY;
  typedef JointModelRevoluteUnboundedTpl<double,0,1> JointModelRUBY;

  typedef JointRevoluteUnboundedTpl<double,0,2> JointRUBZ;
  typedef JointDataRevoluteUnboundedTpl<double,0,2> JointDataRUBZ;
  typedef JointModelRevoluteUnboundedTpl<double,0,2> JointModelRUBZ;

} //namespace pinocchio

#include <boost/type_traits.hpp>

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

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

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

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

#endif // ifndef __pinocchio_joint_revolute_unbounded_hpp__


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

    @copyright 2016 NumScale SAS

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

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

  /*!
    @ingroup group-predicates

    This function object returns @ref True  or @ref False depending on whether x is
    not less or equal to y or is.

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

    @par Note

     Using `is_not_less_equal(x,y)` is similar to: `!(x <= y)`

    @par Example:

      @snippet is_not_less_equal.cpp is_not_less_equal

    @par Possible output:

      @snippet is_not_less_equal.txt is_not_less_equal

  **/
  as_logical_t<Value> is_not_less_equal(Value const& x, Value const &y);
} }
#endif

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

#endif


/*
 Copyright (C) 2016 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.
*/

#include <orea/scenario/scenariogeneratorbuilder.hpp>
#include <orea/scenario/simplescenariofactory.hpp>
#include <ored/utilities/log.hpp>
#include <ored/utilities/parsers.hpp>
#include <ored/utilities/to_string.hpp>

#include <boost/algorithm/string.hpp>
#include <boost/algorithm/string/case_conv.hpp>
#include <boost/algorithm/string/trim.hpp>
#include <boost/lexical_cast.hpp>
#include <boost/tokenizer.hpp>

#include <map>

using namespace ore::data;
using namespace std;

namespace ore {
namespace analytics {

void ScenarioGeneratorData::clear() {
    if (grid_)
        grid_->truncate(0);
}

CrossAssetStateProcess::discretization parseDiscretization(const string& s) {
    static map<string, QuantExt::CrossAssetStateProcess::discretization> m = {
        {"Exact", QuantExt::CrossAssetStateProcess::exact}, {"Euler", QuantExt::CrossAssetStateProcess::euler}};

    auto it = m.find(s);
    if (it != m.end()) {
        return it->second;
    } else {
        QL_FAIL("Cannot convert \"" << s << "\" to QuantExt::CrossAssetStateProcess::discretization");
    }
}

void ScenarioGeneratorData::setGrid(boost::shared_ptr<DateGrid> grid) {
    grid_ = grid;

    std::ostringstream oss;
    if (grid->tenors().size() == 0) {
        oss << "";
    } else {
        oss << grid->tenors()[0];
        for (Size i = 1; i < grid->tenors().size(); i++) {
            oss << ", " << grid->tenors()[i];
        }
    }

    gridString_ = oss.str();
}

void ScenarioGeneratorData::fromXML(XMLNode* root) {
    XMLNode* sim = XMLUtils::locateNode(root, "Simulation");
    XMLNode* node = XMLUtils::getChildNode(sim, "Parameters");
    XMLUtils::checkNode(node, "Parameters");

    std::string discString = XMLUtils::getChildValue(node, "Discretization", true); // mandatory
    discretization_ = parseDiscretization(discString);
    LOG("ScenarioGeneratorData discretization = " << discString);

    std::string calString = XMLUtils::getChildValue(node, "Calendar", true);
    Calendar cal = parseCalendar(calString);

    std::string dcString = XMLUtils::getChildValue(node, "DayCounter", false);
    DayCounter dc = dcString.empty() ? ActualActual() : parseDayCounter(dcString);

    gridString_ = XMLUtils::getChildValue(node, "Grid", true);
    std::vector<std::string> tokens;
    boost::split(tokens, gridString_, boost::is_any_of(","));
    if (tokens.size() <= 2) {
        grid_ = boost::make_shared<DateGrid>(gridString_, cal, dc);
    } else {
        std::vector<Period> gridTenors = XMLUtils::getChildrenValuesAsPeriods(node, "Grid", true);
        grid_ = boost::make_shared<DateGrid>(gridTenors, cal, dc);
    }
    LOG("ScenarioGeneratorData grid points size = " << grid_->size());

    std::string sequenceTypeString = XMLUtils::getChildValue(node, "Sequence", true);
    sequenceType_ = parseSequenceType(sequenceTypeString);
    LOG("ScenarioGeneratorData sequence type = " << sequenceTypeString);

    seed_ = XMLUtils::getChildValueAsInt(node, "Seed", true);
    LOG("ScenarioGeneratorData seed = " << seed_);

    samples_ = XMLUtils::getChildValueAsInt(node, "Samples", true);
    LOG("ScenarioGeneratorData samples = " << samples_);

    // overwrite samples with enviroment variable OVERWRITE_SCENARIOGENERATOR_SAMPLES
    if (auto c = getenv("OVERWRITE_SCENARIOGENERATOR_SAMPLES")) {
        samples_ = std::stol(c);
        LOG("Overwrite samples with " << samples_ << " from enviroment variable OVERWRITE_SCENARIOGENERATOR_SAMPLES");
    }

    if (auto n = XMLUtils::getChildNode(node, "Ordering"))
        ordering_ = parseSobolBrownianGeneratorOrdering(XMLUtils::getNodeValue(n));
    else
        ordering_ = SobolBrownianGenerator::Steps;

    if (auto n = XMLUtils::getChildNode(node, "DirectionIntegers"))
        directionIntegers_ = parseSobolRsgDirectionIntegers(XMLUtils::getNodeValue(n));
    else
        directionIntegers_ = SobolRsg::JoeKuoD7;

    withCloseOutLag_ = false;
    if (XMLUtils::getChildNode(node, "CloseOutLag") != NULL) {
        withCloseOutLag_ = true;
        closeOutLag_ = parsePeriod(XMLUtils::getChildValue(node, "CloseOutLag", true));
        grid_->addCloseOutDates(closeOutLag_);
        LOG("Use lagged close out grid, lag period is " << closeOutLag_);
    }
    withMporStickyDate_ = false;
    if (XMLUtils::getChildNode(node, "MporMode") != NULL) {
        string mporMode = XMLUtils::getChildValue(node, "MporMode", true);
        if (mporMode == "StickyDate") {
            withMporStickyDate_ = true;
            LOG("Use Mpor sticky date mode");
        } else if (mporMode == "ActualDate") {
            withMporStickyDate_ = false;
            LOG("Use Mpor actual date mode");
        } else {
            QL_FAIL("MporMode " << mporMode << " not recognised");
        }
    }

    LOG("ScenarioGeneratorData done.");
}

XMLNode* ScenarioGeneratorData::toXML(XMLDocument& doc) {
    XMLNode* node = doc.allocNode("Simulation");
    XMLNode* pNode = XMLUtils::addChild(doc, node, "Parameters");

    XMLUtils::addChild(doc, pNode, "Discretization", ore::data::to_string((QuantExt::CrossAssetStateProcess::discretization) discretization_));

    if (grid_) {
        XMLUtils::addChild(doc, pNode, "Calendar", grid_->calendar().name());
        XMLUtils::addChild(doc, pNode, "DayCounter", grid_->dayCounter().name());
        if (gridString_ != "") {
            XMLUtils::addChild(doc, node, "Grid", gridString_);
        } else {
            XMLUtils::addGenericChildAsList(doc, node, "Grid", grid_->tenors());
        }
    }

    XMLUtils::addChild(doc, pNode, "Sequence", ore::data::to_string( sequenceType_));
    XMLUtils::addChild(doc, pNode, "Seed", to_string(seed_));
    XMLUtils::addChild(doc, pNode, "Samples", to_string(samples_));

    XMLUtils::addChild(doc, pNode, "Ordering", ore::data::to_string((SobolBrownianGenerator::Ordering) ordering_) );
    XMLUtils::addChild(doc, pNode, "DirectionIntegers", ore::data::to_string(directionIntegers_));

    if (withCloseOutLag_) {
        XMLUtils::addChild(doc, pNode, "CloseOutLag", closeOutLag_);
    }
    if (withMporStickyDate_) {
        XMLUtils::addChild(doc, pNode, "MporMode", "StickyDate");
    } else {
        XMLUtils::addChild(doc, pNode, "MporMode", "ActualDate");
    }

    return node;
}
} // namespace analytics
} // namespace ore


//---------------------------------------------------------------------------//
// Copyright (c) 2015 Muhammad Junaid Muzammil <mjunaidmuzammil@gmail.com>
//
// 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
//
// See http://boostorg.github.com/compute for more information.
//---------------------------------------------------------------------------//

#ifndef BOOST_COMPUTE_RANDOM_THREEFRY_HPP
#define BOOST_COMPUTE_RANDOM_THREEFRY_HPP

#include <algorithm>

#include <boost/compute/types.hpp>
#include <boost/compute/buffer.hpp>
#include <boost/compute/kernel.hpp>
#include <boost/compute/context.hpp>
#include <boost/compute/program.hpp>
#include <boost/compute/command_queue.hpp>
#include <boost/compute/algorithm/transform.hpp>
#include <boost/compute/detail/iterator_range_size.hpp>
#include <boost/compute/utility/program_cache.hpp>
#include <boost/compute/container/vector.hpp>
#include <boost/compute/iterator/discard_iterator.hpp>

namespace boost {
namespace compute {

/// \class threefry_engine
/// \brief Threefry pseudorandom number generator.
template<class T = uint_>
class threefry_engine
{
public:
    static const size_t threads = 1024;
    typedef T result_type;

    /// Creates a new threefry_engine and seeds it with \p value.
    explicit threefry_engine(command_queue &queue)
        : m_context(queue.get_context())
    {
        // setup program
        load_program();
    }

    /// Creates a new threefry_engine object as a copy of \p other.
    threefry_engine(const threefry_engine<T> &other)
        : m_context(other.m_context),
          m_program(other.m_program)
    {
    }

    /// Copies \p other to \c *this.
    threefry_engine<T>& operator=(const threefry_engine<T> &other)
    {
        if(this != &other){
            m_context = other.m_context;
            m_program = other.m_program;
        }

        return *this;
    }

    /// Destroys the threefry_engine object.
    ~threefry_engine()
    {
    }

private:
    /// \internal_
    void load_program()
    {
        boost::shared_ptr<program_cache> cache =
            program_cache::get_global_cache(m_context);
        std::string cache_key =
            std::string("threefry_engine_32x2");

        // Copyright 2010-2012, D. E. Shaw Research.
        // All rights reserved.

        // Redistribution and use in source and binary forms, with or without
        // modification, are permitted provided that the following conditions are
        // met:

        // * Redistributions of source code must retain the above copyright
        //   notice, this list of conditions, and the following disclaimer.

        // * Redistributions in binary form must reproduce the above copyright
        //   notice, this list of conditions, and the following disclaimer in the
        //   documentation and/or other materials provided with the distribution.

        // * Neither the name of D. E. Shaw Research nor the names of its
        //   contributors may be used to endorse or promote products derived from
        //   this software without specific prior written permission.

        // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
        // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
        // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
        // A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
        // OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
        // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
        // LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
        // DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
        // THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
        // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
        // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
        const char source[] =
            "#define THREEFRY2x32_DEFAULT_ROUNDS 20\n"
            "#define SKEIN_KS_PARITY_32 0x1BD11BDA\n"

            "enum r123_enum_threefry32x2 {\n"
            "    R_32x2_0_0=13,\n"
            "    R_32x2_1_0=15,\n"
            "    R_32x2_2_0=26,\n"
            "    R_32x2_3_0= 6,\n"
            "    R_32x2_4_0=17,\n"
            "    R_32x2_5_0=29,\n"
            "    R_32x2_6_0=16,\n"
            "    R_32x2_7_0=24\n"
            "};\n"

            "static uint RotL_32(uint x, uint N)\n"
            "{\n"
            "    return (x << (N & 31)) | (x >> ((32-N) & 31));\n"
            "}\n"

            "struct r123array2x32 {\n"
            "    uint v[2];\n"
            "};\n"
            "typedef struct r123array2x32 threefry2x32_ctr_t;\n"
            "typedef struct r123array2x32 threefry2x32_key_t;\n"

            "threefry2x32_ctr_t threefry2x32_R(unsigned int Nrounds, threefry2x32_ctr_t in, threefry2x32_key_t k)\n"
            "{\n"
            "    threefry2x32_ctr_t X;\n"
            "    uint ks[3];\n"
            "    uint  i; \n"
            "    ks[2] =  SKEIN_KS_PARITY_32;\n"
            "    for (i=0;i < 2; i++) {\n"
            "        ks[i] = k.v[i];\n"
            "        X.v[i]  = in.v[i];\n"
            "        ks[2] ^= k.v[i];\n"
            "    }\n"
            "    X.v[0] += ks[0]; X.v[1] += ks[1];\n"
            "    if(Nrounds>0){  X.v[0] += X.v[1]; X.v[1] = RotL_32(X.v[1],R_32x2_0_0); X.v[1] ^= X.v[0]; }\n"
            "    if(Nrounds>1){  X.v[0] += X.v[1]; X.v[1] = RotL_32(X.v[1],R_32x2_1_0); X.v[1] ^= X.v[0]; }\n"
            "    if(Nrounds>2){  X.v[0] += X.v[1]; X.v[1] = RotL_32(X.v[1],R_32x2_2_0); X.v[1] ^= X.v[0]; }\n"
            "    if(Nrounds>3){  X.v[0] += X.v[1]; X.v[1] = RotL_32(X.v[1],R_32x2_3_0); X.v[1] ^= X.v[0]; }\n"
            "    if(Nrounds>3){\n"
            "        X.v[0] += ks[1]; X.v[1] += ks[2];\n"
            "        X.v[1] += 1;\n"
            "    }\n"
            "    if(Nrounds>4){  X.v[0] += X.v[1]; X.v[1] = RotL_32(X.v[1],R_32x2_4_0); X.v[1] ^= X.v[0]; }\n"
            "    if(Nrounds>5){  X.v[0] += X.v[1]; X.v[1] = RotL_32(X.v[1],R_32x2_5_0); X.v[1] ^= X.v[0]; }\n"
            "    if(Nrounds>6){  X.v[0] += X.v[1]; X.v[1] = RotL_32(X.v[1],R_32x2_6_0); X.v[1] ^= X.v[0]; }\n"
            "    if(Nrounds>7){  X.v[0] += X.v[1]; X.v[1] = RotL_32(X.v[1],R_32x2_7_0); X.v[1] ^= X.v[0]; }\n"
            "    if(Nrounds>7){\n"
            "        X.v[0] += ks[2]; X.v[1] += ks[0];\n"
            "        X.v[1] += 2;\n"
            "    }\n"
            "    if(Nrounds>8){  X.v[0] += X.v[1]; X.v[1] = RotL_32(X.v[1],R_32x2_0_0); X.v[1] ^= X.v[0]; }\n"
            "    if(Nrounds>9){  X.v[0] += X.v[1]; X.v[1] = RotL_32(X.v[1],R_32x2_1_0); X.v[1] ^= X.v[0]; }\n"
            "    if(Nrounds>10){  X.v[0] += X.v[1]; X.v[1] = RotL_32(X.v[1],R_32x2_2_0); X.v[1] ^= X.v[0]; }\n"
            "    if(Nrounds>11){  X.v[0] += X.v[1]; X.v[1] = RotL_32(X.v[1],R_32x2_3_0); X.v[1] ^= X.v[0]; }\n"
            "    if(Nrounds>11){\n"
            "        X.v[0] += ks[0]; X.v[1] += ks[1];\n"
            "        X.v[1] += 3;\n"
            "    }\n"
            "    if(Nrounds>12){  X.v[0] += X.v[1]; X.v[1] = RotL_32(X.v[1],R_32x2_4_0); X.v[1] ^= X.v[0]; }\n"
            "    if(Nrounds>13){  X.v[0] += X.v[1]; X.v[1] = RotL_32(X.v[1],R_32x2_5_0); X.v[1] ^= X.v[0]; }\n"
            "    if(Nrounds>14){  X.v[0] += X.v[1]; X.v[1] = RotL_32(X.v[1],R_32x2_6_0); X.v[1] ^= X.v[0]; }\n"
            "    if(Nrounds>15){  X.v[0] += X.v[1]; X.v[1] = RotL_32(X.v[1],R_32x2_7_0); X.v[1] ^= X.v[0]; }\n"
            "    if(Nrounds>15){\n"
            "        X.v[0] += ks[1]; X.v[1] += ks[2];\n"
            "        X.v[1] += 4;\n"
            "    }\n"
            "    if(Nrounds>16){  X.v[0] += X.v[1]; X.v[1] = RotL_32(X.v[1],R_32x2_0_0); X.v[1] ^= X.v[0]; }\n"
            "    if(Nrounds>17){  X.v[0] += X.v[1]; X.v[1] = RotL_32(X.v[1],R_32x2_1_0); X.v[1] ^= X.v[0]; }\n"
            "    if(Nrounds>18){  X.v[0] += X.v[1]; X.v[1] = RotL_32(X.v[1],R_32x2_2_0); X.v[1] ^= X.v[0]; }\n"
            "    if(Nrounds>19){  X.v[0] += X.v[1]; X.v[1] = RotL_32(X.v[1],R_32x2_3_0); X.v[1] ^= X.v[0]; }\n"
            "    if(Nrounds>19){\n"
            "        X.v[0] += ks[2]; X.v[1] += ks[0];\n"
            "        X.v[1] += 5;\n"
            "    }\n"
            "    if(Nrounds>20){  X.v[0] += X.v[1]; X.v[1] = RotL_32(X.v[1],R_32x2_4_0); X.v[1] ^= X.v[0]; }\n"
            "    if(Nrounds>21){  X.v[0] += X.v[1]; X.v[1] = RotL_32(X.v[1],R_32x2_5_0); X.v[1] ^= X.v[0]; }\n"
            "    if(Nrounds>22){  X.v[0] += X.v[1]; X.v[1] = RotL_32(X.v[1],R_32x2_6_0); X.v[1] ^= X.v[0]; }\n"
            "    if(Nrounds>23){  X.v[0] += X.v[1]; X.v[1] = RotL_32(X.v[1],R_32x2_7_0); X.v[1] ^= X.v[0]; }\n"
            "    if(Nrounds>23){\n"
            "        X.v[0] += ks[0]; X.v[1] += ks[1];\n"
            "        X.v[1] += 6;\n"
            "    }\n"
            "    if(Nrounds>24){  X.v[0] += X.v[1]; X.v[1] = RotL_32(X.v[1],R_32x2_0_0); X.v[1] ^= X.v[0]; }\n"
            "    if(Nrounds>25){  X.v[0] += X.v[1]; X.v[1] = RotL_32(X.v[1],R_32x2_1_0); X.v[1] ^= X.v[0]; }\n"
            "    if(Nrounds>26){  X.v[0] += X.v[1]; X.v[1] = RotL_32(X.v[1],R_32x2_2_0); X.v[1] ^= X.v[0]; }\n"
            "    if(Nrounds>27){  X.v[0] += X.v[1]; X.v[1] = RotL_32(X.v[1],R_32x2_3_0); X.v[1] ^= X.v[0]; }\n"
            "    if(Nrounds>27){\n"
            "        X.v[0] += ks[1]; X.v[1] += ks[2];\n"
            "        X.v[1] += 7;\n"
            "    }\n"
            "    if(Nrounds>28){  X.v[0] += X.v[1]; X.v[1] = RotL_32(X.v[1],R_32x2_4_0); X.v[1] ^= X.v[0]; }\n"
            "    if(Nrounds>29){  X.v[0] += X.v[1]; X.v[1] = RotL_32(X.v[1],R_32x2_5_0); X.v[1] ^= X.v[0]; }\n"
            "    if(Nrounds>30){  X.v[0] += X.v[1]; X.v[1] = RotL_32(X.v[1],R_32x2_6_0); X.v[1] ^= X.v[0]; }\n"
            "    if(Nrounds>31){  X.v[0] += X.v[1]; X.v[1] = RotL_32(X.v[1],R_32x2_7_0); X.v[1] ^= X.v[0]; }\n"
            "    if(Nrounds>31){\n"
            "        X.v[0] += ks[2]; X.v[1] += ks[0];\n"
            "        X.v[1] += 8;\n"
            "    }\n"
            "    return X;\n"
            "}\n"

            "__kernel void generate_rng(__global uint *ctr, __global uint *key, const uint offset) {\n"
            "    threefry2x32_ctr_t in;\n"
            "    threefry2x32_key_t k;\n"
            "    const uint i = get_global_id(0);\n"
            "    in.v[0] = ctr[2 * (offset + i)];\n"
            "    in.v[1] = ctr[2 * (offset + i) + 1];\n"
            "    k.v[0] = key[2 * (offset + i)];\n"
            "    k.v[1] = key[2 * (offset + i) + 1];\n"
            "    in = threefry2x32_R(20, in, k);\n"
            "    ctr[2 * (offset + i)] = in.v[0];\n"
            "    ctr[2 * (offset + i) + 1] = in.v[1];\n"
            "}\n";

        m_program = cache->get_or_build(cache_key, std::string(), source, m_context);
    }

public:

    /// Generates Threefry random numbers using both the counter and key values, and then stores
    /// them to the range [\p first_ctr, \p last_ctr).
    template<class OutputIterator>
    void generate(OutputIterator first_ctr, OutputIterator last_ctr, OutputIterator first_key, OutputIterator last_key, command_queue &queue) {
        const size_t size_ctr = detail::iterator_range_size(first_ctr, last_ctr);
        const size_t size_key = detail::iterator_range_size(first_key, last_key);
        if(!size_ctr || !size_key || (size_ctr != size_key)) {
            return;
        }
        kernel rng_kernel = m_program.create_kernel("generate_rng");

        rng_kernel.set_arg(0, first_ctr.get_buffer());
        rng_kernel.set_arg(1, first_key.get_buffer());
        size_t offset = 0;

        for(;;){
            size_t count = 0;
            size_t size = size_ctr/2;
            if(size > threads){
                count = (std::min)(static_cast<size_t>(threads), size - offset);
            }
            else {
                count = size;
            }
            rng_kernel.set_arg(2, static_cast<const uint_>(offset));
            queue.enqueue_1d_range_kernel(rng_kernel, 0, count, 0);

            offset += count;

            if(offset >= size){
                break;
            }

        }
    }

    template<class OutputIterator>
    void generate(OutputIterator first_ctr, OutputIterator last_ctr, command_queue &queue) {
        const size_t size_ctr = detail::iterator_range_size(first_ctr, last_ctr);
        if(!size_ctr) {
            return;
        }
        boost::compute::vector<uint_> vector_key(size_ctr, m_context);
        vector_key.assign(size_ctr, 0, queue);
        kernel rng_kernel = m_program.create_kernel("generate_rng");

        rng_kernel.set_arg(0, first_ctr.get_buffer());
        rng_kernel.set_arg(1, vector_key);
        size_t offset = 0;

        for(;;){
            size_t count = 0;
            size_t size = size_ctr/2;
            if(size > threads){
                count = (std::min)(static_cast<size_t>(threads), size - offset);
            }
            else {
                count = size;
            }
            rng_kernel.set_arg(2, static_cast<const uint_>(offset));
            queue.enqueue_1d_range_kernel(rng_kernel, 0, count, 0);

            offset += count;

            if(offset >= size){
                break;
            }

        }
    }
private:
    context m_context;
    program m_program;
};

} // end compute namespace
} // end boost namespace

#endif // BOOST_COMPUTE_RANDOM_THREEFRY_HPP