// Boost.Geometry Index
// Additional tests

// Copyright (c) 2011-2013 Adam Wulkiewicz, Lodz, Poland.

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

#include <GL/glut.h>

#include <boost/foreach.hpp>

#include <boost/geometry/index/rtree.hpp>

#include <boost/geometry/geometries/linestring.hpp>
#include <boost/geometry/geometries/ring.hpp>
#include <boost/geometry/geometries/polygon.hpp>
#include <boost/geometry/multi/geometries/multi_polygon.hpp>

#include <boost/geometry/index/detail/rtree/utilities/gl_draw.hpp>
#include <boost/geometry/index/detail/rtree/utilities/print.hpp>
#include <boost/geometry/index/detail/rtree/utilities/are_boxes_ok.hpp>
#include <boost/geometry/index/detail/rtree/utilities/are_levels_ok.hpp>
#include <boost/geometry/index/detail/rtree/utilities/statistics.hpp>

namespace bg = boost::geometry;
namespace bgi = bg::index;

typedef bg::model::point<float, 2, boost::geometry::cs::cartesian> P;
typedef bg::model::box<P> B;
//bgi::rtree<B> t(2, 1);
typedef bg::model::linestring<P> LS;
typedef bg::model::ring<P> R;
typedef bg::model::polygon<P> Poly;
typedef bg::model::multi_polygon<Poly> MPoly;

typedef bgi::rtree<
    B,
    bgi::rstar<4, 2>
> RTree;
RTree t;
std::vector<B> vect;

size_t found_count = 0;
P search_point;
size_t count = 5;
std::vector<B> nearest_boxes;
B search_box;
R search_ring;
Poly search_poly;
MPoly search_multi_poly;
LS search_path;

enum query_mode_type {
    qm_knn, qm_c, qm_d, qm_i, qm_o, qm_w, qm_nc, qm_nd, qm_ni, qm_no, qm_nw, qm_all, qm_ri, qm_pi, qm_mpi, qm_path
} query_mode = qm_knn;

bool search_valid = false;

void query_knn()
{
    float x = ( rand() % 1000 ) / 10.0f;
    float y = ( rand() % 1000 ) / 10.0f;

    search_point = P(x, y);
    nearest_boxes.clear();
    found_count = t.query(
        bgi::nearest(search_point, count),
        std::back_inserter(nearest_boxes)
        );

    if ( found_count > 0 )
    {
        std::cout << "search point: ";
        bgi::detail::utilities::print_indexable(std::cout, search_point);
        std::cout << "\nfound: ";
        for ( size_t i = 0 ; i < nearest_boxes.size() ; ++i )
        {
            bgi::detail::utilities::print_indexable(std::cout, nearest_boxes[i]);
            std::cout << '\n';
        }
    }
    else
        std::cout << "nearest not found\n";
}

void query_path()
{
    float x = ( rand() % 1000 ) / 10.0f;
    float y = ( rand() % 1000 ) / 10.0f;
    float w = 20 + ( rand() % 1000 ) / 100.0f;
    float h = 20 + ( rand() % 1000 ) / 100.0f;

    search_path.resize(10);
    float yy = y-h;
    for ( int i = 0 ; i < 5 ; ++i, yy += h / 2 )
    {
        search_path[2 * i] = P(x-w, yy);
        search_path[2 * i + 1] = P(x+w, yy);
    }

    nearest_boxes.clear();
    found_count = t.query(
        bgi::detail::path<LS>(search_path, count),
        std::back_inserter(nearest_boxes)
        );

    if ( found_count > 0 )
    {
        std::cout << "search path: ";
        BOOST_FOREACH(P const& p, search_path)
            bgi::detail::utilities::print_indexable(std::cout, p);
        std::cout << "\nfound: ";
        for ( size_t i = 0 ; i < nearest_boxes.size() ; ++i )
        {
            bgi::detail::utilities::print_indexable(std::cout, nearest_boxes[i]);
            std::cout << '\n';
        }
    }
    else
        std::cout << "values on path not found\n";
}

template <typename Predicate>
void query()
{
    if ( query_mode != qm_all )
    {
        float x = ( rand() % 1000 ) / 10.0f;
        float y = ( rand() % 1000 ) / 10.0f;
        float w = 10 + ( rand() % 1000 ) / 100.0f;
        float h = 10 + ( rand() % 1000 ) / 100.0f;

        search_box = B(P(x - w, y - h), P(x + w, y + h));
        nearest_boxes.clear();
        found_count = t.query(Predicate(search_box), std::back_inserter(nearest_boxes) );
    }
    else
    {
        search_box = t.bounds();
        nearest_boxes.clear();
        found_count = t.query(Predicate(search_box), std::back_inserter(nearest_boxes) );
    }

    if ( found_count > 0 )
    {
        std::cout << "search box: ";
        bgi::detail::utilities::print_indexable(std::cout, search_box);
        std::cout << "\nfound: ";
        for ( size_t i = 0 ; i < nearest_boxes.size() ; ++i )
        {
            bgi::detail::utilities::print_indexable(std::cout, nearest_boxes[i]);
            std::cout << '\n';
        }
    }
    else
        std::cout << "boxes not found\n";
}

template <typename Predicate>
void query_ring()
{
    float x = ( rand() % 1000 ) / 10.0f;
    float y = ( rand() % 1000 ) / 10.0f;
    float w = 10 + ( rand() % 1000 ) / 100.0f;
    float h = 10 + ( rand() % 1000 ) / 100.0f;

    search_ring.clear();
    search_ring.push_back(P(x - w, y - h));
    search_ring.push_back(P(x - w/2, y - h));
    search_ring.push_back(P(x, y - 3*h/2));
    search_ring.push_back(P(x + w/2, y - h));
    search_ring.push_back(P(x + w, y - h));
    search_ring.push_back(P(x + w, y - h/2));
    search_ring.push_back(P(x + 3*w/2, y));
    search_ring.push_back(P(x + w, y + h/2));
    search_ring.push_back(P(x + w, y + h));
    search_ring.push_back(P(x + w/2, y + h));
    search_ring.push_back(P(x, y + 3*h/2));
    search_ring.push_back(P(x - w/2, y + h));
    search_ring.push_back(P(x - w, y + h));
    search_ring.push_back(P(x - w, y + h/2));
    search_ring.push_back(P(x - 3*w/2, y));
    search_ring.push_back(P(x - w, y - h/2));
    search_ring.push_back(P(x - w, y - h));

    nearest_boxes.clear();
    found_count = t.query(Predicate(search_ring), std::back_inserter(nearest_boxes) );

    if ( found_count > 0 )
    {
        std::cout << "search ring: ";
        BOOST_FOREACH(P const& p, search_ring)
        {
            bgi::detail::utilities::print_indexable(std::cout, p);
            std::cout << ' ';
        }
        std::cout << "\nfound: ";
        for ( size_t i = 0 ; i < nearest_boxes.size() ; ++i )
        {
            bgi::detail::utilities::print_indexable(std::cout, nearest_boxes[i]);
            std::cout << '\n';
        }
    }
    else
        std::cout << "boxes not found\n";
}

template <typename Predicate>
void query_poly()
{
    float x = ( rand() % 1000 ) / 10.0f;
    float y = ( rand() % 1000 ) / 10.0f;
    float w = 10 + ( rand() % 1000 ) / 100.0f;
    float h = 10 + ( rand() % 1000 ) / 100.0f;

    search_poly.clear();
    search_poly.outer().push_back(P(x - w, y - h));
    search_poly.outer().push_back(P(x - w/2, y - h));
    search_poly.outer().push_back(P(x, y - 3*h/2));
    search_poly.outer().push_back(P(x + w/2, y - h));
    search_poly.outer().push_back(P(x + w, y - h));
    search_poly.outer().push_back(P(x + w, y - h/2));
    search_poly.outer().push_back(P(x + 3*w/2, y));
    search_poly.outer().push_back(P(x + w, y + h/2));
    search_poly.outer().push_back(P(x + w, y + h));
    search_poly.outer().push_back(P(x + w/2, y + h));
    search_poly.outer().push_back(P(x, y + 3*h/2));
    search_poly.outer().push_back(P(x - w/2, y + h));
    search_poly.outer().push_back(P(x - w, y + h));
    search_poly.outer().push_back(P(x - w, y + h/2));
    search_poly.outer().push_back(P(x - 3*w/2, y));
    search_poly.outer().push_back(P(x - w, y - h/2));
    search_poly.outer().push_back(P(x - w, y - h));

    search_poly.inners().push_back(Poly::ring_type());
    search_poly.inners()[0].push_back(P(x - w/2, y - h/2));
    search_poly.inners()[0].push_back(P(x + w/2, y - h/2));
    search_poly.inners()[0].push_back(P(x + w/2, y + h/2));
    search_poly.inners()[0].push_back(P(x - w/2, y + h/2));
    search_poly.inners()[0].push_back(P(x - w/2, y - h/2));

    nearest_boxes.clear();
    found_count = t.query(Predicate(search_poly), std::back_inserter(nearest_boxes) );

    if ( found_count > 0 )
    {
        std::cout << "search poly outer: ";
        BOOST_FOREACH(P const& p, search_poly.outer())
        {
            bgi::detail::utilities::print_indexable(std::cout, p);
            std::cout << ' ';
        }
        std::cout << "\nfound: ";
        for ( size_t i = 0 ; i < nearest_boxes.size() ; ++i )
        {
            bgi::detail::utilities::print_indexable(std::cout, nearest_boxes[i]);
            std::cout << '\n';
        }
    }
    else
        std::cout << "boxes not found\n";
}

template <typename Predicate>
void query_multi_poly()
{
    float x = ( rand() % 1000 ) / 10.0f;
    float y = ( rand() % 1000 ) / 10.0f;
    float w = 10 + ( rand() % 1000 ) / 100.0f;
    float h = 10 + ( rand() % 1000 ) / 100.0f;

    search_multi_poly.clear();

    search_multi_poly.push_back(Poly());
    search_multi_poly[0].outer().push_back(P(x - w, y - h));
    search_multi_poly[0].outer().push_back(P(x - w/2, y - h));
    search_multi_poly[0].outer().push_back(P(x, y - 3*h/2));
    search_multi_poly[0].outer().push_back(P(x + w/2, y - h));
    search_multi_poly[0].outer().push_back(P(x + w, y - h));
    search_multi_poly[0].outer().push_back(P(x + w, y - h/2));
    search_multi_poly[0].outer().push_back(P(x + 3*w/2, y));
    search_multi_poly[0].outer().push_back(P(x + w, y + h/2));
    search_multi_poly[0].outer().push_back(P(x + w, y + h));
    search_multi_poly[0].outer().push_back(P(x + w/2, y + h));
    search_multi_poly[0].outer().push_back(P(x, y + 3*h/2));
    search_multi_poly[0].outer().push_back(P(x - w/2, y + h));
    search_multi_poly[0].outer().push_back(P(x - w, y + h));
    search_multi_poly[0].outer().push_back(P(x - w, y + h/2));
    search_multi_poly[0].outer().push_back(P(x - 3*w/2, y));
    search_multi_poly[0].outer().push_back(P(x - w, y - h/2));
    search_multi_poly[0].outer().push_back(P(x - w, y - h));

    search_multi_poly[0].inners().push_back(Poly::ring_type());
    search_multi_poly[0].inners()[0].push_back(P(x - w/2, y - h/2));
    search_multi_poly[0].inners()[0].push_back(P(x + w/2, y - h/2));
    search_multi_poly[0].inners()[0].push_back(P(x + w/2, y + h/2));
    search_multi_poly[0].inners()[0].push_back(P(x - w/2, y + h/2));
    search_multi_poly[0].inners()[0].push_back(P(x - w/2, y - h/2));

    search_multi_poly.push_back(Poly());
    search_multi_poly[1].outer().push_back(P(x - 2*w, y - 2*h));
    search_multi_poly[1].outer().push_back(P(x - 6*w/5, y - 2*h));
    search_multi_poly[1].outer().push_back(P(x - 6*w/5, y - 6*h/5));
    search_multi_poly[1].outer().push_back(P(x - 2*w, y - 6*h/5));
    search_multi_poly[1].outer().push_back(P(x - 2*w, y - 2*h));

    search_multi_poly.push_back(Poly());
    search_multi_poly[2].outer().push_back(P(x + 6*w/5, y + 6*h/5));
    search_multi_poly[2].outer().push_back(P(x + 2*w, y + 6*h/5));
    search_multi_poly[2].outer().push_back(P(x + 2*w, y + 2*h));
    search_multi_poly[2].outer().push_back(P(x + 6*w/5, y + 2*h));
    search_multi_poly[2].outer().push_back(P(x + 6*w/5, y + 6*h/5));

    nearest_boxes.clear();
    found_count = t.query(Predicate(search_multi_poly), std::back_inserter(nearest_boxes) );

    if ( found_count > 0 )
    {
        std::cout << "search multi_poly[0] outer: ";
        BOOST_FOREACH(P const& p, search_multi_poly[0].outer())
        {
            bgi::detail::utilities::print_indexable(std::cout, p);
            std::cout << ' ';
        }
        std::cout << "\nfound: ";
        for ( size_t i = 0 ; i < nearest_boxes.size() ; ++i )
        {
            bgi::detail::utilities::print_indexable(std::cout, nearest_boxes[i]);
            std::cout << '\n';
        }
    }
    else
        std::cout << "boxes not found\n";
}

void search()
{
    if ( query_mode == qm_knn )
        query_knn();
    else if ( query_mode == qm_c )
        query< bgi::detail::covered_by<B> >();
    else if ( query_mode == qm_d )
        query< bgi::detail::disjoint<B> >();
    else if ( query_mode == qm_i )
        query< bgi::detail::intersects<B> >();
    else if ( query_mode == qm_o )
        query< bgi::detail::overlaps<B> >();
    else if ( query_mode == qm_w )
        query< bgi::detail::within<B> >();
    else if ( query_mode == qm_nc )
        query< bgi::detail::not_covered_by<B> >();
    else if ( query_mode == qm_nd )
        query< bgi::detail::not_disjoint<B> >();
    else if ( query_mode == qm_ni )
        query< bgi::detail::not_intersects<B> >();
    else if ( query_mode == qm_no )
        query< bgi::detail::not_overlaps<B> >();
    else if ( query_mode == qm_nw )
        query< bgi::detail::not_within<B> >();
    else if ( query_mode == qm_all )
        query< bgi::detail::intersects<B> >();
    else if ( query_mode == qm_ri )
        query_ring< bgi::detail::intersects<R> >();
    else if ( query_mode == qm_pi )
        query_poly< bgi::detail::intersects<Poly> >();
    else if ( query_mode == qm_mpi )
        query_multi_poly< bgi::detail::intersects<MPoly> >();
    else if ( query_mode == qm_path )
        query_path();

    search_valid = true;
}

void draw_knn_area(float min_distance, float max_distance)
{
    float x = boost::geometry::get<0>(search_point);
    float y = boost::geometry::get<1>(search_point);
    float z = bgi::detail::rtree::utilities::view<RTree>(t).depth();

    // search point
    glBegin(GL_TRIANGLES);
    glVertex3f(x, y, z);
    glVertex3f(x + 1, y, z);
    glVertex3f(x + 1, y + 1, z);
    glEnd();

    // search min circle

    glBegin(GL_LINE_LOOP);
    for(float a = 0 ; a < 3.14158f * 2 ; a += 3.14158f / 180)
        glVertex3f(x + min_distance * ::cos(a), y + min_distance * ::sin(a), z);
    glEnd();

    // search max circle

    glBegin(GL_LINE_LOOP);
    for(float a = 0 ; a < 3.14158f * 2 ; a += 3.14158f / 180)
        glVertex3f(x + max_distance * ::cos(a), y + max_distance * ::sin(a), z);
    glEnd();
}

void draw_path(LS const& ls)
{
    glBegin(GL_LINE_STRIP);

    BOOST_FOREACH(P const& p, ls)
    {
        float x = boost::geometry::get<0>(p);
        float y = boost::geometry::get<1>(p);
        float z = bgi::detail::rtree::utilities::view<RTree>(t).depth();
        glVertex3f(x, y, z);
    }

    glEnd();
}

template <typename Box>
void draw_box(Box const& box)
{
    float x1 = boost::geometry::get<bg::min_corner, 0>(box);
    float y1 = boost::geometry::get<bg::min_corner, 1>(box);
    float x2 = boost::geometry::get<bg::max_corner, 0>(box);
    float y2 = boost::geometry::get<bg::max_corner, 1>(box);
    float z = bgi::detail::rtree::utilities::view<RTree>(t).depth();

    // search box
    glBegin(GL_LINE_LOOP);
        glVertex3f(x1, y1, z);
        glVertex3f(x2, y1, z);
        glVertex3f(x2, y2, z);
        glVertex3f(x1, y2, z);
    glEnd();
}

template <typename Range>
void draw_ring(Range const& range)
{
    float z = bgi::detail::rtree::utilities::view<RTree>(t).depth();

    // search box
    glBegin(GL_LINE_LOOP);

    BOOST_FOREACH(P const& p, range)
    {
        float x = boost::geometry::get<0>(p);
        float y = boost::geometry::get<1>(p);

        glVertex3f(x, y, z);
    }
    glEnd();
}

template <typename Polygon>
void draw_polygon(Polygon const& polygon)
{
    draw_ring(polygon.outer());
    BOOST_FOREACH(Poly::ring_type const& r, polygon.inners())
        draw_ring(r);
}

template <typename MultiPolygon>
void draw_multi_polygon(MultiPolygon const& multi_polygon)
{
    BOOST_FOREACH(Poly const& p, multi_polygon)
        draw_polygon(p);
}

void render_scene(void)
{
    glClear(GL_COLOR_BUFFER_BIT);

    bgi::detail::rtree::utilities::gl_draw(t);

    if ( search_valid )
    {
        glColor3f(1.0f, 0.5f, 0.0f);

        if ( query_mode == qm_knn )
            draw_knn_area(0, 0);
        else if ( query_mode == qm_ri )
            draw_ring(search_ring);
        else if ( query_mode == qm_pi )
            draw_polygon(search_poly);
        else if ( query_mode == qm_mpi )
            draw_multi_polygon(search_multi_poly);
        else if ( query_mode == qm_path )
            draw_path(search_path);
        else
            draw_box(search_box);

        for ( size_t i = 0 ; i < nearest_boxes.size() ; ++i )
            bgi::detail::utilities::gl_draw_indexable(
                nearest_boxes[i],
                bgi::detail::rtree::utilities::view<RTree>(t).depth());
    }

    glFlush();
}

void resize(int w, int h)
{
    if ( h == 0 )
        h = 1;

    //float ratio = float(w) / h;

    glMatrixMode(GL_PROJECTION);
    glLoadIdentity();

    glViewport(0, 0, w, h);

    //gluPerspective(45, ratio, 1, 1000);
    glOrtho(-150, 150, -150, 150, -150, 150);
    glMatrixMode(GL_MODELVIEW);
    glLoadIdentity();
    /*gluLookAt(
        120.0f, 120.0f, 120.0f,
        50.0f, 50.0f, -1.0f,
        0.0f, 1.0f, 0.0f);*/
    gluLookAt(
        50.0f, 50.0f, 75.0f,
        50.0f, 50.0f, -1.0f,
        0.0f, 1.0f, 0.0f);

    glClearColor(1.0f, 1.0f, 1.0f, 1.0f);
    glLineWidth(1.5f);

    srand(1);
}

void mouse(int button, int state, int /*x*/, int /*y*/)
{
    if ( button == GLUT_LEFT_BUTTON && state == GLUT_DOWN )
    {
        float x = ( rand() % 100 );
        float y = ( rand() % 100 );
        float w = ( rand() % 2 ) + 1;
        float h = ( rand() % 2 ) + 1;

        B b(P(x - w, y - h),P(x + w, y + h));

        boost::geometry::index::insert(t, b);
        vect.push_back(b);

        std::cout << "inserted: ";
        bgi::detail::utilities::print_indexable(std::cout, b);
        std::cout << '\n';

        std::cout << ( bgi::detail::rtree::utilities::are_boxes_ok(t) ? "boxes OK\n" : "WRONG BOXES!\n" );
        std::cout << ( bgi::detail::rtree::utilities::are_levels_ok(t) ? "levels OK\n" : "WRONG LEVELS!\n" );
        std::cout << "\n";

        search_valid = false;
    }
    else if ( button == GLUT_RIGHT_BUTTON && state == GLUT_DOWN )
    {
        if ( vect.empty() )
            return;

        size_t i = rand() % vect.size();
        B b = vect[i];

        bgi::remove(t, b);
        vect.erase(vect.begin() + i);

        std::cout << "removed: ";
        bgi::detail::utilities::print_indexable(std::cout, b);
        std::cout << '\n';

        std::cout << ( bgi::detail::rtree::utilities::are_boxes_ok(t) ? "boxes OK\n" : "WRONG BOXES!\n" );
        std::cout << ( bgi::detail::rtree::utilities::are_levels_ok(t) ? "levels OK\n" : "WRONG LEVELS!\n" );
        std::cout << "\n";

        search_valid = false;
    }
    else if ( button == GLUT_MIDDLE_BUTTON && state == GLUT_DOWN )
    {
        search();
    }

    glutPostRedisplay();
}

std::string current_line;

void keyboard(unsigned char key, int /*x*/, int /*y*/)
{
    if ( key == '\r' || key == '\n' )
    {
        if ( current_line == "t" )
        {
            std::cout << "\n";
            bgi::detail::rtree::utilities::print(std::cout, t);
            std::cout << "\n";
        }
        else if ( current_line == "rand" )
        {
            for ( size_t i = 0 ; i < 35 ; ++i )
            {
                float x = ( rand() % 100 );
                float y = ( rand() % 100 );
                float w = ( rand() % 2 ) + 1;
                float h = ( rand() % 2 ) + 1;

                B b(P(x - w, y - h),P(x + w, y + h));

                boost::geometry::index::insert(t, b);
                vect.push_back(b);

                std::cout << "inserted: ";
                bgi::detail::utilities::print_indexable(std::cout, b);
                std::cout << '\n';
            }

            std::cout << ( bgi::detail::rtree::utilities::are_boxes_ok(t) ? "boxes OK\n" : "WRONG BOXES!\n" );
            std::cout << ( bgi::detail::rtree::utilities::are_levels_ok(t) ? "levels OK\n" : "WRONG LEVELS!\n" );
            std::cout << "\n";

            search_valid = false;

            glutPostRedisplay();
        }
        else if ( current_line == "bulk" )
        {
            vect.clear();

            for ( size_t i = 0 ; i < 35 ; ++i )
            {
                float x = ( rand() % 100 );
                float y = ( rand() % 100 );
                float w = ( rand() % 2 ) + 1;
                float h = ( rand() % 2 ) + 1;

                B b(P(x - w, y - h),P(x + w, y + h));
                vect.push_back(b);

                std::cout << "inserted: ";
                bgi::detail::utilities::print_indexable(std::cout, b);
                std::cout << '\n';
            }

            RTree t2(vect);
            t = boost::move(t2);

            std::cout << ( bgi::detail::rtree::utilities::are_boxes_ok(t) ? "boxes OK\n" : "WRONG BOXES!\n" );
            std::cout << ( bgi::detail::rtree::utilities::are_levels_ok(t) ? "levels OK\n" : "WRONG LEVELS!\n" );
            std::cout << "\n";

            search_valid = false;

            glutPostRedisplay();
        }
        else
        {
            if ( current_line == "knn" )
                query_mode = qm_knn;
            else if ( current_line == "c" )
                query_mode = qm_c;
            else if ( current_line == "d" )
                query_mode = qm_d;
            else if ( current_line == "i" )
                query_mode = qm_i;
            else if ( current_line == "o" )
                query_mode = qm_o;
            else if ( current_line == "w" )
                query_mode = qm_w;
            else if ( current_line == "nc" )
                query_mode = qm_nc;
            else if ( current_line == "nd" )
                query_mode = qm_nd;
            else if ( current_line == "ni" )
                query_mode = qm_ni;
            else if ( current_line == "no" )
                query_mode = qm_no;
            else if ( current_line == "nw" )
                query_mode = qm_nw;
            else if ( current_line == "all" )
                query_mode = qm_all;
            else if ( current_line == "ri" )
                query_mode = qm_ri;
            else if ( current_line == "pi" )
                query_mode = qm_pi;
            else if ( current_line == "mpi" )
                query_mode = qm_mpi;
            else if ( current_line == "path" )
                query_mode = qm_path;

            search();
            glutPostRedisplay();
        }

        current_line.clear();
        std::cout << '\n';
    }
    else
    {
        current_line += key;
        std::cout << key;
    }
}

int main(int argc, char **argv)
{
    glutInit(&argc, argv);
    glutInitDisplayMode(GLUT_DEPTH | GLUT_SINGLE | GLUT_RGBA);
    glutInitWindowPosition(100,100);
    glutInitWindowSize(600, 600);
    glutCreateWindow("boost::geometry::index::rtree GLUT test");

    glutDisplayFunc(render_scene);
    glutReshapeFunc(resize);
    glutMouseFunc(mouse);
    glutKeyboardFunc(keyboard);

    glutMainLoop();

    return 0;
}


#include <boost/python.hpp>
#include <boost/python/def.hpp>
#include <dials/algorithms/scaling/scaling_helper.h>

namespace dials_scaling { namespace boost_python {

  using namespace boost::python;

  using scitbx::sparse::matrix;

  void export_determine_outlier_indices() {
    def("determine_outlier_indices",
        &determine_outlier_indices,
        (arg("h_index_matrix"), arg("z_scores"), arg("zmax")));
  }

  void export_elementwise_square() {
    def("elementwise_square", &elementwise_square, (arg("m")));
  }

  void export_calc_dIh_by_dpi() {
    def("calc_dIh_by_dpi",
        &calculate_dIh_by_dpi,
        (arg("a"), arg("sumgsq"), arg("h_index_mat"), arg("derivatives")));
  }

  void export_calc_jacobian() {
    def("calc_jacobian",
        &calc_jacobian,
        (arg("derivatives"),
         arg("h_index_mat"),
         arg("Ih"),
         arg("g"),
         arg("dIh"),
         arg("sumgsq")));
  }

  void export_sph_harm_table() {
    def("create_sph_harm_table",
        &create_sph_harm_table,
        (arg("s0_theta_phi"), arg("s1_theta_phi"), arg("lmax")));
  }

  void export_rotate_vectors_about_axis() {
    def("rotate_vectors_about_axis",
        &rotate_vectors_about_axis,
        (arg("rot_axis"), arg("vectors"), arg("angles")));
  }

  void export_calc_theta_phi() {
    def("calc_theta_phi", &calc_theta_phi, (arg("xyz")));
  }

  void export_calc_sigmasq() {
    def("calc_sigmasq", &calc_sigmasq, (arg("jacobian_transpose"), arg("var_cov")));
  }

  void export_row_multiply() {
    def("row_multiply", &row_multiply, (arg("m"), arg("v")));
  }

  void export_limit_outlier_weights() {
    def("limit_outlier_weights",
        &limit_outlier_weights,
        (arg("weights"), arg("h_index_mat")));
  }

  void export_calc_lookup_index() {
    def("calc_lookup_index",
        &calc_lookup_index,
        (arg("thetaphi"), arg("points_per_degree")));
  }

  void export_create_sph_harm_lookup_table() {
    def("create_sph_harm_lookup_table",
        &create_sph_harm_lookup_table,
        (arg("lmax"), arg("points_per_degree")));
  }

  void export_calculate_harmonic_tables_from_selections() {
    def("calculate_harmonic_tables_from_selections",
        &calculate_harmonic_tables_from_selections,
        (arg("s0_selection"), arg("s1_selection"), arg("coefficients_list")));
  }

  void export_gaussian_smoother_first_fixed() {
    class_<GaussianSmootherFirstFixed>("GaussianSmootherFirstFixed", no_init)
      .def(init<vec2<double>, std::size_t>((arg("x_range"), arg("num_intervals"))))
      .def("set_smoothing", &GaussianSmootherFirstFixed::set_smoothing)
      .def("num_values", &GaussianSmootherFirstFixed::num_values)
      .def("num_samples", &GaussianSmootherFirstFixed::num_samples)
      .def("num_average", &GaussianSmootherFirstFixed::num_average)
      .def("sigma", &GaussianSmootherFirstFixed::sigma)
      .def("spacing", &GaussianSmootherFirstFixed::spacing)
      .def("positions", &GaussianSmootherFirstFixed::positions)
      .def("value_weight", &GaussianSmootherFirstFixed::value_weight)
      .def("value_weight_first_fixed",
           &GaussianSmootherFirstFixed::value_weight_first_fixed)
      .def("multi_value_weight", &GaussianSmootherFirstFixed::multi_value_weight)
      .def("multi_value_weight_first_fixed",
           &GaussianSmootherFirstFixed::multi_value_weight_first_fixed);
  }

}}  // namespace dials_scaling::boost_python


// Functions for manipulating circuit values via MNA in Eigen
// Author: Jeff Trull <edaskel@att.net>

/*
Copyright (c) 2014 Jeffrey E. Trull

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.
*/

#include <Eigen/Dense>

// "stamp" functions for adding components to a circuit
template<int sz>
void stamp_r(Eigen::Matrix<double, sz, sz>& G,
             Eigen::Matrix<double, sz, sz> const&,
             int node1, int node2, double r) {
    // You can think of this as KCL at the two nodes the resistor connects
    G(node1, node1) += 1.0/r;
    G(node1, node2) -= 1.0/r;
    G(node2, node2) += 1.0/r;
    G(node2, node1) -= 1.0/r;
}

// ground lumped variant
template<int sz>
void stamp_r(Eigen::Matrix<double, sz, sz>& G,
             Eigen::Matrix<double, sz, sz> const&,
             int node, double r) {
    G(node, node) += 1.0/r;
}

// voltage source inputs and inductors get this treatment:
template<int sz>
void stamp_i(Eigen::Matrix<double, sz, sz>& G,
             Eigen::Matrix<double, sz, sz> const&,
             int node1, int node2, int istate) {
    G(node1, istate) =  1;
    G(istate, node1) = -1;
    G(node2, istate) = -1;
    G(istate, node2) =  1;
}

template<int sz>
void stamp_i(Eigen::Matrix<double, sz, sz>& G,
             Eigen::Matrix<double, sz, sz> const&,
             int node, int istate) {
    G(node, istate) =  1;
    G(istate, node) = -1;
}

template<int sz>
void stamp_c(Eigen::Matrix<double, sz, sz> const&,
             Eigen::Matrix<double, sz, sz>& C,
             int node1, int node2, double c) {
    C(node1, node1) += c;
    C(node1, node2) -= c;
    C(node2, node2) += c;
    C(node2, node1) -= c;
}

template<int sz>
void stamp_c(Eigen::Matrix<double, sz, sz> const&,
             Eigen::Matrix<double, sz, sz>& C,
             int node, double c) {
    C(node, node) += c;  // assumes other terminal is ground
}

template<int sz>
void stamp_l(Eigen::Matrix<double, sz, sz>& G,
             Eigen::Matrix<double, sz, sz>& C,
             int node1, int node2, int istate, double l) {
    C(istate, istate) += l;
    // For inductors we have an extra state that remembers its current
    stamp_i(G, C, node1, node2, istate);
}

template<int sz>
void stamp_l(Eigen::Matrix<double, sz, sz>& G,
             Eigen::Matrix<double, sz, sz>& C,
             int node, int istate, double l) {
    C(istate, istate)  +=  l;
    stamp_i(G, C, node, istate);
}

template<class M>
bool isSingular(const M& m) {
   // A singular matrix has at least one zero eigenvalue -
   // in theory, at least... but due to machine precision we can have "nearly singular"
   // matrices that misbehave.  Comparing rank instead is safer because it uses thresholds
   // for near-zero values.

   assert(m.rows() == m.cols());   // singularity has no meaning for a non-square matrix
   return (m.fullPivLu().rank() != m.rows());

}

// Calculate moments of given system in MNA form
template<typename Float, int nrows, int ncols>
using MatrixVector = std::vector<Eigen::Matrix<Float, nrows, ncols>,
                                 Eigen::aligned_allocator<Eigen::Matrix<Float, nrows, ncols> > >;

template<int icount, int ocount, int scount, typename Float = double>
MatrixVector<Float, ocount, icount>
moments(Eigen::Matrix<Float, scount, scount> const & G,
        Eigen::Matrix<Float, scount, scount> const & C,
        Eigen::Matrix<Float, scount, icount> const & B,
        Eigen::Matrix<Float, scount, ocount> const & L,
        Eigen::Matrix<Float, ocount, icount> const & E,
        size_t count) {
    using namespace Eigen;

    MatrixVector<Float, ocount, icount> result;

    auto G_QR = G.fullPivHouseholderQr();
    Matrix<Float, scount, scount> A = -G_QR.solve(C);
    Matrix<Float, scount, icount> R = G_QR.solve(B);

    result.push_back(L.transpose() * R + E);   // incorporate feedthrough into first moment
    Matrix<Float, scount, scount> AtotheI = A;
    for (size_t i = 1; i < count; ++i) {
        result.push_back(L.transpose() * AtotheI * R);
        AtotheI = A * AtotheI;
    }

    return result;
}

// Implementation of Natarajan regularization
// Each iteration of this process can produce an input derivative term that we subsequently
// absorb into the state variable once the process is complete.  This means potentially
// a series of input derivative coefficients (B's).  We hide that from the users by delegating here:

template<int icount, int ocount, int scount, typename Float = double>
std::tuple<Eigen::Matrix<Float, Eigen::Dynamic, Eigen::Dynamic>,   // G result
           Eigen::Matrix<Float, Eigen::Dynamic, Eigen::Dynamic>,   // C result
           Eigen::Matrix<Float, Eigen::Dynamic, icount>,    // B result
           Eigen::Matrix<Float, Eigen::Dynamic, ocount>,    // D result
           Eigen::Matrix<Float, ocount, icount> >    // E result (feedthrough)
regularize(Eigen::Matrix<Float, scount, scount> const & G,
           Eigen::Matrix<Float, scount, scount> const & C,
           MatrixVector<Float, scount, icount> const & B, // in decreasing order of derived-ness
           Eigen::Matrix<Float, scount, ocount> const & D) {

    // Implements the algorithm in [Natarajan]
    // Circuits, Devices and Systems, IEE Proceedings G, June 1991

    using namespace Eigen;
    typedef Matrix<Float, Dynamic, Dynamic> MatrixD;

    // Step 1: put C into "Row Echelon" form by performing LU factorization
    auto lu = C.fullPivLu();
    auto k = lu.rank();
    if (k == C.rows()) {
        // C is already non-singular
        Matrix<Float, ocount, icount>   E = Matrix<Float, ocount, icount>::Zero();
        return std::make_tuple(G, C, B.back(), D, E);
    }

    MatrixD U = lu.matrixLU().template triangularView<Upper>();
    MatrixD L = lu.matrixLU().template triangularView<UnitLower>();

    // Step 2: "perform the same elementary operations on G and B"
    // given that C = P.inverse() * L * U * Q.inverse()
    // (from source it seems that permutationP/Q is inverse)
    // then to get the new G we reverse those operations:
    auto Cprime = U;   // note we may have small non-zero values in bottom rows, but they will be ignored
    auto P = lu.permutationP();
    auto Q = lu.permutationQ();

    assert(!isSingular(L));
    MatrixD Gprime = L.fullPivLu().solve(P * G * Q);                   // rows and columns
    MatrixVector<Float, scount, icount> Bprime;
    std::transform(B.begin(), B.end(), std::back_inserter(Bprime),
                   [L, P](Matrix<Float, scount, icount> const& b) -> Matrix<Float, scount, icount> {
                       return L.fullPivLu().solve(P * b);              // rows only
                   });

    // The D input is like L in PRIMA but this algorithm uses the transpose
    Matrix<Float, ocount, scount> Dprime = D.transpose() * Q;          // columns only

    // Step 3: "Convert [G21 G22] matrix into row echelon form starting from the last row"
    MatrixD Cnew, Gnew, Dnew;
    MatrixVector<Float, scount, icount> Bnew;

    if (Cprime.rows() == (k+1)) {
        // if G22 is only a single row, there is no point attempting to decompose it
        Cnew = Cprime; Gnew = Gprime; Bnew = Bprime; Dnew = Dprime;
    } else {
        // decompose the bottom rows
        // Upon close review of the first example in the paper, the author is not only
        // converting from the last row, but also *from the last column*, i.e., he
        // performs a standard gaussian elimination on the matrix rotated 180 degrees

        // Plan of attack: reverse G2, perform LU decomposition, reverse result, reverse permutations
        MatrixD G2R = Gprime.bottomRows(C.rows() - k).reverse();

        auto G2R_LU = G2R.fullPivLu();
        MatrixD G2R_U = (G2R_LU.matrixLU().template triangularView<Upper>());

        // Since the order of the rows is irrelevant, I'll perform the decomposition, then
        // combine reversing the rows with the row reordering the LU decomposition produces

        MatrixD exchange_columns = G2R_LU.permutationQ();
        Gnew = Gprime * exchange_columns.reverse();

        // insert already-permuted rows that came from LU, but in reverse order
        Gnew.block(k, 0, Gprime.rows() - k, Gprime.cols()) = G2R_U.reverse();

        // Step 4: "Carry out the same row operations in the B matrix"
        // Note: not necessary to do it for C, because all coefficients are zero in those rows

        // 4.1 reverse the rows in B2
        typedef PermutationMatrix<Dynamic, Dynamic, std::size_t> PermutationD;
        PermutationD reverse_rows;                // order of rows is completely reversed
        reverse_rows.setIdentity(G2R.rows());     // start with null permutation
        for (std::size_t i = 0; i < (G2R.rows() / 2); ++i) {
            reverse_rows.applyTranspositionOnTheRight(i, (G2R.rows()-1) - i);
        }

        // 4.2 extract and apply L operation from reversed G2
        MatrixD G2R_L = G2R_LU.matrixLU().leftCols(G2R.rows()).template triangularView<UnitLower>();
        std::transform(Bprime.begin(), Bprime.end(), std::back_inserter(Bnew),
                       [reverse_rows, k, G2R_L, G2R_LU]
                       (Matrix<Float, scount, icount> const& bp) {
                           MatrixD B2R = reverse_rows * bp.bottomRows(bp.rows() - k);
                           Matrix<Float, scount, icount> bn = bp;
                           bn.block(k, 0, bn.rows() - k, bn.cols()) =
                               reverse_rows.transpose() * G2R_L.fullPivLu().solve(G2R_LU.permutationP() * B2R);
                           return bn;
                       });

        // Step 5: "Interchange the columns in the G, C, and D matrices... such that G22 is non-singular"
        // Since we have done a full pivot factorization of G2 I assume G22 is already non-singular,
        // so the only thing left to do is reorder the C and D matrices according to the G2 factorization
        Cnew = Cprime * exchange_columns.reverse();
        Dnew = Dprime * exchange_columns.reverse();
    }

    // Step 6: compute reduced matrices using equations given in paper
    MatrixD G11 = Gnew.topLeftCorner(k, k);
    MatrixD G12 = Gnew.topRightCorner(k, Gnew.rows() - k);
    MatrixD G21 = Gnew.bottomLeftCorner(Gnew.rows() - k, k);
    MatrixD G22 = Gnew.bottomRightCorner(Gnew.rows() - k, Gnew.rows() - k);
    MatrixD C11 = Cnew.topLeftCorner(k, k);
    MatrixD C12 = Cnew.topRightCorner(k, Cnew.rows() - k);
    MatrixD D01 = Dnew.leftCols(k);
    MatrixD D02 = Dnew.rightCols(Dnew.cols() - k);

    assert(!isSingular(G22));
    auto    G22_LU = G22.fullPivLu();

    MatrixD Gfinal  = G11 - G12 * G22_LU.solve(G21);
    MatrixD Cfinal  = C11 - C12 * G22_LU.solve(G21);
    Matrix<Float, ocount, Dynamic> Dfinal
                    = D01 - D02 * G22_LU.solve(G21);

    Matrix<Float, Dynamic, icount> B02 = Bnew.back().bottomRows(Bnew.back().rows() - k);
    Matrix<Float, ocount, icount> E1
                    =       D02 * G22_LU.solve(B02);

    // reduce the entire series of B's to the new size
    // Performing the same substitution as in Natarajan beginning with eqn [5]
    // but with additional input derivatives present.  Adding B11/B12 multiplying a first
    // derivative of Ws demonstrates that each additional input derivative term contributes:
    // Bn1 - G12 * G22^-1 * Bn2  to its own term, and
    //     - C12 * G22^-1 * Bn2  to the derivative n+1 coefficient,
    // once reduced.
    MatrixVector<Float, Dynamic, icount> Btrans;
    // n+1's first (equation 9d)
    std::transform(Bnew.begin(), Bnew.end(), std::back_inserter(Btrans),
                   [k, G12, C12, G22_LU](Matrix<Float, scount, icount> const& Bn) {
                       Matrix<Float, Dynamic, icount> Bn2 = Bn.bottomRows(Bn.rows() - k);
                       return -C12 * G22_LU.solve(Bn2);
                   });
    Btrans.push_back(Matrix<Float, Dynamic, icount>::Zero(k, icount));  // contribution from n-1 is 0 (nonexistent)

    // n's next, shifted by one (equation 9c)
    std::transform(Bnew.begin(), Bnew.end(), Btrans.begin()+1, Btrans.begin()+1,
                   [k, G12, G22_LU](Matrix<Float, scount, icount> const& Bn,
                                         Matrix<Float, Dynamic, icount> const& Bnm1_contribution)
                   -> Matrix<Float, Dynamic, icount> {  // without explicitly declared return type Eigen
                                                        // will keep references to these locals:
                       Matrix<Float, Dynamic, icount> Bn1 = Bn.topRows(k);
                       Matrix<Float, Dynamic, icount> Bn2 = Bn.bottomRows(Bn.rows() - k);

                       return Bn1 - G12 * G22_LU.solve(Bn2) + Bnm1_contribution;
                   });

    // If Cfinal is singular, we need to repeat this analysis on the new matrices
    if (isSingular(Cfinal)) {
        Matrix<Float, Dynamic, ocount> Dtrans = Dfinal.transpose();   // no implicit conversion on fn tmpl args
        auto recursive_result = regularize<icount, ocount, Dynamic>(Gfinal, Cfinal, Btrans, Dtrans);
        return std::make_tuple(std::get<0>(recursive_result),  // G
                               std::get<1>(recursive_result),  // C
                               std::get<2>(recursive_result),  // B
                               std::get<3>(recursive_result),  // D
                               std::get<4>(recursive_result) + E1);  // combine E
    }

    // We've found a non-singular Cfinal and a set of B's
    // We need to apply a transformation suggested by Chen (TCAD July 2012) to eliminate
    // all input derivative terms.  Chen gives only the simplest case, for B0 * Ws + B1 * Ws' :
    // Br = B0 - Gr * Cr^-1 * B1
    // based on a variable substitution of:
    // Xnew = X - Cr^-1 * B1 * Ws
    // and mentions the rest should be done "recursively".  I believe the general case is:
    // Br = B0 - Gr * Cr^-1 * (B1 - Gr * Cr^-1 *(B2 - ... ))
    Matrix<Float, Dynamic, icount> Bfinal = Matrix<Float, Dynamic, icount>::Zero(k, icount);
    Bfinal = std::accumulate(
        // starting with the first (most derived) coefficient, compute above expression for Br:
        Btrans.begin(), Btrans.end(), Bfinal,
        [Gfinal, Cfinal](Matrix<Float, Dynamic, icount> const& acc,
                         Matrix<Float, Dynamic, icount> const& B) {
            return B - Gfinal * Cfinal.fullPivHouseholderQr().solve(acc);
        });

    // The variable substitution for the 2nd derivative case is:
    // Xnew = X - Cr^-1 * (B2 * Ws' - (Gr * Cr^-1 * B2 - B1) * Ws)
    // Making this substitution in the output equation Y = D * X + E * Ws gives
    // Y = D * Xnew + D * Cr^-1 * (B1 - Gr * Cr^-1 * B2) * Ws + Cr^-1 * B2 * Ws'
    // however, if the Ws' term is nonzero the system is ill-formed:
    if (Btrans.size() >= 3) {
        Matrix<Float, Dynamic, icount> CinvB = Cfinal.fullPivLu().solve(*(Btrans.rbegin()+2));
        assert(CinvB.isZero());
    }

    // now I can calculate the new value for E, which can only be:
    // E = E1 + D * Cr^-1 * B1
    // because, thanks to the assertion, all other terms must be 0
    Matrix<Float, ocount, icount> Efinal = E1 + Dfinal * Cfinal.fullPivHouseholderQr().solve(*(Btrans.rbegin()+1));

    return std::make_tuple(Gfinal, Cfinal, Bfinal,
                           Dfinal.transpose(),  // for PRIMA compatibility
                           Efinal);
}

// user-facing function (only one "B" parameter)
template<int icount, int ocount, int scount, typename Float = double>
std::tuple<Eigen::Matrix<Float, Eigen::Dynamic, Eigen::Dynamic>,   // G result
           Eigen::Matrix<Float, Eigen::Dynamic, Eigen::Dynamic>,   // C result
           Eigen::Matrix<Float, Eigen::Dynamic, icount>,    // B result
           Eigen::Matrix<Float, Eigen::Dynamic, ocount>,    // D result
           Eigen::Matrix<Float, ocount, icount> >    // E result (feedthrough)
regularize(Eigen::Matrix<Float, scount, scount> const & G,
           Eigen::Matrix<Float, scount, scount> const & C,
           Eigen::Matrix<Float, scount, icount> const & B,
           Eigen::Matrix<Float, scount, ocount> const & D) {
    return regularize(G, C, MatrixVector<Float, scount, icount>(1, B), D);
}


#include <iostream>
#include <string>
#include <fstream>
#include <vector>
#include <boost/algorithm/string.hpp>
#include <boost/lexical_cast.hpp>
#include <kmcluster/KMeansCluster.h>

// compile:  g++ testcluster.cpp ../random/rand_isaac.cpp -I../.. -o cluster

using namespace std;
using namespace boost;

int main (int argc, char ** argv)
{
  string fname     = argv[1];
  int    nClusters = atoi(argv[2]);

  rps::KMeansClusterND clusters (nClusters);

  // we support two different file types
  // .txt is just a flat file with one 2D point per line
  ifstream fin (fname.c_str());
  if (!fin)
    {
      cerr << "could not open file: " << fname << endl;
      exit(-1);
    }

  if (boost::ends_with (fname, ".txt"))
    {
      do
        {
          double x,y;
          fin >> x >> y;
          if (!fin.eof ())
            {
              rps::PointND pt(x,y);
              clusters.add (pt);
            }
        }
      while (!fin.eof());
    }

  // we also support a csv format where each line is a
  // labeled points, with the label stored in the first
  // field of the csv
  else if (boost::ends_with (fname, ".csv"))
    {
      //cout << "doing csv\n";
      string line;
      while (!fin.eof())
        {
          getline (fin, line);

          // filer out empty lines
          if (line.size() == 0)
            continue;

          vector<string> fields;
          boost::split (fields, line, boost::is_any_of(","));

          vector<double> pt;
          for (size_t i=1; i<fields.size(); i++)
            pt.push_back (lexical_cast<double>(fields[i]));

          clusters.add (rps::PointND(fields[0], pt));
        }
    }

  //srand ();

  clusters.cluster ();
  //cout << clusters.str () << endl;

  cout << clusters.clusterSets () << endl;
}


/*
 * Copyright (c) 2015-2021 Agalmic Ventures LLC (www.agalmicventures.com)
 *
 * 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.
 */

#include <boost/test/unit_test.hpp>

#include "Werk/Math/Random.hpp"

BOOST_AUTO_TEST_SUITE(RandomTest)

BOOST_AUTO_TEST_CASE(TestBasic)
{
	werk::Random r;
	uint64_t n;

	//Test singleton ranges
	n = r.getUint64(0, 0);
	BOOST_REQUIRE_EQUAL(n, 0);

	n = r.getUint64(253, 253);
	BOOST_REQUIRE_EQUAL(n, 253);

	n = r.getUint64(1000, 1000);
	BOOST_REQUIRE_EQUAL(n, 1000);

	//Test larger ranges
	for (int i = 0; i < 100000; ++i) {
		n = r.getUint64(17, 1017);
		BOOST_REQUIRE_GE(n, 17);
		BOOST_REQUIRE_LE(n, 1017);
	}
}

BOOST_AUTO_TEST_SUITE_END()


//
// Created by lynn on 2020/7/19.
//
//#include <acado/acado_optimal_control.hpp>
#include <acado/acado_toolkit.hpp>
#include "ros/ros.h"
#include "std_msgs/Float32.h"
#include <chrono>
#include "iomanip"
#include <acado/acado_gnuplot.hpp>
#include <geometry_msgs/Point.h>
#include <geometry_msgs/Vector3.h>
#include <geometry_msgs/Quaternion.h>
#include <geometry_msgs/Pose.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/TwistStamped.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <nav_msgs/Odometry.h>
#include <mavros_msgs/State.h>
#include <mavros_msgs/AttitudeTarget.h>
#include <sensor_msgs/Imu.h>
#include <std_msgs/Bool.h>
#include <std_msgs/Float32.h>
#include "offb_posctl/controlstate.h"
#include <fstream>
#include <string>
#include <sstream>
#include <iostream>
#include "FILTER.h"
//#include <Eigen/Eigen>// This step will generate error about eigen ,the reason is not clear by far
#include <Eigen/Geometry>
#include <Eigen/Core>
#include <thread>
#include <math.h>
#include <stdlib.h>
#include <FILTER.h>

using namespace Eigen;
using namespace std;

geometry_msgs::PoseStamped pos_ref;         //无人机参考位置

mavros_msgs::State current_state;           //无人机当前状态(mode arm)
sensor_msgs::Imu   imu_drone;               //读入的无人机的IMU信息 包括姿态角和线加速度

geometry_msgs::PoseStamped  pos_drone;      //读入的无人机当前位置
geometry_msgs::PoseStamped  pos_drone_last; //读入的无人机上一次位置
geometry_msgs::PoseStamped  fused_drone; //融合了vicon数据的无人机位姿，注意是NED坐标下的，需要翻转

geometry_msgs::TwistStamped vel_drone;      //读入的无人机当前速度

geometry_msgs::Vector3 acc_receive;         //读入的无人机线加速度
geometry_msgs::Vector3 angle_receive;       //读入的无人机姿态（欧拉角）
geometry_msgs::Vector3 angle_fromvicon;       //读入的vicon计算的sgement姿态（欧拉角）
geometry_msgs::Vector3 angle_fromvicon_qua;       //读入的vicon计算的sgement姿态（欧拉角）
geometry_msgs::Vector3 angle_fromviconinit;  //读入的vicon计算的sgement initial 姿态（欧拉角）
geometry_msgs::Vector3 fusedangle_receive;

geometry_msgs::Quaternion orientation_target;   //发给无人机的姿态指令

geometry_msgs::Vector3 angle_des;            //线性模型输出的理想值
//geometry_msgs::Vector3 angle_dis;            //DOB控制器估计的扰动值
geometry_msgs::Vector3 angle_target;            //经DOB控制器作用后的实际系统输入值
geometry_msgs::Vector3 vel_target, vel_read, vel_read2;
geometry_msgs::Vector3 currentRPYangle;   //欧拉角
geometry_msgs::Vector3 filteredPlaneVelmsg;   //欧拉角
// debug data
geometry_msgs::Vector3 vel_vicon;
const float MAX_POSITION_MEASURE_ERROR = 1;
Eigen::Quaterniond current_quaternion(0,0,0,0);

bool pose_initialized = false;
bool vel_initialized = false;
int posvel_updatesynflag= false;// the variable to make sure the pos and vel used in ocp is synchronized, In reality, we can put pos and vel in one topic to avoid this problem
USING_NAMESPACE_ACADO
VariablesGrid state, parameter, control;// used for output result
returnValue resultofsolve;
int timeconsumption=0;
float t_end=2;
int pointnumber=60;// the number is almost always 20. It less, the accuracy won't be enough, if more, the time consumpiton will be too large.
int controlfreq=30;
int discretizedpointpersecond=(int)pointnumber/t_end;
offb_posctl::controlstate controlstate_msg;
bool currentupdateflag= false;
FILTER filterDroneVelx(150);
sensor_msgs::Imu drone_imu;

void state_cb(const mavros_msgs::State::ConstPtr &msg){
    current_state = *msg;
}

void pos_cb(const geometry_msgs::PoseStamped::ConstPtr &msg){ //由ConstPtr可以看到msg是指针的引用，因此*msg相当于对被引用的指针对象取值
    if (pose_initialized== false)
    {
        pos_drone_last=*msg;
        pose_initialized=true;
        return;
    }
    if(fabs((*msg).pose.position.x - pos_drone_last.pose.position.x) < MAX_POSITION_MEASURE_ERROR &&
       fabs((*msg).pose.position.y - pos_drone_last.pose.position.y) < MAX_POSITION_MEASURE_ERROR &&
       fabs((*msg).pose.position.z - pos_drone_last.pose.position.z) < MAX_POSITION_MEASURE_ERROR)
    {
        pos_drone_last = pos_drone;
        pos_drone = *msg;
        current_quaternion=Eigen::Quaterniond(pos_drone.pose.orientation.w,pos_drone.pose.orientation.x,pos_drone.pose.orientation.y,pos_drone.pose.orientation.z);
       if(pose_initialized&&vel_initialized)
       {
           if (fabs(pos_drone.header.stamp.nsec-vel_drone.header.stamp.nsec)<30e6)
           {
               posvel_updatesynflag=true;
           }
       }
    } else
    {
        pos_drone_last=*msg;
    }
}

int main( int argc, char ** argv) {
    ros::init(argc, argv, "acado_lag_control");
    ros::NodeHandle nh;
    ros::Rate rate(
            100);// it is the rate to check and process callback functions. the high rate is to avoid the latency to accept the pos and vel

    ros::Publisher controlstate_pub = nh.advertise<offb_posctl::controlstate>("ocp/control_state", 10);

/*******fscanf method can not work very well
    char start, end;
    float pz, py, vz, vy, phi, phirate, thrust, torque;
    FILE *fin_state,*fin_u;// the instance can be used to read the file
    fin_state=fopen("/home/sensenliu/catkin_ws/src/gazebo_ros_learning/offb_posctl/data/state.csv","r");// the directory of th data.csv
    while (!feof(fin_state)) {// juddge whether the fin reach the end line of the csv file
        fscanf(fin_state,"%c,%f,%f,%f,%f,%f,%f,%f,%f,%c",&start,&pz,&py,&vz,&vy,&phi, &phirate, &thrust, &torque, &end);
//      fin_state >> line;// read one line data to "line"
        cout<<"every row:"<<pz<<endl;
        counter++; // counter plus plus
    }
    fclose(fin_state);//close！！
    ***********************/

    controlstate_msg.discrepointpersecond=discretizedpointpersecond;
    controlstate_msg.inicounter=0;
    controlstate_msg.arraylength=control.getNumPoints();
    controlstate_msg.thrustarray.clear();
    controlstate_msg.thetaarray.clear();
    controlstate_msg.stateXarray.clear();
    controlstate_msg.stateZarray.clear();
    controlstate_msg.stateVXarray.clear();
    controlstate_msg.stateVZarray.clear();
    float pz, py, vz, vy, phi, phirate, thrust, torque;
    ifstream fin_state, fin_u;// the instance can be used to read the file
    string line,number; //used for store the one line data from csv temporarily
    fin_state.open("/home/sensenliu/catkin_ws/src/gazebo_ros_learning/offb_posctl/data/state.csv");// the directory of th data.csv

    while (getline(fin_state, line)) {
        istringstream readstr(line);
//        cout << "every row:" << line << endl;

        getline(readstr,number,'\t');//here we use '\t' as the seperator, because we find the '\t' in line during debug.
        getline(readstr,number,'\t');//the second col is tiemstamp,

        getline(readstr,number,'\t');
        pz=atof(number.c_str());

        getline(readstr,number,'\t');
        py=atof(number.c_str());

        getline(readstr,number,'\t');
        vz=atof(number.c_str());

        getline(readstr,number,'\t');
        vy=atof(number.c_str());

        getline(readstr,number,'\t');
        phi=atof(number.c_str());

        getline(readstr,number,'\t');// phirate is useless

        getline(readstr,number,'\t');
        thrust=atof(number.c_str());

        getline(readstr,number,'\t');
        torque=atof(number.c_str());

        controlstate_msg.thrustarray.push_back(thrust);
        controlstate_msg.stateXarray.push_back(py);
        controlstate_msg.stateZarray.push_back(pz);
        controlstate_msg.stateVXarray.push_back(vy);
        controlstate_msg.stateVZarray.push_back(vz);
        controlstate_msg.thetaarray.push_back(phi);
    }

//                ros::spinOnce();// update to newest state to match the state.
//                controlstate_msg.inicounter=StateMatch();//match the state
    controlstate_pub.publish(controlstate_msg);
//                cout<<"controlstate_msg.stateXarray[0]-----------fffffff:"<<controlstate_msg.stateXarray[0]<<"  state.getMatrix(i)(0,0):"<<state.getMatrix(0)(0,0)<<endl;

    return 0;
}


#include <boost/test/unit_test.hpp>
#include "algorithms/other/buy_sell_stock_one_transaction.hpp"

BOOST_AUTO_TEST_SUITE(BuyAndSellStockOneTransaction)

BOOST_AUTO_TEST_CASE(invalid_arg)
{
    BOOST_CHECK(0 == Algo::Other::BuySellStockOneTrans::GetMaxProfit({}));
    BOOST_CHECK(0 == Algo::Other::BuySellStockOneTrans::GetMaxProfit({1}));
}

BOOST_AUTO_TEST_CASE(two_elements)
{
    BOOST_CHECK(8 == Algo::Other::BuySellStockOneTrans::GetMaxProfit({2, 10}));
    BOOST_CHECK(0 == Algo::Other::BuySellStockOneTrans::GetMaxProfit({10, 2}));
}

BOOST_AUTO_TEST_CASE(leetcode_tc)
{
    BOOST_CHECK(2 == Algo::Other::BuySellStockOneTrans::GetMaxProfit({2, 4, 1}));
    BOOST_CHECK(3 == Algo::Other::BuySellStockOneTrans::GetMaxProfit({2, 1, 4}));
    BOOST_CHECK(16 == Algo::Other::BuySellStockOneTrans::GetMaxProfit(
            {10, 26, 1, 3, 2, 4, 7}));
}

BOOST_AUTO_TEST_SUITE_END()


#include <chrono>
#include <cstdlib>
#include <iostream>
#include <string>
#include <boost/asio.hpp>
#include <asio-zmq.hpp>
#include "helper.hpp"

int main(int argc, char* argv[])
{
    if (argc != 4) {
        std::cerr << "usage: local_lat <bind-to> <message-size> "
                  << "<roundtrip-count>\n";
        return 1;
    }

    std::string const ep = argv[1];
    int message_size = std::atoi(argv[2]);
    int roundtrip_count = std::atoi(argv[3]);

    boost::asio::io_service ios;
    boost::asio::zmq::context ctx;

    boost::asio::zmq::test::perf::requester requester(ios, ctx, roundtrip_count, message_size, ep);

    auto watch = std::chrono::system_clock::now();

    ios.run();

    auto elapsed = std::chrono::system_clock::now() - watch;
    double latency = static_cast<double>(
                         std::chrono::duration_cast<std::chrono::microseconds>(elapsed).count()) /
                     (roundtrip_count * 2);
    std::cout << "message size: " << message_size << " [B]\n";
    std::cout << "roundtrip count: " << roundtrip_count << "\n";
    std::cout << "average latency: " << latency << " [us]\n";
}


// Copyright Louis Dionne 2013-2016
// 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 TEST_SUPPORT_CNUMERIC_HPP
#define TEST_SUPPORT_CNUMERIC_HPP

#include <boost/hana/concept/integral_constant.hpp>
#include <boost/hana/core/when.hpp>
#include <boost/hana/fwd/core/to.hpp>
#include <boost/hana/fwd/equal.hpp>
#include <boost/hana/fwd/less.hpp>

template <typename T>
struct CNumeric { using value_type = T; };

template <typename T, T v>
struct cnumeric_t {
    static constexpr T value = v;
    using hana_tag = CNumeric<T>;
    constexpr operator T() const { return value; }
};

template <typename T, T v>
constexpr cnumeric_t<T, v> cnumeric{};

template <typename T, T v>
constexpr cnumeric_t<T, v> make_cnumeric() { return {}; }

namespace boost { namespace hana {
    // Constant and IntegralConstant
    template <typename T>
    struct IntegralConstant<CNumeric<T>> {
        static constexpr bool value = true;
    };

    template <typename T, typename C>
    struct to_impl<CNumeric<T>, C, when<
        hana::IntegralConstant<C>::value
    >>
        : embedding<is_embedded<typename C::value_type, T>::value>
    {
        template <typename N>
        static constexpr auto apply(N const&)
        { return cnumeric<T, N::value>; }
    };

    // Comparable
    template <typename T, typename U>
    struct equal_impl<CNumeric<T>, CNumeric<U>> {
        template <typename X, typename Y>
        static constexpr auto apply(X const&, Y const&)
        { return cnumeric<bool, X::value == Y::value>; }
    };

    // Orderable
    template <typename T, typename U>
    struct less_impl<CNumeric<T>, CNumeric<U>> {
        template <typename X, typename Y>
        static constexpr auto apply(X const&, Y const&)
        { return cnumeric<bool, (X::value < Y::value)>; }
    };
}} // end namespace boost::hana

#endif // !TEST_SUPPORT_CNUMERIC_HPP


#ifndef SPARSE_HPP
#define SPARSE_HPP

#include <Eigen/Sparse>
#include "real.hpp"

using rmat = Eigen::SparseMatrix<real, Eigen::RowMajor>;
using cmat = Eigen::SparseMatrix<real, Eigen::ColMajor>;

using triplet = Eigen::Triplet<real>;
using triplet_iterator = std::back_insert_iterator< std::vector<triplet> >;

#endif


//===----------------------------------------------------------------------===//
//
//                     The LLVM Compiler Infrastructure
//
// This file is dual licensed under the MIT and the University of Illinois Open
// Source Licenses. See LICENSE.TXT for details.
//
//===----------------------------------------------------------------------===//
//  Adaptation to Boost of the libcxx
//  Copyright 2010 Vicente J. Botet Escriba
//  Distributed under the Boost Software License, Version 1.0.
//  See http://www.boost.org/LICENSE_1_0.txt

#include <boost/ratio/ratio.hpp>
#if !defined(BOOST_NO_STATIC_ASSERT)
#define NOTHING ""
#endif

void test()
{
    {
    typedef boost::ratio<1, 1> R1;
    typedef boost::ratio<1, 1> R2;
    BOOST_RATIO_STATIC_ASSERT((boost::ratio_greater_equal<R1, R2>::value), NOTHING, ());
    }
    {
    typedef boost::ratio<BOOST_RATIO_INTMAX_T_MAX, 1> R1;
    typedef boost::ratio<BOOST_RATIO_INTMAX_T_MAX, 1> R2;
    BOOST_RATIO_STATIC_ASSERT((boost::ratio_greater_equal<R1, R2>::value), NOTHING, ());
    }
    {
    typedef boost::ratio<1,BOOST_RATIO_INTMAX_T_MAX> R1;
    typedef boost::ratio<1,BOOST_RATIO_INTMAX_T_MAX> R2;
    BOOST_RATIO_STATIC_ASSERT((boost::ratio_greater_equal<R1, R2>::value), NOTHING, ());
    }
    {
    typedef boost::ratio<-BOOST_RATIO_INTMAX_T_MAX, 1> R1;
    typedef boost::ratio<-BOOST_RATIO_INTMAX_T_MAX, 1> R2;
    BOOST_RATIO_STATIC_ASSERT((boost::ratio_greater_equal<R1, R2>::value), NOTHING, ());
    }
    {
    typedef boost::ratio<1, BOOST_RATIO_INTMAX_T_MAX> R1;
    typedef boost::ratio<1, BOOST_RATIO_INTMAX_T_MAX> R2;
    BOOST_RATIO_STATIC_ASSERT((boost::ratio_greater_equal<R1, R2>::value), NOTHING, ());
    }
    {
    typedef boost::ratio<1, 1> R1;
    typedef boost::ratio<1, -1> R2;
    BOOST_RATIO_STATIC_ASSERT((boost::ratio_greater_equal<R1, R2>::value), NOTHING, ());
    }
    {
    typedef boost::ratio<BOOST_RATIO_INTMAX_T_MAX, 1> R1;
    typedef boost::ratio<-BOOST_RATIO_INTMAX_T_MAX, 1> R2;
    BOOST_RATIO_STATIC_ASSERT((boost::ratio_greater_equal<R1, R2>::value), NOTHING, ());
    }
    {
    typedef boost::ratio<-BOOST_RATIO_INTMAX_T_MAX, 1> R1;
    typedef boost::ratio<BOOST_RATIO_INTMAX_T_MAX, 1> R2;
    BOOST_RATIO_STATIC_ASSERT((!boost::ratio_greater_equal<R1, R2>::value), NOTHING, ());
    }
    {
    typedef boost::ratio<1, BOOST_RATIO_INTMAX_T_MAX> R1;
    typedef boost::ratio<1, -BOOST_RATIO_INTMAX_T_MAX> R2;
    BOOST_RATIO_STATIC_ASSERT((boost::ratio_greater_equal<R1, R2>::value), NOTHING, ());
    }
}


//==================================================================================================
/*
  Copyright 2017 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#include <boost/simd/algorithm/iota.hpp>
#include <boost/simd/pack.hpp>
#include <numeric>
#include <vector>
#include <simd_test.hpp>

using namespace boost::simd;

STF_CASE_TPL( "Check unary simd::iota", (float)) // STF_NUMERIC_TYPES )
{
  static const int N = pack<T>::static_size;

  std::vector<T> values(2*N+1), ref(2*N+1);
  std::iota(ref.begin(), ref.end(), T(3));

  boost::simd::iota(values.data(), values.data()+values.size(), T(3));

  STF_EQUAL( values, ref );
}


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


#ifndef BAYESMIX_CLUSTERESTIMATOR_HPP
#define BAYESMIX_CLUSTERESTIMATOR_HPP

#include "lossfunction/LossFunction.hpp"
#include "lossfunction/BinderLoss.hpp"
#include "lossfunction/VariationInformation.hpp"
#include <Eigen/Dense>
#include <iostream>
#include <cstdlib>
#include <map>

// in case we want to add other minimization methods in the future.
enum MINIMIZATION_METHOD {
  GREEDY
};

/**
 * Describe the full process of clustering :
 *      - choice of loss function via LossFunction member
 *      - Choice of minimization method via loss_type in the constructor
 *      - computation of cluster estimate through cluster_estimate method.
 */

class ClusterEstimator {
 private:
  LossFunction* loss_function;
  Eigen::MatrixXi mcmc_sample; // T*N matrix
  int T; // total time of the process
  int N;
  int K_up;
  Eigen::VectorXi initial_partition;
 public:
  ClusterEstimator(Eigen::MatrixXi &mcmc_sample_, LOSS_FUNCTION loss_type_,
                    int K_up, Eigen::VectorXi &initial_partition_);
  ~ClusterEstimator();
  double expected_posterior_loss(Eigen::VectorXi a);
//  Eigen::VectorXd expected_posterior_loss_for_each_Kup(Eigen::VectorXi a);
  Eigen::VectorXi cluster_estimate(MINIMIZATION_METHOD method);
  Eigen::VectorXi greedy_algorithm(Eigen::VectorXi &a);
};

#endif  // BAYESMIX_CLUSTERESTIMATOR_HPP


//kambuchaCarteOPoseTorque
//  Move to the given Cartesian pose w.r.t. base frame
//  You can specify a file name to assign the goal pose
//
//  Haopeng Hu
//  2020.06.30
//
//  kambuchaCarteOPoseTorque <fci-ip> fileName

#include <iostream>
#include <string>
#include <cmath>
#include <fstream>
#include <sstream>
#include <vector>

#include <franka/robot.h>
#include <franka/exception.h>
#include <franka/model.h>

//#include <Eigen/Dense>

int main(int argc, char** argv){
    if(argc < 4){
        std::cerr << "Usage: " << argv[0] << "<fci-ip> " << "fileName" << std::endl;
        return -1;
    }
    // Read goal poses
    std::string fileName(argv[2]);
    fileName.append(".csv");            // fileName.csv
    std::ifstream fileIn(fileName,std::ios::in);
    if(fileIn.fail()){
        std::cerr << "File: " << argv[2] << ".csv not found" << std::endl;
        return -1;
    }
    std::vector<std::vector<double>> cartePoseData; // Store the Cartesian pose data
    std::string line;                               // Store one line of data for file reading
    std::vector<double> cartePoseDataTmp;           // Store one Cartesian pose data
    while (std::getline(fileIn,line) && fileIn.good())
    {
        std::istringstream dataIn(line);
        std::string dataTmp;
        while (std::getline(dataIn,dataTmp,','))
        {
            cartePoseDataTmp.push_back(std::stod(dataTmp));
        }
        cartePoseData.push_back(cartePoseDataTmp);
    }
    std::cout << cartePoseData.size() << " Cartesian poses are read" << std::endl;
    try
    {
        // Init. the robot
        franka::Robot robot(argv[1]);
        franka::Model model = robot.loadModel();
        std::cout << "This program will run the robot. Please keep the user stop button at hand!" << std::endl
            << "Press Enter to continue..." << std::endl;
        std::cin.ignore();
        robot.setCollisionBehavior(
            {{20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0}}, {{20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0}},
            {{20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0}}, {{20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0}},
            {{20.0, 20.0, 20.0, 25.0, 25.0, 25.0}}, {{20.0, 20.0, 20.0, 25.0, 25.0, 25.0}},
            {{20.0, 20.0, 20.0, 25.0, 25.0, 25.0}}, {{20.0, 20.0, 20.0, 25.0, 25.0, 25.0}});
        // Torque controller
        auto controllerTorque = [&](const franka::RobotState& state, franka::Duration period) -> franka::Torques{
            // Impedance control law
            franka::Torques tau_c({{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}});
            return tau_c;
        };
        // Run the Cartesin pose one by one
        for (int i = 0; i < cartePoseData.size(); i++)
        {
            // Cartesian pose [i]
            try
            {
                // Motion generator
                double timer = 0.0;
                std::array<double,16> init_carte_pose;
                std::array<double,16> goal_carte_pose;
                for (unsigned int j = 0; j < 16; j++)
                {
                    goal_carte_pose[j] = cartePoseData[i][j];
                }
                robot.control(controllerTorque, [&](const franka::RobotState state, franka::Duration period)
                    -> franka::CartesianPose{
                    // Position and Quaternion interpolation
                    timer += period.toSec();
                    if(timer == 0.0){
                        // The first pose must be the initial pose
                        init_carte_pose = state.O_T_EE_d;
                    }
                    std::array<double,16> carte_pose_c = init_carte_pose;
                    // Position intepolation
                    carte_pose_c[12] = init_carte_pose[12] + (goal_carte_pose[12] - init_carte_pose[12])/2*(1-cos(M_PI_4*timer));
                    carte_pose_c[13] = init_carte_pose[13] + (goal_carte_pose[13] - init_carte_pose[13])/2*(1-cos(M_PI_4*timer));
                    carte_pose_c[14] = init_carte_pose[14] + (goal_carte_pose[14] - init_carte_pose[14])/2*(1-cos(M_PI_4*timer));
                    if(timer >= 4){
                        return franka::MotionFinished(carte_pose_c);
                    }
                    return carte_pose_c;
                });
            }
            catch(const franka::ControlException& e)
            {
                std::cerr << e.what() << '\n';
                std::cout << "Running error recovery..." << std::endl;
                robot.automaticErrorRecovery();
            }
            std::cout << "Cartesian pose: " << i << "finished" << std::endl;
        }
        std::cout << "Motion finished!" << std::endl;
    }
    catch(const franka::Exception& e)
    {
        std::cerr << e.what() << '\n';
        return -1;
    }
    return 0;
}


/*******************************************************************************
 * reservoir/reservoir_gather.hpp
 *
 * Naive Distributed Reservoir Sampling
 *
 * Copyright (C) 2019 Lorenz Hübschle-Schneider <lorenz@4z2.de>
 *
 * All rights reserved. Published under the GNU General Public License 3
 ******************************************************************************/

#pragma once

#ifndef RESERVOIR_RESERVOIR_GATHER_HEADER
#define RESERVOIR_RESERVOIR_GATHER_HEADER

#include <reservoir/aggregate.hpp>
#include <reservoir/logger.hpp>
#include <reservoir/select_helpers.hpp>
#include <reservoir/stats.hpp>
#include <reservoir/timer.hpp>
#include <reservoir/util.hpp>

#include <tlx/define.hpp>
#include <tlx/die/core.hpp>

#include <boost/mpi.hpp>

#include <utility>
#include <vector>

namespace mpi = boost::mpi;

namespace reservoir {

namespace _detail {
struct gather_selection {
    static const std::string name() {
        return "gather";
    }
};
} // namespace _detail

template <typename Key, typename RNG>
class reservoir_gather {
public:
    static constexpr const char *short_name = "[res]";

    using key_type = Key;
    using select_type = _detail::gather_selection;

    static constexpr bool check = false;
    static constexpr bool debug = false;
    static constexpr bool time = true;

    reservoir_gather(mpi::communicator &comm, size_t size, size_t seed)
        : rng_(seed + static_cast<size_t>(comm.rank())), comm_(comm),
          size_(size), threshold_(0.0), batch_id_(0) {
        LOGRC(check) << "Checking is active, things might be slow!";
    }

    // Iterator should dereference to (weight, id) pairs
    template <typename Iterator>
    void insert(Iterator begin, Iterator end) {
        timer t, t_total;

        pLOG << "batch " << batch_id_ << " beginning";

        items_.clear();

        // Step 1: process new items locally
        Iterator it = begin;

        if (threshold_ == 0.0) {
            // Woop, just insert them all!
            while (it != end) {
                // generate exponentially distributed variables
                double key = rng_.next_exponential(it->first);
                spLOG0 << "item" << *it << "key" << key;
                items_.emplace_back(key, it->second);
                ++it;
            }
            spLOG0 << "first batch took" << t.get() << "ms";
        } else {
            while (it != end) {
                it = insert_skip<true>(it, end, threshold_);
            }
        }
        pLOG0 << "done processing items";

        // Step 1b: local selection
        if (items_.size() > size_) {
            // Select `size_` smallest items locally to avoid transmitting
            // unnecessarily many (and OOM at root)
            std::nth_element(items_.begin(), items_.begin() + size_ - 1,
                             items_.end());
            // Discard the rest
            items_.resize(size_);
        }

        if constexpr (time) {
            stats_.record("size", items_.size());
            double t_insert = t.get();
            stats_.record("insert", t_insert);
            LOG0 << "RESULT op=insert pe=" << comm_.rank()
                 << " np=" << comm_.size() << " batchsize=" << end - begin
                 << " batch=" << batch_id_ << " samplesize=" << size_
                 << " time=" << t_insert;
            t.reset();
        }

        pLOG << "batch " << batch_id_ << " gathering...";

        // Step 2: gather
        if (comm_.rank() == 0) {
            sizes_.resize(comm_.size());
        }
        mpi::gather(comm_, static_cast<int>(items_.size()), sizes_, 0);
        size_t old_size = all_items_.size();
        if (comm_.rank() == 0) {
            // Compute displacements. According to the MPI standard,
            // displacements are significant only at the root, but boost::mpi
            // wants to compute them at every PE if they're not provided...
            int nprocs = comm_.size(), aux = 0;
            displ_.resize(nprocs);
            for (int rank = 0; rank < nprocs; ++rank) {
                displ_[rank] = aux;
                aux += sizes_[rank];
            }
            // Allocate space for receiving all items at the end of the all_items_ vector
            all_items_.resize(old_size + aux);
        }
        mpi::gatherv(comm_, items_, all_items_.data() + old_size, sizes_,
                     displ_, 0);
        if constexpr (time) {
            double t_select = t.get();
            stats_.record("gather", t_select);
            LOG0 << "RESULT op=gather pe=" << comm_.rank()
                 << " np=" << comm_.size() << " batchsize=" << end - begin
                 << " batch=" << batch_id_ << " samplesize=" << size_
                 << " time=" << t_select;
            t.reset();
        }

        // Step 3: sequential selection
        sLOGR << "Have" << all_items_.size()
              << "items under consideration in batch" << batch_id_;
        if (comm_.rank() == 0) {
            // Select result sequentially at root
            std::nth_element(all_items_.begin(), all_items_.begin() + size_ - 1,
                             all_items_.end());
            // Discard the rest
            all_items_.resize(size_);
            threshold_ = all_items_[size_ - 1].first;
            tlx_die_unless(threshold_ > 0);
        }
        // Broadcast new threshold
        mpi::broadcast(comm_, threshold_, 0);
        sLOGR << "Threshold:" << threshold_ << "in batch" << batch_id_;

        if constexpr (time) {
            double t_split = t.get();
            stats_.record("select", t_split);
            LOG0 << "RESULT op=select pe=" << comm_.rank()
                 << " np=" << comm_.size() << " batchsize=" << end - begin
                 << " batch=" << batch_id_ << " samplesize=" << size_
                 << " time=" << t_split;
            t.reset();

            stats_.record("total", t_total.get());
        }

        pLOG << "batch " << batch_id_ << " done";

        ++batch_id_;
    }

    template <typename Callback>
    void sample(Callback &&callback) const {
        // only relevant at PE 0
        for (auto it = all_items_.begin(); it != all_items_.end(); ++it) {
            callback(it->second);
        }
    }

    _detail::res_stats<time> &get_stats() {
        return stats_;
    }

    auto get_mss_stats() {
        return _detail::select_stats<false>();
    }

protected:
    template <size_t w, typename Iterator>
    constexpr double vec_sum(Iterator it) {
        if constexpr (w == 1) {
            return it->first;
        } else if constexpr (w == 3) {
            return it->first + (it + 1)->first + (it + 2)->first;
        } else {
            return vec_sum<w / 2>(it) + vec_sum<w / 2>(it + w / 2);
        }
    }

    template <bool far, typename Iterator>
    TLX_ATTRIBUTE_ALWAYS_INLINE Iterator insert_skip(Iterator it, Iterator end,
                                                     double threshold) {
        double skip = rng_.next_exponential(threshold);
        pLOG0 << "skip = " << skip;

        if constexpr (far) {
            // number of elements to skip at a time
            constexpr size_t w = 32;

            double sum = 0.0;
            // Check that the next w items (including this) appear before `end`
            Iterator curr_last = it + (w - 1);
            Iterator prev;
            while (curr_last < end && skip >= 0) {
                sum = vec_sum<w>(it);

                if constexpr (check) {
                    double s2 = 0.0;
                    for (size_t i = 0; i < w; i++) {
                        s2 += (it + i)->first;
                    }
                    tlx_die_unless(std::abs(s2 - sum) < 1e-10);
                }

                skip -= sum;
                prev = it;
                // Avoid advancing iterator twice, it might not be a random
                // access iterator
                it = curr_last + 1;
                curr_last += w;
            }
            if (skip < 0) {
                // undo jump
                it = prev;
                skip += sum;
            } else if (it >= end) {
                return end;
            }
        }

        while (++it != end && skip >= 0) {
            skip -= it->first;
        }
        if (it == end)
            return it;

        double minv = std::exp(-threshold * it->first);
        double r = rng_.next(minv, 1.0);
        double key = -std::log(r) / it->first;
        my_assert(key > 0);
        spLOG0 << "item" << *it << "minv" << minv << "r" << r << "key" << key;
        items_.emplace_back(key, it->second);
        return it;
    }

    // all_items is significant only at PE 0
    std::vector<std::pair<double, Key>> items_, all_items_;
    std::vector<int> sizes_, displ_;
    RNG rng_;
    mpi::communicator &comm_;
    size_t size_;
    double threshold_;

    size_t batch_id_;
    mutable _detail::res_stats<time> stats_;
};

} // namespace reservoir

#endif // RESERVOIR_RESERVOIR_GATHER_HEADER


//
// Copyright (c) 2019 CNRS
//

#include "pinocchio/bindings/python/algorithm/algorithms.hpp"
#include "pinocchio/algorithm/centroidal-derivatives.hpp"

#include <boost/python/tuple.hpp>

namespace pinocchio
{
  namespace python
  {

    bp::tuple computeCentroidalDynamicsDerivatives_proxy(const Model & model,
                                                         Data & data,
                                                         const Eigen::VectorXd & q,
                                                         const Eigen::VectorXd & v,
                                                         const Eigen::VectorXd & a)
    {
      typedef Data::Matrix6x Matrix6x;
      Matrix6x partialh_dq(Matrix6x::Zero(6,model.nv));
      Matrix6x partial_dq(Matrix6x::Zero(6,model.nv));
      Matrix6x partial_dv(Matrix6x::Zero(6,model.nv));
      Matrix6x partial_da(Matrix6x::Zero(6,model.nv));

      computeCentroidalDynamicsDerivatives(model,data,q, v, a,
                                           partialh_dq, partial_dq, partial_dv, partial_da);

      return bp::make_tuple(partialh_dq, partial_dq,partial_dv,partial_da);
    }

    bp::tuple getCentroidalDynamicsDerivatives_proxy(const Model & model,
                                                     Data & data)
    {
      typedef Data::Matrix6x Matrix6x;

      Matrix6x partialh_dq(Matrix6x::Zero(6,model.nv));
      Matrix6x partial_dq(Matrix6x::Zero(6,model.nv));
      Matrix6x partial_dv(Matrix6x::Zero(6,model.nv));
      Matrix6x partial_da(Matrix6x::Zero(6,model.nv));

      getCentroidalDynamicsDerivatives(model,data, partialh_dq, partial_dq, partial_dv, partial_da);
      return bp::make_tuple(partialh_dq,partial_dq,partial_dv,partial_da);
    }

    void exposeCentroidalDerivatives()
    {
      using namespace Eigen;

      bp::def("computeCentroidalDynamicsDerivatives",
              computeCentroidalDynamicsDerivatives_proxy,
              bp::args("Model","Data",
                       "q: configuration vector (size model.nq)",
                       "v: velocity vector (size model.nv)",
                       "a: acceleration vector (size model.nv)"),
              "Computes the analytical derivatives of the centroidal dynamics\n"
              "with respect to the joint configuration vector, velocity and acceleration.");
      bp::def("getCentroidalDynamicsDerivatives",
              getCentroidalDynamicsDerivatives_proxy,
              bp::args("Model","Data"),
              "Retrive the analytical derivatives of the centroidal dynamics\n"
              "from the RNEA derivatives.\n"
              "pinocchio.computeRNEADerivatives should have been called first.");
    }

  } // namespace python
} // namespace pinocchio


/*****************************************************************************
*
* Rokko: Integrated Interface for libraries of eigenvalue decomposition
*
* Copyright (C) 2013-2015 Rokko Developers https://github.com/t-sakashita/rokko
*
* 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 <rokko/rokko.hpp>
#include <rokko/utility/helmert_matrix.hpp>
#include <boost/foreach.hpp>
#define BOOST_TEST_MODULE test_solver
#ifndef BOOST_TEST_DYN_LINK
#include <boost/test/included/unit_test.hpp>
#else
#include <boost/test/unit_test.hpp>
#endif

BOOST_AUTO_TEST_CASE(test_solver) {
  int provided;
  MPI_Init_thread(&boost::unit_test::framework::master_test_suite().argc,
                  &boost::unit_test::framework::master_test_suite().argv,
                  MPI_THREAD_MULTIPLE, &provided);

  MPI_Comm comm = MPI_COMM_WORLD;
  const int dim = 100;
  rokko::localized_vector<double> diag(dim);
  diag.setLinSpaced(diag.size(), 1, diag.size()); // diag = [1, 2, 3, ..., dim]

  std::vector<std::string> names;
  int argc = boost::unit_test::framework::master_test_suite().argc;
  if (argc == 1) {
    names = rokko::parallel_dense_solver::solvers();
  } else {
    for (int num=1; num < argc; ++num) {
      names.push_back(boost::unit_test::framework::master_test_suite().argv[num]);
    }
  }

  BOOST_FOREACH(std::string name, names) {
    std::cout << "solver=" << name << std::endl;
    rokko::parallel_dense_solver solver(name);
    if (solver.is_available_grid_major(rokko::grid_col_major)) {
      solver.initialize(boost::unit_test::framework::master_test_suite().argc,
                        boost::unit_test::framework::master_test_suite().argv);
      rokko::grid g(comm, rokko::grid_col_major);
      rokko::distributed_matrix<double, rokko::matrix_col_major> mat(dim, dim, g, solver);
      rokko::helmert_matrix::generate_for_given_eigenvalues(mat, diag);
      rokko::localized_vector<double> w(dim);
      rokko::distributed_matrix<double, rokko::matrix_col_major> Z(dim, dim, g, solver);

      solver.diagonalize(mat, w, Z);

      BOOST_CHECK_CLOSE(w.sum(), diag.sum(), 10e-5);

      solver.finalize();
    }
  }

  BOOST_FOREACH(std::string name, names) {
    std::cout << "solver=" << name << std::endl;
    rokko::parallel_dense_solver solver(name);
    if (solver.is_available_grid_major(rokko::grid_row_major)) {
      solver.initialize(boost::unit_test::framework::master_test_suite().argc,
                        boost::unit_test::framework::master_test_suite().argv);
      rokko::grid g(comm, rokko::grid_row_major);
      rokko::distributed_matrix<double, rokko::matrix_col_major> mat(dim, dim, g, solver);
      rokko::helmert_matrix::generate_for_given_eigenvalues(mat, diag);
      rokko::localized_vector<double> w(dim);
      rokko::distributed_matrix<double, rokko::matrix_col_major> Z(dim, dim, g, solver);

      solver.diagonalize(mat, w, Z);

      BOOST_CHECK_CLOSE(w.sum(), diag.sum(), 10e-5);

      solver.finalize();
    }
  }
  MPI_Finalize();
}


//---------------------------------------------------------------------------//
// Copyright (c) 2018-2021 Mikhail Komarov <nemo@nil.foundation>
// Copyright (c) 2020-2021 Nikita Kaskov <nbering@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.
//---------------------------------------------------------------------------//
// @file Declaration of functionality that runs the R1CS ppzkSNARK for
// a given R1CS example.
//---------------------------------------------------------------------------//

#ifndef CRYPTO3_RUN_R1CS_PPZKSNARK_HPP
#define CRYPTO3_RUN_R1CS_PPZKSNARK_HPP

#include <boost/config.hpp>

#include <nil/crypto3/zk/snark/systems/ppzksnark/r1cs_ppzksnark.hpp>

#include "../r1cs_examples.hpp"

#include <nil/crypto3/zk/snark/algorithms/generate.hpp>
#include <nil/crypto3/zk/snark/algorithms/verify.hpp>
#include <nil/crypto3/zk/snark/algorithms/prove.hpp>

namespace nil {
    namespace crypto3 {
        namespace zk {
            namespace snark {

                /*template<typename CurveType>
                typename std::enable_if<CurveType::has_affine_pairing, void>::type
                test_affine_verifier(const typename r1cs_ppzksnark<CurveType>::verification_key_type &vk,
                                     const typename r1cs_ppzksnark<CurveType>::primary_input_type &primary_input,
                                     const typename r1cs_ppzksnark<CurveType>::proof_type &proof,
                                     const bool expected_answer) {
                    const bool answer = r1cs_ppzksnark_affine_verifier_weak_IC<CurveType>(vk, primary_input, proof);
                    BOOST_CHECK(answer == expected_answer);
                }

                template<typename CurveType>
                typename std::enable_if<!CurveType::has_affine_pairing, void>::type
                test_affine_verifier(const typename r1cs_ppzksnark<CurveType>::verification_key_type &vk,
                                     const typename r1cs_ppzksnark<CurveType>::primary_input_type &primary_input,
                                     const typename r1cs_ppzksnark<CurveType>::proof_type &proof,
                                     const bool expected_answer) {
                    BOOST_ATTRIBUTE_UNUSED(vk, primary_input, proof, expected_answer);
                }*/

                /**
                 * The code below provides an example of all stages of running a R1CS ppzkSNARK.
                 *
                 * Of course, in a real-life scenario, we would have three distinct entities,
                 * mangled into one in the demonstration below. The three entities are as follows.
                 * (1) The "generator", which runs the ppzkSNARK generator on input a given
                 *     constraint system CS to create a proving and a verification key for CS.
                 * (2) The "prover", which runs the ppzkSNARK prover on input the proving key,
                 *     a primary input for CS, and an auxiliary input for CS.
                 * (3) The "verifier", which runs the ppzkSNARK verifier on input the verification key,
                 *     a primary input for CS, and a proof.
                 */
                template<typename CurveType>
                bool run_r1cs_ppzksnark(const r1cs_example<typename CurveType::scalar_field_type> &example) {

                    using basic_proof_system = r1cs_ppzksnark<CurveType>;
                    using weak_proof_system = r1cs_ppzksnark<CurveType,
                                          r1cs_ppzksnark_generator<CurveType>,
                                          r1cs_ppzksnark_prover<CurveType>,
                                          r1cs_ppzksnark_verifier_weak_input_consistency<CurveType>>;

                    std::cout << "Starting generator" << std::endl;
                    typename basic_proof_system::keypair_type keypair =
                        generate<basic_proof_system>(example.constraint_system);

                    std::cout << "Starting verification key processing" << std::endl;

                    typename basic_proof_system::processed_verification_key_type pvk =
                        r1cs_ppzksnark_process_verification_key<CurveType>::process(keypair.second);

                    std::cout << "Starting prover" << std::endl;

                    typename basic_proof_system::proof_type proof =
                        prove<basic_proof_system>(keypair.first, example.primary_input, example.auxiliary_input);

                    std::cout << "Starting verifier" << std::endl;

                    const bool ans = verify<basic_proof_system>(keypair.second, example.primary_input, proof);

                    std::cout << "Verifier finished, result: " << ans << std::endl;

                    std::cout << "Starting online verifier" << std::endl;

                    const bool ans2 =
                        verify<basic_proof_system>(pvk, example.primary_input, proof);

                    std::cout << "Online verifier finished, result: " << ans2 << std::endl;

                    BOOST_CHECK(ans == ans2);

                    std::cout << "Starting weak verifier" << std::endl;

                    const bool ans3 = verify<weak_proof_system>(keypair.second,
                    example.primary_input, proof);

                    std::cout << "Weak verifier finished, result: " << ans3 << std::endl;

                    BOOST_CHECK(ans == ans3);

                    std::cout << "Starting online weak verifier" << std::endl;

                    const bool ans4 = verify<weak_proof_system>(pvk, example.primary_input, proof);

                    std::cout << "Online weak verifier finished, result: " << ans4 << std::endl;

                    BOOST_CHECK(ans == ans4);

                    /*test_affine_verifier<CurveType>(keypair.second, example.primary_input, proof, ans);*/

                    return ans;
                }
            }    // namespace snark
        }        // namespace zk
    }            // namespace crypto3
}    // namespace nil

#endif    // CRYPTO3_RUN_R1CS_PPZKSNARK_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_SSE2_SIMD_FUNCTION_SPLIT_MULTIPLIES_HPP_INCLUDED
#define BOOST_SIMD_ARCH_X86_SSE2_SIMD_FUNCTION_SPLIT_MULTIPLIES_HPP_INCLUDED

#include <boost/simd/detail/overload.hpp>
#include <boost/simd/function/bitwise_cast.hpp>
#include <boost/simd/function/interleave_first.hpp>
#include <boost/simd/function/interleave_second.hpp>

namespace boost { namespace simd { namespace ext
{
  namespace bd =  boost::dispatch;
  namespace bs =  boost::simd;

  BOOST_DISPATCH_OVERLOAD ( split_multiplies_
                          , (typename A0)
                          , bs::sse2_
                          , bs::pack_<bd::int16_<A0>, bs::sse_>
                          , bs::pack_<bd::int16_<A0>, bs::sse_>
                         )
  {
    using up_t = bd::upgrade_t<A0>;
    using result_t = std::pair<up_t,up_t>;

    BOOST_FORCEINLINE result_t operator()(const A0 & a0, const A0 & a1) const BOOST_NOEXCEPT
    {
      A0 lo = _mm_mullo_epi16(a0, a1);
      A0 hi = _mm_mulhi_epi16(a0, a1);

      return  { bitwise_cast<up_t>(interleave_first(lo, hi))
              , bitwise_cast<up_t>(interleave_second(lo, hi))
              };
    }
  };

  BOOST_DISPATCH_OVERLOAD ( split_multiplies_
                          , (typename A0)
                          , bs::sse2_
                          , bs::pack_<bd::uint16_<A0>, bs::sse_>
                          , bs::pack_<bd::uint16_<A0>, bs::sse_>
                         )
  {
    using up_t = bd::upgrade_t<A0>;
    using result_t = std::pair<up_t,up_t>;

    BOOST_FORCEINLINE result_t operator()(const A0 & a0, const A0 & a1) const BOOST_NOEXCEPT
    {
      A0 lo = _mm_mullo_epi16(a0, a1);
      A0 hi = _mm_mulhi_epu16(a0, a1);

      return  { bitwise_cast<up_t>(interleave_first(lo, hi))
              , bitwise_cast<up_t>(interleave_second(lo, hi))
              };
    }
  };

  BOOST_DISPATCH_OVERLOAD ( split_multiplies_
                          , (typename A0)
                          , bs::sse2_
                          , bs::pack_<bd::uint32_<A0>, bs::sse_>
                          , bs::pack_<bd::uint32_<A0>, bs::sse_>
                         )
  {
    using up_t = bd::upgrade_t<A0>;
    using result_t = std::pair<up_t,up_t>;

    BOOST_FORCEINLINE result_t operator()(const A0 & a0, const A0 & a1) const BOOST_NOEXCEPT
    {
      up_t lo = _mm_mul_epu32(a0, a1);
      up_t hi = _mm_mul_epu32(_mm_srli_si128(a0, 4), _mm_srli_si128(a1, 4));

      return { interleave_first(lo, hi), interleave_second(lo, hi) };
    }
  };
} } }

#endif


#include "nanocv/nanocv.h"
#include "nanocv/measure.hpp"
#include "nanocv/vision/image_grid.h"
#include <boost/program_options.hpp>
#include <boost/filesystem.hpp>

int main(int argc, char *argv[])
{
        ncv::init();

        using namespace ncv;

        // prepare object string-based selection
        const strings_t model_ids = ncv::get_models().ids();

        // parse the command line
        boost::program_options::options_description po_desc("", 160);
        po_desc.add_options()("help,h", "generate samples that produce the classes for a model");
        po_desc.add_options()("model",
                boost::program_options::value<string_t>(),
                text::concatenate(model_ids, ", ").c_str());
        po_desc.add_options()("model-file",
                boost::program_options::value<string_t>(),
                "filepath to load the model from");
        po_desc.add_options()("save-dir",
                boost::program_options::value<string_t>()->default_value("./"),
                "directory to save generated samples to");
        po_desc.add_options()("save-group-rows",
                boost::program_options::value<size_t>()->default_value(8),
                "number of generated samples to construct for each label [1, 128]");
        po_desc.add_options()("save-group-cols",
                boost::program_options::value<size_t>()->default_value(8),
                "number of generated samples to construct for each label [1, 128]");

        boost::program_options::variables_map po_vm;
        boost::program_options::store(
                boost::program_options::command_line_parser(argc, argv).options(po_desc).run(),
                po_vm);
        boost::program_options::notify(po_vm);

        // check arguments and options
        if (	po_vm.empty() ||
                !po_vm.count("model") ||
                !po_vm.count("model-file") ||
                po_vm.count("help"))
        {
                std::cout << po_desc;
                return EXIT_FAILURE;
        }

        const string_t cmd_model = po_vm["model"].as<string_t>();
        const string_t cmd_input = po_vm["model-file"].as<string_t>();
        const string_t cmd_save_dir = po_vm.count("save-dir") ? po_vm["save-dir"].as<string_t>() : "";
        const size_t cmd_save_group_rows = math::clamp(po_vm["save-group-rows"].as<size_t>(), 1, 128);
        const size_t cmd_save_group_cols = math::clamp(po_vm["save-group-cols"].as<size_t>(), 1, 128);

        // create model
        const rmodel_t rmodel = ncv::get_models().get(cmd_model);

        // load model
        ncv::measure_critical_and_log(
                [&] () { return rmodel->load(cmd_input); },
                "loaded model",
                "failed to load model from <" + cmd_input + ">");

        // generate samples for each output class label
        const size_t labels = rmodel->osize();
        for (size_t l = 0; l < labels; l ++)
        {
                image_grid_t grid_image(rmodel->irows(), rmodel->icols(), cmd_save_group_rows, cmd_save_group_cols);

                const vector_t target = ncv::class_target(l, labels);
                for (size_t r = 0; r < cmd_save_group_rows; r ++)
                {
                        for (size_t c = 0; c < cmd_save_group_cols; c ++)
                        {
                                const tensor_t input = rmodel->generate(target);

                                image_t image;
                                if (!image.load(input))
                                {
                                        log_error() << "failed to map the generated input to RGBA image!";
                                        return EXIT_FAILURE;
                                }

                                grid_image.set(r, c, image);
                        }
                }

                const string_t path =
                        cmd_save_dir + "/" + boost::filesystem::basename(cmd_input) +
                        "_label" + text::to_string(l) + ".png";
                if (!grid_image.image().save(path))
                {
                        log_error() << "failed to save the generated input as RGBA image!";
                        return EXIT_FAILURE;
                }
        }

        // OK
        log_info() << done;
        return EXIT_SUCCESS;
}