#ifndef SHIROSE_COLLISION_HPP
#define SHIROSE_COLLISION_HPP

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

#include "shirose/pair.hpp"
#include "shirose/particle_system.hpp"

namespace shirose {

/// @brief If two particles will collide.
/// @param[in] x1 Position of particle 1
/// @param[in] x2 Position of particle 2
/// @param[in] v1 Velocity of particle 1
/// @param[in] v2 Velocity of particle 2
/// @param[in] r1 Radius of particle 1
/// @param[in] r2 Radius of particle 2
///
/// This function assumes both particle positions are at the same time point.
bool doCollide(const Eigen::Ref<const Eigen::Vector2d>& x1,
               const Eigen::Ref<const Eigen::Vector2d>& x2,
               const Eigen::Ref<const Eigen::Vector2d>& v1,
               const Eigen::Ref<const Eigen::Vector2d>& v2,  //
               double r1, double r2);

/// @brief Computes a time to the collision of two particles.
/// @param[in] x1 Position of particle 1
/// @param[in] x2 Position of particle 2
/// @param[in] v1 Velocity of particle 1
/// @param[in] v2 Velocity of particle 2
/// @param[in] r1 Radius of particle 1
/// @param[in] r2 Radius of particle 2
/// @returns Time to collision
///
/// This function assumes both particle positions are at the same time point.
std::optional<double> calcTimeToCollision(
    const Eigen::Ref<const Eigen::Vector2d>& x1,
    const Eigen::Ref<const Eigen::Vector2d>& x2,
    const Eigen::Ref<const Eigen::Vector2d>& v1,
    const Eigen::Ref<const Eigen::Vector2d>& v2,  //
    double r1, double r2);

/// @brief Computes collision times between particles.
/// @param[in] particles Particles
///
/// If a particle will not collide, its collision time is set to the maximum
/// value of double.
Eigen::MatrixXd calcTimeToCollision(const ParticleSystem& particles);

/// @brief Comutes velocity changes due to a collision.
/// @param[in] v1 Velocity of particle 1
/// @param[in] v2 Velocity of particle 2
/// @param[in] m1 Mass of particle 1
/// @param[in] m2 Mass of particle 2
/// @param[in] e Coefficient of restitution
/// @returns Velocity changes of particle 1 and 2
Pair<Eigen::Vector2d, Eigen::Vector2d> calcVelocityChanges(
    const Eigen::Ref<const Eigen::Vector2d>& v1,
    const Eigen::Ref<const Eigen::Vector2d>& v2,  //
    double m1, double m2, double e);

}  // namespace shirose

#endif  // SHIROSE_COLLISION_HPP


// demo gathered from http://www.boost.org/doc/libs/1_65_1/libs/graph/doc/file_dependency_example.html

#include <iostream> // std::cout
#include <utility>  // std::pair
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/topological_sort.hpp>

int main(int argc, char **argv)
{
    enum files_e
    {
        dax_h,
        yow_h,
        boz_h,
        zow_h,
        foo_cpp,
        foo_o,
        bar_cpp,
        bar_o,
        libfoobar_a,
        zig_cpp,
        zig_o,
        zag_cpp,
        zag_o,
        libzigzag_a,
        killerapp,
        N
    };
    const char *name[] = {"dax.h", "yow.h", "boz.h", "zow.h", "foo.cpp",
                          "foo.o", "bar.cpp", "bar.o", "libfoobar.a",
                          "zig.cpp", "zig.o", "zag.cpp", "zag.o",
                          "libzigzag.a", "killerapp"};

    typedef std::pair<int, int> Edge;
    Edge used_by[] = {
        Edge(dax_h, foo_cpp), Edge(dax_h, bar_cpp), Edge(dax_h, yow_h),
        Edge(yow_h, bar_cpp), Edge(yow_h, zag_cpp),
        Edge(boz_h, bar_cpp), Edge(boz_h, zig_cpp), Edge(boz_h, zag_cpp),
        Edge(zow_h, foo_cpp),
        Edge(foo_cpp, foo_o),
        Edge(foo_o, libfoobar_a),
        Edge(bar_cpp, bar_o),
        Edge(bar_o, libfoobar_a),
        Edge(libfoobar_a, libzigzag_a),
        Edge(zig_cpp, zig_o),
        Edge(zig_o, libzigzag_a),
        Edge(zag_cpp, zag_o),
        Edge(zag_o, libzigzag_a),
        Edge(libzigzag_a, killerapp)};

    using namespace boost;
    typedef adjacency_list<vecS, vecS, bidirectionalS,
                           property<vertex_color_t, default_color_type> >
        Graph;
    Graph g(used_by, used_by + sizeof(used_by) / sizeof(Edge), N);
    typedef graph_traits<Graph>::vertex_descriptor Vertex;

    typedef std::list<Vertex> MakeOrder;
    MakeOrder make_order;
    boost::topological_sort(g, std::front_inserter(make_order));

    std::cout << "make ordering: ";
    for (MakeOrder::iterator i = make_order.begin();
         i != make_order.end(); ++i)
        std::cout << name[*i] << " ";
    std::cout << std::endl;

    return 0;
}


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

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

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

// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#include <geometry_test_common.hpp>

#include <boost/geometry/arithmetic/arithmetic.hpp>
#include <boost/geometry/algorithms/assign.hpp>
#include <boost/geometry/algorithms/num_points.hpp>

#include <boost/geometry/geometries/geometries.hpp>
#include <boost/geometry/geometries/adapted/c_array.hpp>
#include <boost/geometry/geometries/adapted/boost_tuple.hpp>
#include <boost/geometry/io/wkt/wkt.hpp>
#include <boost/variant/variant.hpp>
#include <test_common/test_point.hpp>

BOOST_GEOMETRY_REGISTER_C_ARRAY_CS(cs::cartesian)
BOOST_GEOMETRY_REGISTER_BOOST_TUPLE_CS(cs::cartesian)

template <typename Geometry>
void check_geometry(Geometry const& geometry, std::string const& expected)
{
    std::ostringstream out;
    out << bg::wkt(geometry);
    BOOST_CHECK_EQUAL(out.str(), expected);
}

template <typename Geometry, typename Points>
void check_assign_points(Points const& points, std::string const& /*expected*/)
{
    Geometry geometry;
    bg::assign_points(geometry, points);
    check_geometry(geometry, "LINESTRING(1 2,3 4,5 6)");

    boost::variant<Geometry> v;
    bg::assign_points(v, points);
}

template <typename Point>
void test_assign_linestring_2d()
{
    bg::model::linestring<Point> line;

    // Test assignment of plain array (note that this is only possible if adapted c-array is included!)
    const double coors[3][2] = { {1, 2}, {3, 4}, {5, 6} };
    check_assign_points<bg::model::linestring<Point> >(coors, "LINESTRING(1 2,3 4,5 6)");

    // Test assignment of point array
    Point points[3];
    bg::assign_values(points[0], 1, 2);
    bg::assign_values(points[1], 3, 4);
    bg::assign_values(points[2], 5, 6);
    check_assign_points<bg::model::linestring<Point> >(points, "LINESTRING(1 2,3 4,5 6)");

    // Test assignment of array with different point-type (tuple adaption should be included)
    boost::tuple<float, float> tuples[3];
    tuples[0] = boost::make_tuple(1, 2);
    tuples[1] = boost::make_tuple(3, 4);
    tuples[2] = boost::make_tuple(5, 6);
    check_assign_points<bg::model::linestring<Point> >(tuples, "LINESTRING(1 2,3 4,5 6)");
}

namespace detail
{
    template <typename BoxOrSegment>
    void test_assign_box_or_segment_2d()
    {
        BoxOrSegment geometry;
        bg::assign_values(geometry, 1, 2, 3, 4);
        BOOST_CHECK((bg::get<bg::min_corner, 0>(geometry) == 1));
        BOOST_CHECK((bg::get<bg::min_corner, 1>(geometry) == 2));
        BOOST_CHECK((bg::get<bg::max_corner, 0>(geometry) == 3));
        BOOST_CHECK((bg::get<bg::max_corner, 1>(geometry) == 4));

        bg::assign_zero(geometry);
        BOOST_CHECK((bg::get<bg::min_corner, 0>(geometry) == 0));
        BOOST_CHECK((bg::get<bg::min_corner, 1>(geometry) == 0));
        BOOST_CHECK((bg::get<bg::max_corner, 0>(geometry) == 0));
        BOOST_CHECK((bg::get<bg::max_corner, 1>(geometry) == 0));

        bg::assign_inverse(geometry);
        BOOST_CHECK((bg::get<bg::min_corner, 0>(geometry) > 9999));
        BOOST_CHECK((bg::get<bg::min_corner, 1>(geometry) > 9999));
        BOOST_CHECK((bg::get<bg::max_corner, 0>(geometry) < 9999));
        BOOST_CHECK((bg::get<bg::max_corner, 1>(geometry) < 9999));
    }
}

template <typename Point>
void test_assign_box_or_segment_2d()
{
    detail::test_assign_box_or_segment_2d<bg::model::box<Point> >();
    detail::test_assign_box_or_segment_2d<bg::model::segment<Point> >();
}

template <typename Point>
void test_assign_box_2d()
{
    detail::test_assign_box_or_segment_2d<bg::model::box<Point> >();
}

template <typename Point>
void test_assign_point_3d()
{
    Point p;
    bg::assign_values(p, 1, 2, 3);
    check_geometry(p, "POINT(1 2 3)");

    bg::assign_value(p, 123);
    check_geometry(p, "POINT(123 123 123)");

    bg::assign_zero(p);
    check_geometry(p, "POINT(0 0 0)");
}

template <typename P>
void test_assign_conversion()
{
    typedef bg::model::box<P> box_type;
    typedef bg::model::ring<P> ring_type;
    typedef bg::model::polygon<P> polygon_type;

    P p;
    bg::assign_values(p, 1, 2);

    box_type b;
    bg::assign(b, p);

    BOOST_CHECK_CLOSE((bg::get<0, 0>(b)), 1.0, 0.001);
    BOOST_CHECK_CLOSE((bg::get<0, 1>(b)), 2.0, 0.001);
    BOOST_CHECK_CLOSE((bg::get<1, 0>(b)), 1.0, 0.001);
    BOOST_CHECK_CLOSE((bg::get<1, 1>(b)), 2.0, 0.001);

    bg::set<bg::min_corner, 0>(b, 1);
    bg::set<bg::min_corner, 1>(b, 2);
    bg::set<bg::max_corner, 0>(b, 3);
    bg::set<bg::max_corner, 1>(b, 4);

    ring_type ring;
    bg::assign(ring, b);

    {
        typedef bg::model::ring<P, false, false> ring_type_ccw;
        ring_type_ccw ring_ccw;
        // Should NOT compile (currently): bg::assign(ring_ccw, ring);

    }

    typename boost::range_const_iterator<ring_type>::type it = ring.begin();
    BOOST_CHECK_CLOSE(bg::get<0>(*it), 1.0, 0.001);
    BOOST_CHECK_CLOSE(bg::get<1>(*it), 2.0, 0.001);
    it++;
    BOOST_CHECK_CLOSE(bg::get<0>(*it), 1.0, 0.001);
    BOOST_CHECK_CLOSE(bg::get<1>(*it), 4.0, 0.001);
    it++;
    BOOST_CHECK_CLOSE(bg::get<0>(*it), 3.0, 0.001);
    BOOST_CHECK_CLOSE(bg::get<1>(*it), 4.0, 0.001);
    it++;
    BOOST_CHECK_CLOSE(bg::get<0>(*it), 3.0, 0.001);
    BOOST_CHECK_CLOSE(bg::get<1>(*it), 2.0, 0.001);
    it++;
    BOOST_CHECK_CLOSE(bg::get<0>(*it), 1.0, 0.001);
    BOOST_CHECK_CLOSE(bg::get<1>(*it), 2.0, 0.001);

    BOOST_CHECK_EQUAL(ring.size(), 5u);

    polygon_type polygon;

    bg::assign(polygon, ring);
    BOOST_CHECK_EQUAL(bg::num_points(polygon), 5u);

    ring_type ring2;
    bg::assign(ring2, polygon);
    BOOST_CHECK_EQUAL(bg::num_points(ring2), 5u);
}

template <typename P>
void test_assign_conversion_variant()
{
    typedef bg::model::box<P> box_type;
    typedef bg::model::ring<P> ring_type;
    typedef bg::model::polygon<P> polygon_type;

    P p;
    bg::assign_values(p, 1, 2);

    box_type b;
    boost::variant<box_type&> variant_b(b);
    bg::assign(variant_b, p);

    BOOST_CHECK_CLOSE((bg::get<0, 0>(b)), 1.0, 0.001);
    BOOST_CHECK_CLOSE((bg::get<0, 1>(b)), 2.0, 0.001);
    BOOST_CHECK_CLOSE((bg::get<1, 0>(b)), 1.0, 0.001);
    BOOST_CHECK_CLOSE((bg::get<1, 1>(b)), 2.0, 0.001);

    bg::set<bg::min_corner, 0>(b, 1);
    bg::set<bg::min_corner, 1>(b, 2);
    bg::set<bg::max_corner, 0>(b, 3);
    bg::set<bg::max_corner, 1>(b, 4);

    ring_type ring;
    boost::variant<ring_type&> variant_ring(ring);
    bg::assign(variant_ring, boost::variant<box_type>(b));

    {
        typedef bg::model::ring<P, false, false> ring_type_ccw;
        ring_type_ccw ring_ccw;
        // Should NOT compile (currently): bg::assign(ring_ccw, ring);

    }

    typename boost::range_const_iterator<ring_type>::type it = ring.begin();
    BOOST_CHECK_CLOSE(bg::get<0>(*it), 1.0, 0.001);
    BOOST_CHECK_CLOSE(bg::get<1>(*it), 2.0, 0.001);
    it++;
    BOOST_CHECK_CLOSE(bg::get<0>(*it), 1.0, 0.001);
    BOOST_CHECK_CLOSE(bg::get<1>(*it), 4.0, 0.001);
    it++;
    BOOST_CHECK_CLOSE(bg::get<0>(*it), 3.0, 0.001);
    BOOST_CHECK_CLOSE(bg::get<1>(*it), 4.0, 0.001);
    it++;
    BOOST_CHECK_CLOSE(bg::get<0>(*it), 3.0, 0.001);
    BOOST_CHECK_CLOSE(bg::get<1>(*it), 2.0, 0.001);
    it++;
    BOOST_CHECK_CLOSE(bg::get<0>(*it), 1.0, 0.001);
    BOOST_CHECK_CLOSE(bg::get<1>(*it), 2.0, 0.001);

    BOOST_CHECK_EQUAL(ring.size(), 5u);

    polygon_type polygon;
    boost::variant<polygon_type&> variant_polygon(polygon);

    bg::assign(variant_polygon, boost::variant<ring_type>(ring));
    BOOST_CHECK_EQUAL(bg::num_points(polygon), 5u);

    ring_type ring2;
    boost::variant<ring_type&> variant_ring2(ring2);
    bg::assign(variant_ring2, boost::variant<polygon_type>(polygon));
    BOOST_CHECK_EQUAL(bg::num_points(ring2), 5u);
}

template <typename Point>
void test_assign_point_2d()
{
    Point p;
    bg::assign_values(p, 1, 2);
    check_geometry(p, "POINT(1 2)");

    bg::assign_value(p, 123);
    check_geometry(p, "POINT(123 123)");

    bg::assign_zero(p);
    check_geometry(p, "POINT(0 0)");
}

int test_main(int, char* [])
{
    test_assign_point_3d<int[3]>();
    test_assign_point_3d<float[3]>();
    test_assign_point_3d<double[3]>();
    test_assign_point_3d<test::test_point>();
    test_assign_point_3d<bg::model::point<int, 3, bg::cs::cartesian> >();
    test_assign_point_3d<bg::model::point<float, 3, bg::cs::cartesian> >();
    test_assign_point_3d<bg::model::point<double, 3, bg::cs::cartesian> >();

    test_assign_point_2d<int[2]>();
    test_assign_point_2d<float[2]>();
    test_assign_point_2d<double[2]>();
    test_assign_point_2d<bg::model::point<int, 2, bg::cs::cartesian> >();
    test_assign_point_2d<bg::model::point<float, 2, bg::cs::cartesian> >();
    test_assign_point_2d<bg::model::point<double, 2, bg::cs::cartesian> >();

    test_assign_conversion<bg::model::point<double, 2, bg::cs::cartesian> >();
    test_assign_conversion_variant<bg::model::point<double, 2, bg::cs::cartesian> >();

    // Segment (currently) cannot handle array's because derived from std::pair
    test_assign_box_2d<int[2]>();
    test_assign_box_2d<float[2]>();
    test_assign_box_2d<double[2]>();

    test_assign_box_or_segment_2d<bg::model::point<int, 2, bg::cs::cartesian> >();
    test_assign_box_or_segment_2d<bg::model::point<float, 2, bg::cs::cartesian> >();
    test_assign_box_or_segment_2d<bg::model::point<double, 2, bg::cs::cartesian> >();

    test_assign_linestring_2d<bg::model::point<int, 2, bg::cs::cartesian> >();
    test_assign_linestring_2d<bg::model::point<float, 2, bg::cs::cartesian> >();
    test_assign_linestring_2d<bg::model::point<double, 2, bg::cs::cartesian> >();

#ifdef HAVE_TTMATH
    // Next 3 need extra traits for c-array with custom type:
    // test_assign_point_2d<ttmath_big[2]>();
    // test_assign_point_3d<ttmath_big[3]>();
    // test_assign_box_2d<ttmath_big[2]>();

    test_assign_point_2d<bg::model::point<ttmath_big, 2, bg::cs::cartesian> >();
    test_assign_point_3d<bg::model::point<ttmath_big, 3, bg::cs::cartesian> >();
    test_assign_box_or_segment_2d<bg::model::point<ttmath_big, 2, bg::cs::cartesian> >();
    test_assign_linestring_2d<bg::model::point<ttmath_big, 2, bg::cs::cartesian> >();
#endif

    return 0;
}


#include <boost/compute/algorithm/set_intersection.hpp>


/* vim: set tabstop=4 expandtab shiftwidth=4 softtabstop=4: */

/**
 * \file libs/numeric/ublasx/test/matrix_diagonal_proxy.cpp
 *
 * \brief Test suite for the matrix diagonal proxy.
 *
 * \author Marco Guazzone (marco.guazzone@gmail.com)
 *
 * <hr/>
 *
 * Copyright (c) 2010, Marco Guazzone
 *
 * Distributed under the Boost Software License, Version 1.0. (See
 * accompwhiching file LICENSE_1_0.txt or copy at
 * http://www.boost.org/LICENSE_1_0.txt)
 */

#include <boost/numeric/ublas/functional.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/matrix_expression.hpp>
#include <boost/numeric/ublasx/detail/debug.hpp>
#include <boost/numeric/ublasx/operation/size.hpp>
#include <boost/numeric/ublasx/proxy/matrix_diagonal.hpp>
#include <cstddef>
#include <iostream>
#include "libs/numeric/ublasx/test/utils.hpp"

const double tol = 1e-5;

namespace ublas = boost::numeric::ublas;
namespace ublasx = boost::numeric::ublasx;

BOOST_UBLASX_TEST_DEF( test_square_matrix_main_diag )
{
    BOOST_UBLASX_DEBUG_TRACE( "Test Case: Square Matrix -- Main Diagonal" );

    typedef double value_type;
    typedef ublas::matrix<value_type> matrix_type;

    const std::size_t n(5);

    matrix_type A(n,n);
    A(0,0) =  1; A(0,1) =  2; A(0,2) =  3; A(0,3) =  4; A(0,4) =  5;
    A(1,0) =  6; A(1,1) =  7; A(1,2) =  8; A(1,3) =  9; A(1,4) = 10;
    A(2,0) = 11; A(2,1) = 12; A(2,2) = 13; A(2,3) = 14; A(2,4) = 15;
    A(3,0) = 16; A(3,1) = 17; A(3,2) = 18; A(3,3) = 19; A(3,4) = 20;
    A(4,0) = 21; A(4,1) = 22; A(4,2) = 23; A(4,3) = 24; A(4,4) = 25;

    ublasx::matrix_diagonal<matrix_type> D(A, 0);

    BOOST_UBLASX_DEBUG_TRACE( "A = " << A );
    BOOST_UBLASX_DEBUG_TRACE( "D = " << D );

    BOOST_UBLASX_TEST_CHECK( ublasx::size(D) == n );
    for (std::size_t i = 0; i < n; ++i)
    {
        BOOST_UBLASX_TEST_CHECK_CLOSE( D(i), A(i,i), tol );
    }
}

BOOST_UBLASX_TEST_DEF( test_trans_matrix_expr_main_diag )
{
    BOOST_UBLASX_DEBUG_TRACE( "Test Case: Matrix Expression -- Transpose -- Main Diagonal" );

    typedef double value_type;
    typedef ublas::matrix<value_type> aux_matrix_type;
    typedef ublas::matrix_unary2_traits<
                aux_matrix_type,
                ublas::scalar_identity<value_type>
            >::result_type matrix_type;

    const std::size_t n(5);

    aux_matrix_type A(n,n);
    A(0,0) =  1; A(0,1) =  2; A(0,2) =  3; A(0,3) =  4; A(0,4) =  5;
    A(1,0) =  6; A(1,1) =  7; A(1,2) =  8; A(1,3) =  9; A(1,4) = 10;
    A(2,0) = 11; A(2,1) = 12; A(2,2) = 13; A(2,3) = 14; A(2,4) = 15;
    A(3,0) = 16; A(3,1) = 17; A(3,2) = 18; A(3,3) = 19; A(3,4) = 20;
    A(4,0) = 21; A(4,1) = 22; A(4,2) = 23; A(4,3) = 24; A(4,4) = 25;

    ublasx::matrix_diagonal<matrix_type const> D(ublas::trans(A), 0);
    BOOST_UBLASX_DEBUG_TRACE( "A = " << A );
    BOOST_UBLASX_DEBUG_TRACE( "A' = " << ublas::trans(A) );
    BOOST_UBLASX_DEBUG_TRACE( "D = " << D );
    BOOST_UBLASX_TEST_CHECK( ublasx::size(D) == n );
    for (std::size_t i = 0; i < n; ++i)
    {
        BOOST_UBLASX_TEST_CHECK_CLOSE( D(i), A(i,i), tol );
    }
}

BOOST_UBLASX_TEST_DEF( test_uminus_matrix_expr_main_diag )
{
    BOOST_UBLASX_DEBUG_TRACE( "Test Case: Matrix Expression -- Unary Minus -- Main Diagonal" );

    typedef double value_type;
    typedef ublas::matrix<value_type> aux_matrix_type;
    typedef ublas::matrix_unary1_traits<
                aux_matrix_type,
                ublas::scalar_negate<value_type>
            >::result_type matrix_type;

    const std::size_t n(5);

    aux_matrix_type A(n,n);
    A(0,0) =  1; A(0,1) =  2; A(0,2) =  3; A(0,3) =  4; A(0,4) =  5;
    A(1,0) =  6; A(1,1) =  7; A(1,2) =  8; A(1,3) =  9; A(1,4) = 10;
    A(2,0) = 11; A(2,1) = 12; A(2,2) = 13; A(2,3) = 14; A(2,4) = 15;
    A(3,0) = 16; A(3,1) = 17; A(3,2) = 18; A(3,3) = 19; A(3,4) = 20;
    A(4,0) = 21; A(4,1) = 22; A(4,2) = 23; A(4,3) = 24; A(4,4) = 25;

    ublasx::matrix_diagonal<matrix_type const> D(-A, 0);
    BOOST_UBLASX_DEBUG_TRACE( "A = " << A );
    BOOST_UBLASX_DEBUG_TRACE( "-A = " << -A );
    BOOST_UBLASX_DEBUG_TRACE( "D = " << D );
    BOOST_UBLASX_TEST_CHECK( ublasx::size(D) == n );
    for (std::size_t i = 0; i < n; ++i)
    {
        BOOST_UBLASX_TEST_CHECK_CLOSE( D(i), -A(i,i), tol );
    }
}

int main()
{
    BOOST_UBLASX_TEST_BEGIN();

    BOOST_UBLASX_TEST_DO( test_square_matrix_main_diag );
    BOOST_UBLASX_TEST_DO( test_trans_matrix_expr_main_diag );
    BOOST_UBLASX_TEST_DO( test_uminus_matrix_expr_main_diag );

    BOOST_UBLASX_TEST_END();
}


#ifndef EDP_CONSTRUCTSPARSEMAT_HPP
#define EDP_CONSTRUCTSPARSEMAT_HPP
#include <Eigen/Dense>
#include <Eigen/Sparse>
namespace edp
{
	template<typename T, class ColFunc>
	auto constructMat(uint64_t dim, ColFunc&& colFunc)
	-> Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>
	{
		Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> res(dim, dim);
		res.setZero();

		for(uint64_t i = 0; i < dim; i++)
		{
			auto m = colFunc(i);
			for(auto& elt: m)
			{
				res(elt.first, i) = elt.second;
			}
		}
		return res;
	}

	template<typename T, class ColFunc>
	auto constructSparseMat(uint64_t dim, ColFunc&& colFunc)
	-> Eigen::SparseMatrix<T>
	{
		using TripletT = Eigen::Triplet<T>;
		std::vector<TripletT> tripletList;
		tripletList.reserve(3*dim);
		for(uint64_t col = 0; col < dim; ++col)
		{
			auto m = colFunc(col);
			for(const auto& v: m)
			{
				tripletList.emplace_back(v.first, col, v.second);
			}
		}

		Eigen::SparseMatrix<T> res(dim, dim);
		res.setFromTriplets(tripletList.begin(), tripletList.end());
		return res;
	}

	//basis must be sorted
	template<typename T, typename ColFunc>
	auto constructSubspaceMat(ColFunc&& t, const std::vector<uint32_t>& basis)
	-> Eigen::SparseMatrix<T>
	{
		const int n = basis.size();

		using TripletT = Eigen::Triplet<T>;
		std::vector<TripletT> tripletList;
		for(int i = 0; i < n; i++)
		{
			std::map<uint32_t, T> m = t(basis[i]);
			auto iter = basis.begin();
			for(auto& kv: m)
			{
				iter = std::lower_bound(iter, basis.end(), kv.first);
				if(iter == basis.end())
					break;
				auto j = std::distance(basis.begin(), iter);
				{
					tripletList.emplace_back(i, j, kv.second);
				}
			}
		}

		Eigen::SparseMatrix<T> res(n, n);
		res.setFromTriplets(tripletList.begin(), tripletList.end());
		return res;
	}

}

#endif//EDP_CONSTRUCTSPARSEMAT_HPP


//==============================================================================
//         Copyright 2003 - 2013   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2013   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#include <nt2/include/functions/expi.hpp>

#include <boost/dispatch/functor/meta/call.hpp>
#include <nt2/sdk/functor/meta/call.hpp>
#include <nt2/sdk/unit/tests/relation.hpp>
#include <nt2/sdk/unit/tests/type_expr.hpp>
#include <boost/simd/sdk/simd/native.hpp>
#include <boost/simd/sdk/simd/io.hpp>
#include <complex>
#include <nt2/sdk/complex/complex.hpp>
#include <nt2/sdk/unit/tests/ulp.hpp>
#include <nt2/sdk/unit/module.hpp>
#include <boost/simd/sdk/config.hpp>

#include <nt2/include/constants/mone.hpp>
#include <nt2/include/constants/one.hpp>
#include <nt2/include/constants/pi.hpp>
#include <nt2/include/constants/zero.hpp>
#include <nt2/include/constants/inf.hpp>
#include <nt2/include/constants/minf.hpp>
#include <nt2/include/constants/nan.hpp>
#include <nt2/include/constants/i.hpp>
#include <nt2/include/functions/mul_i.hpp>
#include <nt2/include/functions/exp.hpp>

NT2_TEST_CASE_TPL ( expi_real,  NT2_SIMD_REAL_TYPES)
{
  using nt2::expi;
  using nt2::tag::expi_;
  typedef typename std::complex<T> cT;
  using boost::simd::native;
  typedef BOOST_SIMD_DEFAULT_EXTENSION  ext_t;
  typedef native<T,ext_t>                  vT;
  typedef native<cT,ext_t>                vcT;
  NT2_TEST_TYPE_IS( typename nt2::meta::call<expi_(vcT)>::type, vcT );

  // specific values tests
#ifndef BOOST_SIMD_NO_INVALIDS
  NT2_TEST_ULP_EQUAL(nt2::expi(nt2::Inf<vcT>())   , nt2::exp(nt2::mul_i(nt2::Inf<vcT>()))  , 1);
  NT2_TEST_ULP_EQUAL(nt2::expi(nt2::Minf<vcT>())  , nt2::exp(nt2::mul_i(nt2::Minf<vcT>()) ), 1);
  NT2_TEST_ULP_EQUAL(nt2::expi(nt2::Nan<vcT>())   , nt2::exp(nt2::mul_i(nt2::Nan<vcT>())  ), 1);
  NT2_TEST_ULP_EQUAL(nt2::expi(nt2::Inf<vT>())    , nt2::exp(nt2::mul_i(nt2::Inf<vcT>()))  , 1);
  NT2_TEST_ULP_EQUAL(nt2::expi((nt2::Minf<vT>())) , nt2::exp(nt2::mul_i(nt2::Minf<vcT>()) ), 1);
  NT2_TEST_ULP_EQUAL(nt2::expi((nt2::Nan<vT>()))  , nt2::exp(nt2::mul_i(nt2::Nan<vcT>())  ), 1);
  NT2_TEST_ULP_EQUAL(nt2::expi(nt2::mul_i(nt2::Inf<vT>()))  , nt2::Zero<vcT>()  , 1);
  NT2_TEST_ULP_EQUAL(nt2::expi(nt2::mul_i(nt2::Minf<vT>())) , nt2::Inf<vcT>() , 1);
  NT2_TEST_ULP_EQUAL(nt2::expi(nt2::mul_i(nt2::Nan<vT>()))  , nt2::Nan<vcT>(), 1);
#endif
  NT2_TEST_ULP_EQUAL(nt2::expi(nt2::Mone<vcT>()) , nt2::exp(nt2::mul_i(vcT(nt2::Mone<vT>())) ), 1);
  NT2_TEST_ULP_EQUAL(nt2::expi(nt2::One<vcT>())  , nt2::exp(nt2::mul_i(vcT(nt2::One<vT>()))  ), 1);
  NT2_TEST_ULP_EQUAL(nt2::expi(nt2::Zero<vcT>()) , nt2::exp(nt2::mul_i(vcT(nt2::Zero<vT>())) ), 1);
  NT2_TEST_ULP_EQUAL(nt2::expi(nt2::Pi<vT>())    , nt2::exp(nt2::mul_i(nt2::Pi<vcT>())), 1);
  NT2_TEST_ULP_EQUAL(nt2::expi(nt2::Mone<vT>())  , nt2::exp(nt2::mul_i(vcT(nt2::Mone<vT>())) ), 1);
  NT2_TEST_ULP_EQUAL(nt2::expi(nt2::One<vT>())   , nt2::exp(nt2::mul_i(vcT(nt2::One<vT>()))  ), 1);
  NT2_TEST_ULP_EQUAL(nt2::expi(nt2::Zero<vT>())  , nt2::exp(nt2::mul_i(vcT(nt2::Zero<vT>())) ), 1);
  NT2_TEST_ULP_EQUAL(nt2::expi(nt2::Pi<vT>())    , nt2::exp(nt2::mul_i(vcT(nt2::Pi<vT>()))), 1);
  NT2_TEST_ULP_EQUAL(nt2::expi(nt2::mul_i(nt2::Mone<vT>())) , nt2::exp( nt2::One<vcT>()) , 1);
  NT2_TEST_ULP_EQUAL(nt2::expi(nt2::mul_i(nt2::One<vT>()))  , nt2::exp( nt2::Mone<vcT>()) , 1);
  NT2_TEST_ULP_EQUAL(nt2::expi(nt2::mul_i(nt2::Zero<vT>())) , nt2::exp( nt2::Zero<vcT>()), 1);
  NT2_TEST_ULP_EQUAL(nt2::expi(nt2::mul_i(nt2::Pi<vT>()))   , nt2::exp( -nt2::Pi<vcT>()), 1);
}


#include <boost/test/unit_test.hpp>
#include "algorithms/dp/binary_watch_time.hpp"

BOOST_AUTO_TEST_SUITE(TestBinaryWatchTime)

    BOOST_AUTO_TEST_CASE(test_get_possible_time)
    {
        {
            const int num_led_on = 0;
            const std::vector<std::string> expected = {"0:00"};
            BOOST_CHECK(expected == Algo::DP::BinaryWatch::get_time(num_led_on));
        }

        {
            const int num_led_on = 1;
            auto result = Algo::DP::BinaryWatch::get_time(num_led_on);
            std::vector<std::string> expected = {
                "1:00", "2:00", "4:00", "8:00", "0:01", "0:02", "0:04", "0:08", "0:16", "0:32"};

            std::sort(result.begin(), result.end());
            std::sort(expected.begin(), expected.end());

            BOOST_CHECK(expected == result);
        }

        {
            const int num_led_on = -1;
            const std::vector<std::string> expected = {};
            BOOST_CHECK(expected == Algo::DP::BinaryWatch::get_time(num_led_on));
        }

        {
            const int num_led_on = 9;
            const std::vector<std::string> expected = {};
            BOOST_CHECK(expected == Algo::DP::BinaryWatch::get_time(num_led_on));
        }

        {
            const int num_led_on = 11;
            const std::vector<std::string> expected = {};
            BOOST_CHECK(expected == Algo::DP::BinaryWatch::get_time(num_led_on));
        }
    }

BOOST_AUTO_TEST_SUITE_END()


/*
Part of the Fluid Corpus Manipulation Project (http://www.flucoma.org/)
Copyright 2017-2019 University of Huddersfield.
Licensed under the BSD-3 License.
See license.md file in the project root for full license information.
This project has received funding from the European Research Council (ERC)
under the European Union’s Horizon 2020 research and innovation programme
(grant agreement No 725899).
*/

#pragma once

#include "../../data/FluidTensor.hpp"
#include <Eigen/Core>
#include <algorithm>

/// converting between FluidTensorView and Eigen wrappers around raw poiniters

namespace fluid {
namespace algorithm {
namespace _impl {

using Eigen::Map;
using Eigen::PlainObjectBase;
using Eigen::Stride;

using Eigen::AlignmentType;
using Eigen::ColMajor;
using Eigen::Dynamic;
using Eigen::RowMajor;

/// Eigen Matrix<T>/Array<T> -> FluidTensorView<T>
template <typename Derived>
auto asFluid(PlainObjectBase<Derived>& a)
    -> FluidTensorView<typename PlainObjectBase<Derived>::Scalar,
                       (PlainObjectBase<Derived>::IsVectorAtCompileTime ? 1
                                                                        : 2)>
{
  constexpr size_t N = PlainObjectBase<Derived>::IsVectorAtCompileTime ? 1 : 2;

  if (N == 2)
  {
    if (static_cast<int>(a.Options) == static_cast<int>(ColMajor))
    {
      // Respect the colmajorness of an eigen type
      auto slice = FluidTensorSlice<N>(0, {a.cols(), a.rows()});
      return {slice.transpose(), a.data()};
    }
    return {a.data(), 0, a.rows(), a.cols()};
  }
  else
    return {a.data(), 0, a.rows()};
}

/// const Eigen::Matrix/Array -> FluidTensorView<const T>
template <typename Derived>
auto asFluid(const PlainObjectBase<Derived>& a)
    -> FluidTensorView<const typename PlainObjectBase<Derived>::Scalar,
                       (PlainObjectBase<Derived>::IsVectorAtCompileTime ? 1
                                                                        : 2)>
{
  constexpr size_t N = PlainObjectBase<Derived>::IsVectorAtCompileTime ? 1 : 2;

  if (N == 2)
  {
    if (a.Options == ColMajor)
    {
      // Respect the colmajorness of an eigen type
      auto slice = FluidTensorSlice<N>(0, {a.cols(), a.rows()});
      return {slice.transpose(), a.data()};
    }
    return {a.data(), 0, a.rows(), a.cols()};
  }
  else
    return {a.data(), 0, a.rows()};
}

/// lvalue FluidTensor<T> / FluidTensorView<T> -> Matrix/Array<T> (say which as
/// template param) e.g. asEigen<Matrix>(myView)

template <template <typename, int, int, int, int, int> class EigenType,
          typename T, size_t N>
auto asEigen(FluidTensor<T, N>& a)
    -> Map<EigenType<T, Dynamic, Dynamic, RowMajor, Dynamic, Dynamic>,
           Eigen::AlignmentType::Unaligned, Stride<Dynamic, Dynamic>>
{
  static_assert(N < 3,
                "Can't convert to Eigen types with more than two dimensions");

  if (N == 2)
  {
    return {a.data(), static_cast<Eigen::Index>(a.rows()),
            static_cast<Eigen::Index>(a.cols()),
            Stride<Dynamic, Dynamic>(a.descriptor().strides[0],
                                     a.descriptor().strides[1])};
  }
  else
  {
    return {a.data(), static_cast<Eigen::Index>(a.rows()), 1,
            Stride<Dynamic, Dynamic>(a.descriptor().strides[0], 1)};
  }
}

/// lvalue const FluidTensor<T> -> const Matrix<T>/ const Array<T>
template <template <typename, int, int, int, int, int> class EigenType,
          typename T, size_t N>
auto asEigen(const FluidTensor<T, N>& a)
    -> Map<const EigenType<T, Dynamic, Dynamic, RowMajor, Dynamic, Dynamic>,
           Eigen::AlignmentType::Unaligned, Stride<Dynamic, Dynamic>>
{
  static_assert(N < 3,
                "Can't convert to Eigen types with more than two dimensions");

  if (N == 2)
  {
    return {a.data(), static_cast<Eigen::Index>(a.rows()),
            static_cast<Eigen::Index>(a.cols()),
            Stride<Dynamic, Dynamic>(a.descriptor().strides[0],
                                     a.descriptor().strides[1])};
  }
  else
  {
    return {a.data(), static_cast<Eigen::Index>(a.rows()), 1,
            Stride<Dynamic, Dynamic>(a.descriptor().strides[0], 1)};
  }
}

/// lvalue FluidTensorView<T> ->  Matrix / Array
template <template <typename, int, int, int, int, int> class EigenType,
          typename T, size_t N>
auto asEigen(const FluidTensorView<T, N>& a) -> Map<
    EigenType<std::decay_t<T>, Dynamic, Dynamic, RowMajor, Dynamic, Dynamic>,
    Eigen::AlignmentType::Unaligned, Stride<Dynamic, Dynamic>>
{
  static_assert(N < 3,
                "Can't convert to Eigen types with more than two dimensions");

  if (N == 2)
  {
    return {a.data(), static_cast<Eigen::Index>(a.rows()),
            static_cast<Eigen::Index>(a.cols()),
            Stride<Dynamic, Dynamic>(a.descriptor().strides[0],
                                     a.descriptor().strides[1])};
  }
  else
  {
    return {a.data(), static_cast<Eigen::Index>(a.rows()), 1,
            Stride<Dynamic, Dynamic>(a.descriptor().strides[0], 1)};
  }
}

/// lvalue FluidTensorView<const T> ->  const Matrix / Array
template <template <typename, int, int, int, int, int> class EigenType,
          typename T, size_t N>
auto asEigen(const FluidTensorView<const T, N>& a)
    -> Map<const EigenType<std::decay_t<T>, Dynamic, Dynamic, RowMajor, Dynamic,
                           Dynamic>,
           Eigen::AlignmentType::Unaligned, Stride<Dynamic, Dynamic>>
{
  static_assert(N < 3,
                "Can't convert to Eigen types with more than two dimensions");

  if (N == 2)
  {
    return {a.data(), static_cast<Eigen::Index>(a.rows()),
            static_cast<Eigen::Index>(a.cols()),
            Stride<Dynamic, Dynamic>(a.descriptor().strides[0],
                                     a.descriptor().strides[1])};
  }
  else
  {
    return {a.data(), static_cast<Eigen::Index>(a.rows()), 1,
            Stride<Dynamic, Dynamic>(a.descriptor().strides[0], 1)};
  }
}

/// rvalue FluidTensorView<T> ->  Matrix / Array
template <template <typename, int, int, int, int, int> class EigenType,
          typename T, size_t N>
auto asEigen(const FluidTensorView<T, N>&&
                 a) // restrict this to FluidTensorView because
                    // passing in an rvalue FluidTensor would be silly
    -> Map<EigenType<T, Dynamic, Dynamic, RowMajor, Dynamic, Dynamic>,
           Eigen::AlignmentType::Unaligned, Stride<Dynamic, Dynamic>>
{
  return asEigen<EigenType>(a);
}

/// rvalue FluidTensorView<const T> ->  const Matrix / Array
template <template <typename, int, int, int, int, int> class EigenType,
          typename T, size_t N>
auto asEigen(const FluidTensorView<const T, N>&&
                 a) // restrict this to FluidTensorView because passing in an
                    // rvalue FluidTensor would be silly
    -> Map<const EigenType<T, Dynamic, Dynamic, RowMajor, Dynamic, Dynamic>,
           Eigen::AlignmentType::Unaligned, Stride<Dynamic, Dynamic>>
{
  return asEigen<EigenType>(a);
}

} // namespace _impl
} // namespace algorithm
} // namespace fluid


// Copyright (C) 2015 National ICT Australia (NICTA)
//
// This Source Code Form is subject to the terms of the Mozilla Public
// License, v. 2.0. If a copy of the MPL was not distributed with this
// file, You can obtain one at http://mozilla.org/MPL/2.0/.
// -------------------------------------------------------------------
//
// Written by Conrad Sanderson - http://conradsanderson.id.au

#include <armadillo>
#include "catch.hpp"

using namespace arma;

TEST_CASE("attributes_1")
  {
  mat A(5,6);
  REQUIRE(A.n_rows == 5);
  REQUIRE(A.n_cols == 6);
  REQUIRE(A.n_elem == 30);

  vec B(5);
  REQUIRE(B.n_rows == 5);
  REQUIRE(B.n_cols == 1);
  REQUIRE(B.n_elem == 5);

  rowvec C(6);
  REQUIRE(C.n_rows == 1);
  REQUIRE(C.n_cols == 6);
  REQUIRE(C.n_elem == 6);

  cube D(5,6,2);
  REQUIRE(D.n_rows   == 5);
  REQUIRE(D.n_cols   == 6);
  REQUIRE(D.n_slices == 2);
  REQUIRE(D.n_elem   == 60);

  sp_mat E(50,60);
  E(0,0) = 1.0;
  E(E.n_rows-1,E.n_cols-1) = 1.0;

  REQUIRE(E.n_rows    == 50);
  REQUIRE(E.n_cols    == 60);
  REQUIRE(E.n_elem    == 3000);
  REQUIRE(E.n_nonzero == 2);

  field<mat> G(5,6,2);
  REQUIRE(G.n_rows   == 5);
  REQUIRE(G.n_cols   == 6);
  REQUIRE(G.n_slices == 2);
  REQUIRE(G.n_elem   == 60);
  }


#ifndef TCM_MATRIX_HPP
#define TCM_MATRIX_HPP

#include <cassert>
#include <complex>
#include <memory>
#include <type_traits>
#include <algorithm>

#include <boost/align/aligned_allocator_adaptor.hpp>

#include <detail/utils.hpp>
#include <detail/iterator.hpp>

///////////////////////////////////////////////////////////////////////////////
/// \file include/matrix.hpp
/// \brief This file implements a Matrix class and some operations associated
///        with it.
///////////////////////////////////////////////////////////////////////////////

namespace tcm {

///////////////////////////////////////////////////////////////////////////////
/// \brief A simple wrapper around the (data, ldim, width) representation
///        of matrices used in LAPACK. Uses column-major ordering.

/// This is a _container_ class in the sense that it manages its own memory.
/// Matrix class is meant to be used with LAPACK/BLAS and thus focuses on
/// fundamental numeric types. There are two important differences
/// between this class and, for example, an %std::vector.
/// 1) As we plan to use matrices with LAPACK/BLAS routines, it helps
///    (from the performance point of view) to correctly align our matrices.
///    From Intel MKL's manual:
///    > To improve performance of your application that calls Intel MKL,
///    > align your arrays on 64-byte boundaries and ensure that the leading
///    > dimensions of the arrays are divisible by 64/element_size, where
///    > element_size is the number of bytes for the matrix elements
///    > (4 for single-precision real, 8 for double-precision real and
///    > single-precision complex, and 16 for double-precision complex).
/// 2) We don't initialise storage.
///
/// \tparam _Tp Element type of the matrix.
/// \tparam _Align Alignment. Must be a power of 2. This argument is used
///                for two things:
///                1) Allocation of storage that is aligned at least to _Alloc.
///                2) Computing the leading dimension of the matrix to ensure
///                   that `this->data(0, 1)` is again aligned to _Alloc.
///
///                We use %boost::alignment::aligned_allocator_adaptor
///                for this.
///
///////////////////////////////////////////////////////////////////////////////
template< class _Tp
        , std::size_t _Align = 64 // std::alignment_of<_Tp>::value
        , class _Alloc = std::allocator<_Tp>
        >
class Matrix {

private:
	using _Aligned_Alloc = boost::alignment::
		aligned_allocator_adaptor<_Alloc, _Align>;
	using _Storage_type = utils::_Storage<_Tp, _Aligned_Alloc>;

	static_assert( (_Align != 0) and ((_Align & (_Align - 1)) == 0)
	             , "_Align must be a power of 2" );
	static_assert( _Align % std::alignment_of<_Tp>::value == 0
	             , "Invalid alignment." );

public:
	using value_type        = _Tp;
	using reference         = typename _Storage_type::reference;
	using const_reference   = typename _Storage_type::const_reference;
	using pointer           = typename _Storage_type::pointer;
	using const_pointer     = typename _Storage_type::const_pointer;
	using size_type         = typename _Storage_type::size_type;
	using difference_type   = typename _Storage_type::difference_type;
	using allocator_type    = typename _Storage_type::allocator_type;

private:
	size_type       _height;
	size_type       _width;
	size_type       _ldim;
	_Storage_type   _storage;

	static constexpr auto round_up(size_type const n) -> size_type
	{
		constexpr auto multiple = _Align / std::alignment_of<_Tp>::value;
		return ((n + multiple - 1) / multiple) * multiple;
	}

public:

	///////////////////////////////////////////////////////////////////////////
	/// \brief Default constructor.
	///////////////////////////////////////////////////////////////////////////
	Matrix()
		noexcept(std::is_nothrow_default_constructible<_Storage_type>::value)
		: _height{ 0 }
		, _width{ 0 }
		, _ldim{ 0 }
	    , _storage{}
	{
	}

	///////////////////////////////////////////////////////////////////////////
	/// \brief Move constructor.
	///////////////////////////////////////////////////////////////////////////
	Matrix(Matrix && x)
		noexcept(std::is_nothrow_move_constructible<_Storage_type>::value)
		: _height{ 0 }
		, _width{ 0 }
		, _ldim{ 0 }
	    , _storage{ std::move(x._storage) }
	{
		using std::swap;
		swap(_height, x._height);
		swap(_width, x._width);
		swap(_ldim, x._ldim);
	}

	///////////////////////////////////////////////////////////////////////////
	/// \brief Copy constructor.
	///////////////////////////////////////////////////////////////////////////
	Matrix(Matrix const& x)
		noexcept(std::is_nothrow_copy_constructible<_Storage_type>::value)
		: _height{ x._height }
		, _width{ x._width }
		, _ldim{ x._ldim }
	    , _storage{ x._storage }
	{
	}

	///////////////////////////////////////////////////////////////////////////
	/// \brief Constructs a Matrix of given dimensions.

	/// \param height Height of the matrix, i.e. number of rows.
	/// \param width  Width of the matrix, i.e. number of columns.
	///
	/// \sa Matrix(size_type const, size_type const, difference_type const).
	///////////////////////////////////////////////////////////////////////////
	Matrix(size_type const height, size_type const width)
		: _height{ height }
		, _width{ width }
		// , _ldim{ height }
		, _ldim{ round_up(height) }
		, _storage{ round_up(height) * width }
	{
	}

	///////////////////////////////////////////////////////////////////////////
	/// \brief Destructor.
	///////////////////////////////////////////////////////////////////////////
	~Matrix()
	{
	}

	///////////////////////////////////////////////////////////////////////////
	/// Move assignment operator.

	/// No copy assignment operator is provided
	/// to make it less convenient to perform copies all the time.
	///////////////////////////////////////////////////////////////////////////
	auto operator=(Matrix && other) noexcept -> Matrix &
	{ swap(*this, other);
	  return *this;
	}

	///////////////////////////////////////////////////////////////////////////
	/// Returns the height of the matrix.

	/// \sa width(), ldim().
	///////////////////////////////////////////////////////////////////////////
	constexpr auto height() const noexcept -> size_type
	{ return _height; }

	///////////////////////////////////////////////////////////////////////////
	/// Returns the width of the matrix.

	/// \sa height(), ldim().
	///////////////////////////////////////////////////////////////////////////
	constexpr auto width() const noexcept -> size_type
	{ return _width; }

	///////////////////////////////////////////////////////////////////////////
	/// Returns the leading dimension of the matrix.

	/// \sa width(), height().
	///////////////////////////////////////////////////////////////////////////
	constexpr auto ldim() const noexcept -> size_type
	{ return _ldim; }

	///////////////////////////////////////////////////////////////////////////
	/// \brief Returns a const pointer to the underlying one-dimensional array
	/// of elements.

	/// \sa data()
	///////////////////////////////////////////////////////////////////////////
	constexpr auto data() const noexcept -> const_pointer
	{ return _storage.data(); }

	///////////////////////////////////////////////////////////////////////////
	/// \brief Returns a non-const pointer to the underlying one-dimensional
	/// array of elements.

	/// \sa data() const
	///////////////////////////////////////////////////////////////////////////
	constexpr auto data() noexcept -> pointer
	{ return _storage.data(); }

	///////////////////////////////////////////////////////////////////////////
	/// \brief Returns a const pointer to the element at row \p i and
	/// column \p j.

	/// \se data(std::size_t const, std::size_t const)
	///////////////////////////////////////////////////////////////////////////
	constexpr
	auto data( size_type const i
	         , size_type const j ) const noexcept -> const_pointer
	{ return data() + (i + ldim() * j); }

	///////////////////////////////////////////////////////////////////////////
	/// \brief Returns a non-const pointer to the element at row \p i and
	/// column \p j.

	/// \se data(std::size_t const, std::size_t const) const
	///////////////////////////////////////////////////////////////////////////
	constexpr
	auto data( size_type const i
	         , size_type const j ) noexcept -> pointer
	{ return data() + (i + ldim() * j); }

	///////////////////////////////////////////////////////////////////////////
	/// Returns the element at row \p i and column \p j.

	/// \warning Mind you: not a const-reference, by the element by value!
	/// This is due to the fact that this class is meant to be used with
	/// elementary number types such as `float` or `int`.
	///////////////////////////////////////////////////////////////////////////
	constexpr
	auto operator() ( size_type const i
	                , size_type const j ) const noexcept -> value_type
	{ assert(i < height() and j < width() and "Index out of bounds.");
	  return *data(i, j);
	}

	///////////////////////////////////////////////////////////////////////////
	/// Returns a non-const reference to the element at row \p i and
	/// column \p j.
	///////////////////////////////////////////////////////////////////////////
	constexpr
	auto operator() ( size_type const i
	                , size_type const j ) noexcept -> reference
	{ assert(i < height() and j < width() and "Index out of bounds.");
	  return *data(i, j);
	}

	///////////////////////////////////////////////////////////////////////////
	/// \brief Returns a constant row iterator to the beginning of row \p i.
	///////////////////////////////////////////////////////////////////////////
	constexpr auto cbegin_row(size_type const i) const noexcept
		-> const_blas_iterator<value_type>
	{ assert(i < height() and "Index out of bounds.");
	  return const_blas_iterator<value_type>{ data(i, 0),
	      static_cast<difference_type>(ldim()) };
	}

	///////////////////////////////////////////////////////////////////////////
	/// \brief Returns a mutating row iterator to the beginning of row \p i.
	///////////////////////////////////////////////////////////////////////////
	constexpr auto begin_row(size_type const i) noexcept
		-> blas_iterator<value_type>
	{ assert(i < height() and "Index out of bounds.");
	  return blas_iterator<value_type>{ data(i, 0),
	      static_cast<difference_type>(ldim()) };
	}

	///////////////////////////////////////////////////////////////////////////
	/// \brief Returns a constant row iterator to the end of row \p i.
	///////////////////////////////////////////////////////////////////////////
	constexpr auto cend_row(size_type const i) const noexcept
		-> const_blas_iterator<value_type>
	{ assert(i < height() and "Index out of bounds.");
	  return const_blas_iterator<value_type>{ data(i, width()),
	      static_cast<difference_type>(ldim()) };
	}

	///////////////////////////////////////////////////////////////////////////
	/// \brief Returns a mutating row iterator to the end of row \p i.
	///////////////////////////////////////////////////////////////////////////
	constexpr auto end_row(size_type const i) noexcept
		-> blas_iterator<value_type>
	{ assert(i < height() and "Index out of bounds.");
	  return blas_iterator<value_type>{ data(i, width()),
	      static_cast<difference_type>(ldim()) };
	}

	///////////////////////////////////////////////////////////////////////////
	/// \brief Returns a constant column iterator to the beginning of column
	/// \p j.
	///////////////////////////////////////////////////////////////////////////
	constexpr auto cbegin_column(size_type const j) const noexcept
		-> const_blas_iterator<value_type>
	{ assert(j < width() and "Index out of bounds.");
	  return {data(0, j), 1};
	}

	///////////////////////////////////////////////////////////////////////////
	/// \brief Returns a mutating column iterator to the beginning of column
	/// \p j.
	///////////////////////////////////////////////////////////////////////////
	constexpr auto begin_column(size_type const j) noexcept
		-> blas_iterator<value_type>
	{ assert(j < width() and "Index out of bounds.");
	  return blas_iterator<value_type>{data(0, j), 1};
	}

	///////////////////////////////////////////////////////////////////////////
	/// \brief Returns a constant column iterator to the end of column \p j.
	///////////////////////////////////////////////////////////////////////////
	constexpr auto cend_column(size_type const j) const noexcept
		-> const_blas_iterator<value_type>
	{ assert(j < width() and "Index out of bounds.");
	  return const_blas_iterator<value_type>{data(height(), j), 1};
	}

	///////////////////////////////////////////////////////////////////////////
	/// \brief Returns a mutating column iterator to the end of column \p j.
	///////////////////////////////////////////////////////////////////////////
	constexpr auto end_column(size_type const j) noexcept
		-> blas_iterator<value_type>
	{ assert(j < width() and "Index out of bounds.");
	  return blas_iterator<value_type>{data(height(), j), 1};
	}

private:
	friend
	auto swap(Matrix<value_type>& lhs, Matrix<value_type>& rhs) noexcept
	{ using std::swap;
	  swap(lhs._storage, rhs._storage);
	  swap(lhs._height, rhs._height);
	  swap(lhs._width, rhs._width);
	  swap(lhs._ldim, rhs._ldim);
	}
};

///////////////////////////////////////////////////////////////////////////////
/// Returns true if the given matrix is just a row vector.
///////////////////////////////////////////////////////////////////////////////
template <class _M>
constexpr
auto is_row(_M const& A) noexcept -> bool { return A.height() == 1; }

///////////////////////////////////////////////////////////////////////////////
/// Returns true if the given matrix is just a column vector.
///////////////////////////////////////////////////////////////////////////////
template <class _M>
constexpr
auto is_column(_M const& A) noexcept -> bool { return A.width() == 1; }

///////////////////////////////////////////////////////////////////////////////
/// Returns true if the given matrix is a square matrix.
///////////////////////////////////////////////////////////////////////////////
template <class _M>
constexpr
auto is_square(_M const& A) noexcept -> bool {return A.width() == A.height();}

template <class _T, std::size_t _Align, class _Alloc>
auto operator<< (std::ostream& out, Matrix<_T, _Align, _Alloc> const& A)
	-> std::ostream&
{
	for (std::size_t i = 0; i < A.height(); ++i) {
		for (std::size_t j = 0; j < A.width(); ++j)
			out << A(i, j) << '\t';
		out << '\n';
	}
	return out;
}

template<class _T, std::size_t _Align, class _Alloc>
auto operator>> (std::istream& in, Matrix<_T, _Align, _Alloc>& A)
	-> std::istream&
{
	for (std::size_t i = 0; i < A.height(); ++i) {
		for (std::size_t j = 0; j < A.width(); ++j) {
			in >> A(i, j);
		}
	}
	return in;
}

template<class Func>
auto build_matrix( std::size_t const height, std::size_t const width
                 , Func f )
{
	using T = decltype( f( std::declval<std::size_t>()
	                     , std::declval<std::size_t>()
	                     ) );
	Matrix<T> result{height, width};
	for (std::size_t j = 0; j < width; ++j)
		for (std::size_t i = 0; i < height; ++i)
			result(i, j) = f(i, j);
	return result;
}

} // namespace tcm

#endif // TCM_MATRIX_HPP


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef NT2_LINALG_FUNCTIONS_NORMEST_HPP_INCLUDED
#define NT2_LINALG_FUNCTIONS_NORMEST_HPP_INCLUDED
#include <nt2/include/functor.hpp>
#include <boost/simd/include/functor.hpp>
#include <boost/dispatch/include/functor.hpp>
#include <nt2/sdk/memory/container.hpp>

/*!
 * \ingroup algebra
 * \defgroup algebra_normest normest
 *
 * \par Description
 * norm estimate
 *
 * \par Header file
 *
 * \code
 * #include <nt2/include/functions/normest.hpp>
 * \endcode
 *
 *
 * \synopsis
 *
 * \param a the matrix or vector expression a
 *
 * \param type of normest required
 *
 **/

namespace nt2 { namespace tag
  {
    /*!
     * \brief Define the tag normest_ of functor normest
     *        in namespace nt2::tag for toolbox algebra
     **/
    struct normest_ :  tag::formal_ { typedef tag::formal_ parent; };
  }

  BOOST_DISPATCH_FUNCTION_IMPLEMENTATION(tag::normest_, normest, 2)
  BOOST_DISPATCH_FUNCTION_IMPLEMENTATION(tag::normest_, normest, 1)

}

#endif


#include <stan/math/prim/mat/fun/Eigen.hpp>
#include <stan/math/prim/meta.hpp>
#include <Eigen/Sparse>
#include <gtest/gtest.h>
#include <vector>

TEST(MathMetaPrim, primitive) {
  using stan::is_eigen;
  EXPECT_FALSE((is_eigen<bool>::value));
  EXPECT_FALSE((is_eigen<double>::value));
  EXPECT_FALSE((is_eigen<int>::value));

  EXPECT_FALSE((is_eigen<std::vector<double>>::value));

  EXPECT_TRUE((is_eigen<Eigen::EigenBase<Eigen::MatrixXd>>::value));
  EXPECT_TRUE((is_eigen<Eigen::Matrix<double, -1, -1>>::value));
  EXPECT_TRUE((is_eigen<Eigen::SparseMatrix<double>>::value));
  EXPECT_TRUE((is_eigen<Eigen::MatrixBase<Eigen::MatrixXd>>::value));

  EXPECT_TRUE((is_eigen<const Eigen::Matrix<double, -1, -1>>::value));
  EXPECT_TRUE((is_eigen<Eigen::SparseMatrix<double>&>::value));
  EXPECT_TRUE(
      (is_eigen<Eigen::MatrixBase<Eigen::Matrix<double, -1, -1>>&&>::value));

  Eigen::Matrix<double, -1, -1> a;
  Eigen::Matrix<double, -1, -1> b;

  EXPECT_TRUE((is_eigen<decltype(a * b)>::value));
  EXPECT_TRUE((is_eigen<decltype(a * b + a.transpose())>::value));
}

TEST(MathMetaPrim, expression) {
  using stan::is_eigen_matrix;
  EXPECT_FALSE((is_eigen_matrix<bool>::value));
  EXPECT_FALSE((is_eigen_matrix<double>::value));
  EXPECT_FALSE((is_eigen_matrix<int>::value));

  EXPECT_FALSE((is_eigen_matrix<std::vector<double>>::value));

  EXPECT_FALSE((is_eigen_matrix<Eigen::EigenBase<Eigen::MatrixXd>>::value));
  EXPECT_TRUE((is_eigen_matrix<Eigen::Matrix<double, -1, -1>>::value));
  Eigen::SparseMatrix<double> sparse_mat;
  EXPECT_TRUE((is_eigen_matrix<decltype(sparse_mat)>::value));
  EXPECT_FALSE((is_eigen_matrix<Eigen::MatrixBase<Eigen::MatrixXd>>::value));

  EXPECT_TRUE((is_eigen_matrix<const Eigen::Matrix<double, -1, -1>>::value));
  EXPECT_TRUE((is_eigen_matrix<Eigen::SparseMatrix<double>&>::value));
  EXPECT_FALSE((is_eigen_matrix<
                Eigen::MatrixBase<Eigen::Matrix<double, -1, -1>>&&>::value));

  Eigen::Matrix<double, -1, -1> a;
  Eigen::Matrix<double, -1, -1> b;

  EXPECT_FALSE((is_eigen_matrix<decltype(a * b)>::value));
  EXPECT_FALSE((is_eigen_matrix<decltype(a * b + a.transpose())>::value));
}


#ifndef _LDAPLUSPLUS_ESTEPUTILS_HPP_
#define _LDAPLUSPLUS_ESTEPUTILS_HPP_

#include <Eigen/Core>

namespace ldaplusplus {
namespace e_step_utils {

    template <typename Scalar>
    using MatrixX = Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic>;
    template <typename Scalar>
    using VectorX = Eigen::Matrix<Scalar, Eigen::Dynamic, 1>;
    using Eigen::VectorXi;
    using Eigen::Ref;

    /**
     * Compute the value of the ELBO (using the unsupervised definition of the
     * model) for a given document, model parameters and variational
     * parameters.
     */
    template <typename Scalar>
    Scalar compute_unsupervised_likelihood(
        const VectorXi & X,
        const VectorX<Scalar> &alpha,
        const MatrixX<Scalar> &beta,
        const MatrixX<Scalar> &phi,
        const VectorX<Scalar> &gamma
    );

    /**
     * Compute the value of the ELBO (using the supervised definition of the
     * model) for a given document, model parameters and variational
     * parameters.
     *
     * @param X       The word counts in column-major order for a single
     *                document
     * @param y       The class label as integer for the current document
     * @param alpha   The Dirichlet priors
     * @param beta    The over word topic distributiosn
     * @param eta     The classification parameters
     * @param phi     The Multinomial parameters
     * @param gamma   The Dirichlet parameters
     * @return        The log likelihood
     */
    template <typename Scalar>
    Scalar compute_supervised_likelihood(
        const VectorXi & X,
        int y,
        const VectorX<Scalar> &alpha,
        const MatrixX<Scalar> &beta,
        const MatrixX<Scalar> &eta,
        const MatrixX<Scalar> &phi,
        const VectorX<Scalar> &gamma
    );
    template <typename Scalar>
    Scalar compute_supervised_likelihood(
        const VectorXi & X,
        int y,
        const VectorX<Scalar> &alpha,
        const MatrixX<Scalar> &beta,
        const MatrixX<Scalar> &eta,
        const MatrixX<Scalar> &phi,
        const VectorX<Scalar> &gamma,
        const VectorX<Scalar> &h
    );

    template <typename Scalar>
    Scalar compute_supervised_multinomial_likelihood(
        const VectorXi & X,
        int y,
        const VectorX<Scalar> &alpha,
        const MatrixX<Scalar> &beta,
        const MatrixX<Scalar> &eta,
        const MatrixX<Scalar> &phi,
        const VectorX<Scalar> &gamma,
        Scalar prior_y,
        Scalar mu,
        Scalar portion
    );

    template <typename Scalar>
    Scalar compute_supervised_correspondence_likelihood(
        const VectorXi & X,
        int y,
        const VectorX<Scalar> &alpha,
        const MatrixX<Scalar> &beta,
        const MatrixX<Scalar> &eta,
        const MatrixX<Scalar> &phi,
        const VectorX<Scalar> &gamma,
        const VectorX<Scalar> &tau,
        Scalar mu,
        Scalar portion
    );

    /**
     * Compute the Matrix h as defined in [Wang, C., Blei, D. and Li, F.F.,
     * 2009, June. Simultaneous image classification and annotation. In
     * Computer Vision and Pattern Recognition, 2009. CVPR 2009. IEEE
     * Conference on (pp. 1903-1910). IEEE.]
     *
     * h \in \mathbb{R}^{K \times V}
     *
     * h_{n} = \sum_{y \in Y} \left(
     *      \prod_{l=1, l \neq n}^V \phi_l^T \left( exp(\frac{X_l}{\sum X} \eta^T y) \right)
     *  \right) exp(\frac{X_n}{\sum X} \eta^T y)
     *
     * @param X       The word counts in column-major order for a single
     *                document
     * @param eta     The classification parameters
     * @param phi     The Multinomial parameters
     * @param h       The output value
     */
    template <typename Scalar>
    void compute_h(
        const VectorXi & X,
        const VectorX<Scalar> & X_ratio,
        const MatrixX<Scalar> &eta,
        const MatrixX<Scalar> &phi,
        Ref<VectorX<Scalar> > h
    );

    template <typename Scalar>
    void compute_supervised_phi_gamma(
        const VectorXi & X,
        const VectorX<Scalar> & X_ratio,
        int y,
        const MatrixX<Scalar> & beta,
        const MatrixX<Scalar> & eta,
        size_t fixed_point_iterations,
        Ref<MatrixX<Scalar> > phi,
        Ref<VectorX<Scalar> > gamma,
        Ref<VectorX<Scalar> > h
    );

    /**
     * Compute the gamma based on the prior and the variational parameter phi.
     *
     * This operation is the same in both supervised and unsupervised LDA and
     * it can be summarised to:
     *
     *     gamma_i ^ {t+1} =  alpha_i + \sum_n \phi_{n,i}^{t+1}
     *
     * Equation (7) in Latent Dirichlet Allocation, Blei 2003
     */
    template <typename Scalar>
    void compute_gamma(
        const VectorXi & X,
        const VectorX<Scalar> & alpha,
        const MatrixX<Scalar> & phi,
        Ref<VectorX<Scalar> > gamma
    );

    /**
     * Update Multinomial parameter phi, according to the following
     * pseudocode
     *
     *     for n=1 to Nd do
     *      for i=1 to K do
     *          phi_{n,i}^{t+1} = beta_{i, w_n}exp(\psi(\gamma_i) - \psi(sum_i \gamma_i))
     *      end
     *      normalize phi_{n,i}^{t+1} sum to 1
     *     end
     *
     * Equation (6) in Latent Dirichlet Allocation, Blei 2003
     */
    template <typename Scalar>
    void compute_unsupervised_phi(
        const MatrixX<Scalar> & beta,
        const VectorX<Scalar> & gamma,
        Ref<MatrixX<Scalar> > phi
    );

    /**
     * Update Multinomial parameter phi, according to the following approximation
     *
     *
     */
    template <typename Scalar>
    void compute_supervised_approximate_phi(
        const VectorX<Scalar> & X_ratio,
        int num_words,
        int y,
        const MatrixX<Scalar> & beta,
        const MatrixX<Scalar> & eta,
        const VectorX<Scalar> & gamma,
        Scalar C,
        Ref<MatrixX<Scalar> > phi
    );

    template <typename Scalar>
    void compute_supervised_multinomial_phi(
        const VectorXi & X,
        int y,
        const MatrixX<Scalar> & beta,
        const MatrixX<Scalar> & eta,
        const VectorX<Scalar> & gamma,
        Scalar eta_weight,
        Ref<MatrixX<Scalar> > phi
    );

    template <typename Scalar>
    void compute_supervised_correspondence_phi(
        const VectorXi & X,
        int y,
        const MatrixX<Scalar> & beta,
        const MatrixX<Scalar> & eta,
        const VectorX<Scalar> & gamma,
        const VectorX<Scalar> & tau,
        Ref<MatrixX<Scalar> > phi
    );

    template <typename Scalar>
    void compute_supervised_correspondence_tau(
        const VectorXi & X,
        int y,
        const MatrixX<Scalar> & eta,
        const MatrixX<Scalar> & phi,
        Ref<VectorX<Scalar> > tau
    );
} // namespace e_step_utils

} // namespace ldaplusplus
#endif  // _LDAPLUSPLUS_ESTEPUTILS_HPP_


//
// Copyright (c) 2009, Markus Rickert
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//
// * Redistributions of source code must retain the above copyright notice,
//   this list of conditions and the following disclaimer.
// * Redistributions in binary form must reproduce the above copyright notice,
//   this list of conditions and the following disclaimer in the documentation
//   and/or other materials provided with the distribution.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
//

#include <iostream>
#include <memory>
#include <stdexcept>
#include <boost/lexical_cast.hpp>
#include <rl/kin/Kinematics.h>
#include <rl/math/Unit.h>

int
main(int argc, char** argv)
{
	if (argc < 2)
	{
		std::cout << "Usage: rlJacobianDemo KINEMATICSFILE Q1 ... Qn QD1 ... QDn" << std::endl;
		return EXIT_FAILURE;
	}

	try
	{
		std::shared_ptr<rl::kin::Kinematics> kinematics(rl::kin::Kinematics::create(argv[1]));

		rl::math::Vector q(kinematics->getDof());

		for (std::ptrdiff_t i = 0; i < q.size(); ++i)
		{
			q(i) = boost::lexical_cast<rl::math::Real>(argv[i + 2]);
		}

		rl::math::Vector qdot(kinematics->getDof());

		for (std::ptrdiff_t i = 0; i < qdot.size(); ++i)
		{
			qdot(i) = boost::lexical_cast<rl::math::Real>(argv[q.size() + i + 2]);
		}

		kinematics->setPosition(q);
		kinematics->updateFrames();
		kinematics->updateJacobian();

		std::cout << "J=" << std::endl << kinematics->getJacobian() << std::endl;

		rl::math::Vector xdot(kinematics->getOperationalDof() * 6);

		kinematics->forwardVelocity(qdot, xdot);

		std::cout << "xdot=" << std::endl;

		for (std::size_t i = 0; i < kinematics->getOperationalDof(); ++i)
		{
			std::cout << i << " " << xdot.transpose() << std::endl;
		}

		kinematics->updateJacobianInverse(static_cast<rl::math::Real>(1.0e-9));

		std::cout << "invJ=" << std::endl << kinematics->getJacobianInverse() << std::endl;

		kinematics->inverseVelocity(xdot, qdot);

		std::cout << "qdot=" << std::endl << qdot.transpose() << std::endl;
	}
	catch (const std::exception& e)
	{
		std::cout << e.what() << std::endl;
		return EXIT_FAILURE;
	}

	return EXIT_SUCCESS;
}


// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG (haftungsbeschränkt), www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also license.mtl.txt in the distribution.

#ifndef MTL_SORT_INCLUDE
#define MTL_SORT_INCLUDE

#include <algorithm>
#include <boost/numeric/mtl/interface/vpt.hpp>
#include <boost/numeric/mtl/concept/collection.hpp>
#include <boost/numeric/mtl/operation/swap_row.hpp>
#include <boost/numeric/mtl/operation/size.hpp>

namespace mtl { namespace vector {

/// sort vector
template <typename Vector>
void inline sort(Vector& x)
{
    std::sort(x.begin(), x.end());
}

/// sort vector with qicksort from lo to hi
template <typename Vector>
void quicksort (Vector& a, typename Collection<Vector>::size_type lo, typename Collection<Vector>::size_type hi)
{
		vampir_trace<2032> tracer;
       	typename Collection<Vector>::size_type i=lo, j=hi;

        // VergleichsÂelement x
        typename mtl::Collection<Vector>::value_type x=a[(lo+hi)/2];
        //  Aufteilung
        while (i<=j)
        {
            while (a[i]<x) i++;
            while (a[j]>x) j--;
            if (i<=j)
            {
                swap_row(a, i, j);
                i++; j--;
            }
        }
        // Recursion
        if (lo<j) quicksort(a, lo, j);
        if (i<hi) quicksort(a, i, hi);
	std::cout<< "a=" << a << "\n";
}

/// sort vector and permutaion with qicksort from lo to hi
template <typename Vector, typename PermVec>
void quicksort (Vector& a, PermVec& p, typename Collection<Vector>::size_type lo, typename Collection<Vector>::size_type hi)
{
		vampir_trace<2033> tracer;
       typename Collection<Vector>::size_type i=lo, j=hi;

        // VergleichsÂelement x
        typename mtl::Collection<Vector>::value_type x=a[(lo+hi)/2];
// 	std::cout<< "x=" << x << "\n";
        //  Aufteilung
        while (i<=j)
        {
            while (a[i]<x) i++;
            while (a[j]>x) j--;

            if (i<=j)
            {
                swap_row(a, i, j);
		swap_row(p, i, j);
// 		std::cout<< "a=" << a <<"\n";
		 i++;
  		if( j == 0){
 		  break;
 		} else {
		  j--;
		}

//  		std::cout<< "i=" << i << "\n";
// 		std::cout<< "j=" << j << "\n";
            }
//              std::cout<< "j=" << j << "\n";
        }
//         std::cout<< "aai=" << i << "\n";
// 	std::cout<< "aaj=" << j << "\n";
// 	std::cout<< "lo=" << lo << "\n";
// 	std::cout<< "hi=" << hi << "\n";
        // Recursion
        if (lo<j) quicksort(a, p, lo, j);
        if (i<hi) quicksort(a, p, i, hi);

}

template <typename Vector, typename PermVec>
void quicksort (Vector& a, Vector& b, PermVec& p, typename Collection<Vector>::size_type lo, typename Collection<Vector>::size_type hi)
{
  using vector::swap_row;
       typename Collection<Vector>::size_type i= lo, j= hi;

        // VergleichsÂelement x
       typename mtl::Collection<Vector>::value_type x= a[(lo+hi)/2];
//  	std::cout<< "x=" << x << "\n";
        //  Aufteilung
        while (i<=j)
        {
            while (a[i]<x) i++;
            while (a[j]>x) j--;

            if (i<=j)
            {
                swap_row(a, i, j);
		swap_row(b, i, j);
  		swap_row(p, i, j);
		i++;
  		if( j == 0){
 		  break;
 		} else {
		  j--;
		}
            }
        }
        // Recursion
        if (lo<j) quicksort(a, b, p, lo, j);
        if (i<hi) quicksort(a, b, p, i, hi);
}

template <typename Vector, typename PermVec>
void quicksort_xy(Vector& a, Vector& b, PermVec& p, typename Collection<Vector>::size_type lo,
		  typename Collection<Vector>::size_type hi)
{
    using vector::swap_row;
    typename Collection<Vector>::size_type       i= lo, j= hi;
    // pivot element (x, y)
    typename mtl::Collection<Vector>::value_type x= a[(lo+hi)/2], y= b[(lo+hi)/2];

    while (i<=j) {
	while (a[i] < x || (a[i] == x && b[i] < y)) i++;
	while (a[j] > x || (a[j] == x && b[j] > y)) j--;

	if (i <= j) {
	    swap_row(a, i, j);
	    swap_row(b, i, j);
	    swap_row(p, i, j);
	    i++;
	    if (j == 0)
		break;
	    else
		j--;
	}
    }
    // Recursion
    if (lo < j) quicksort_xy(a, b, p, lo, j);
    if (i < hi) quicksort_xy(a, b, p, i, hi);
}

/// sort vector with permutation
template <typename Vector, typename PermVec>
void inline sort(Vector& x, PermVec& p)
{
//      std::cout<< "x=" << x << "\n";
//      std::cout<< "p=" << p << "\n";
    using mtl::size;
    assert(size(x) == size(p));
    quicksort(x, p, 0, size(x)-1);

}

/// Sort 2 vectors with permutation
template <typename Vector, typename PermVec>
void inline sort(Vector& x, Vector& y, PermVec& p)
{
//      std::cout<< "x=" << x << "\n";
//      std::cout<< "p=" << p << "\n";
    using mtl::size;
    assert(size(x) == size(p));
    assert(size(x) == size(y));
    quicksort(x, y, p, 0, size(x)-1);
}

/// Sort 3 vectors lexicographically after values in x and y
template <typename Vector, typename PermVec>
void inline sort_xy(Vector& x, Vector& y, PermVec& z)
{
    using mtl::size;
    assert(size(x) == size(z));
    assert(size(x) == size(y));
    quicksort_xy(x, y, z, 0, size(x)-1);
}

}} // namespace mtl::vector

#endif // MTL_SORT_INCLUDE


// Copyright (c) 2015-2019 Daniel Cooke
// Use of this source code is governed by the MIT license that can be found in the LICENSE file.

#ifndef coverage_tracker_hpp
#define coverage_tracker_hpp

#include <vector>
#include <deque>
#include <cstddef>
#include <utility>
#include <iterator>
#include <algorithm>
#include <numeric>
#include <stdexcept>
#include <cassert>
#include <limits>

#include <boost/optional.hpp>
#include <boost/iterator/filter_iterator.hpp>

#include "concepts/mappable.hpp"
#include "maths.hpp"

namespace octopus {

/**
 CoverageTracker provides an efficient method for tracking coverage statistics over a range
 of Mappable objects without having to store the entire collection.
 */
template <typename Region, typename T = unsigned>
class CoverageTracker
{
public:
    using RegionType = Region;
    using DepthType  = T;

    CoverageTracker(bool check_overflow = false);

    CoverageTracker(const CoverageTracker&)            = default;
    CoverageTracker& operator=(const CoverageTracker&) = default;
    CoverageTracker(CoverageTracker&&)                 = default;
    CoverageTracker& operator=(CoverageTracker&&)      = default;

    ~CoverageTracker() = default;

    template <typename MappableType>
    void add(const MappableType& mappable);

    bool any() const noexcept;
    bool any(const Region& region) const noexcept;

    std::size_t sum() const noexcept;
    std::size_t sum(const Region& region) const noexcept;

    DepthType max() const noexcept;
    DepthType max(const Region& region) const noexcept;

    DepthType min() const noexcept;
    DepthType min(const Region& region) const noexcept;

    double mean() const noexcept;
    double mean(const Region& region) const noexcept;

    double stdev() const noexcept;
    double stdev(const Region& region) const noexcept;

    double median() const;
    double median(const Region& region) const;

    template <typename OutputIt>
    OutputIt get(const Region& region, OutputIt result) const;
    std::vector<DepthType> get(const Region& region) const;

    boost::optional<Region> encompassing_region() const;
    bool is_empty() const noexcept;
    std::size_t num_tracked() const noexcept;
    void clear() noexcept;

private:
    std::deque<DepthType> coverage_ = {};
    Region encompassing_region_;
    std::size_t num_tracked_ = 0;
    bool check_overflow_ = false;

    using Iterator     = typename decltype(coverage_)::const_iterator;
    using IteratorPair = std::pair<Iterator, Iterator>;

    void do_add(const Region& region);
    IteratorPair range(const Region& region) const;
};

// non-member methods

template <typename InputIterator, typename T = unsigned>
CoverageTracker<RegionType<typename std::iterator_traits<InputIterator>::value_type>, T>
make_coverage_tracker(InputIterator first, InputIterator last)
{
    using MappableType = typename std::iterator_traits<InputIterator>::value_type;
    using RegionTp = RegionType<MappableType>;
    CoverageTracker<RegionTp, T> result {};
    std::for_each(first, last, [&result] (const MappableType& mappable) { result.add(mappable); });
    return result;
}

template <typename Range, typename T = unsigned>
auto make_coverage_tracker(const Range& mappables)
{
    return make_coverage_tracker<typename Range::const_iterator, T>(std::cbegin(mappables), std::cend(mappables));
}

template <typename Range, typename UnaryPredicate, typename T = unsigned>
auto make_coverage_tracker(const Range& mappables, const UnaryPredicate condition)
{
    using boost::make_filter_iterator;
    const auto begin = make_filter_iterator<UnaryPredicate>(std::cbegin(mappables), std::cend(mappables));
    const auto end = make_filter_iterator<UnaryPredicate>(std::cend(mappables), std::cend(mappables));
    return make_coverage_tracker<decltype(begin), T>(begin, end);
}

template <typename Region>
std::vector<Region> get_covered_regions(const CoverageTracker<Region>& tracker, const Region& region)
{
    const auto depths = tracker.coverage(region);
    return select_regions(region, depths, [] (unsigned depth) { return depth > 0; });
}

template <typename Region>
std::vector<Region> get_covered_regions(const CoverageTracker<Region>& tracker)
{
    const auto tracker_region = tracker.encompassing_region();
    if (tracker_region) {
        return get_covered_regions(tracker, *tracker_region);
    } else {
        return {};
    }
}

// public methods

template <typename Region, typename T>
CoverageTracker<Region, T>::CoverageTracker(bool check_overflow)
: check_overflow_ {check_overflow}
{}

template <typename Region, typename T>
template <typename MappableType>
void CoverageTracker<Region, T>::add(const MappableType& mappable)
{
    static_assert(is_region_or_mappable<MappableType>, "MappableType not Mappable");
    do_add(mapped_region(mappable));
}

template <typename Region, typename T>
bool CoverageTracker<Region, T>::any() const noexcept
{
    return std::find_if(std::cbegin(coverage_), std::cend(coverage_),
                        [] (auto depth) noexcept { return depth > 0; }) != std::cend(coverage_);
}

template <typename Region, typename T>
bool CoverageTracker<Region, T>::any(const Region& region) const noexcept
{
    if (octopus::is_empty(region)) return false;
    const auto p = range(region);
    return std::find_if(p.first, p.second, [] (auto depth) noexcept { return depth > 0; }) != p.second;
}

template <typename Region, typename T>
std::size_t CoverageTracker<Region, T>::sum() const noexcept
{
    return std::accumulate(std::cbegin(coverage_), std::cend(coverage_), std::size_t {0});
}

template <typename Region, typename T>
std::size_t CoverageTracker<Region, T>::sum(const Region& region) const noexcept
{
    if (octopus::is_empty(region)) return 0;
    const auto p = range(region);
    return std::accumulate(p.first, p.second, std::size_t {0});
}

template <typename Region, typename T>
T CoverageTracker<Region, T>::max() const noexcept
{
    if (coverage_.empty()) return 0;
    return *std::max_element(std::cbegin(coverage_), std::cend(coverage_));
}

template <typename Region, typename T>
T CoverageTracker<Region, T>::max(const Region& region) const noexcept
{
    if (octopus::is_empty(region)) return 0;
    const auto p = range(region);
    if (p.first == p.second) return 0;
    return *std::max_element(p.first, p.second);
}

template <typename Region, typename T>
T CoverageTracker<Region, T>::min() const noexcept
{
    if (coverage_.empty()) return 0;
    return *std::min_element(std::cbegin(coverage_), std::cend(coverage_));
}

template <typename Region, typename T>
T CoverageTracker<Region, T>::min(const Region& region) const noexcept
{
    if (octopus::is_empty(region)) return 0;
    const auto p = range(region);
    if (p.first == p.second) return 0;
    return *std::min_element(p.first, p.second);
}

template <typename Region, typename T>
double CoverageTracker<Region, T>::mean() const noexcept
{
    if (coverage_.empty()) return 0;
    return maths::mean(coverage_);
}

template <typename Region, typename T>
double CoverageTracker<Region, T>::mean(const Region& region) const noexcept
{
    if (octopus::is_empty(region)) return 0;
    const auto p = range(region);
    return maths::mean(p.first, p.second);
}

template <typename Region, typename T>
double CoverageTracker<Region, T>::stdev() const noexcept
{
    if (coverage_.empty()) return 0;
    return maths::stdev(coverage_);
}

template <typename Region, typename T>
double CoverageTracker<Region, T>::stdev(const Region& region) const noexcept
{
    if (octopus::is_empty(region)) return 0;
    const auto p = range(region);
    return maths::stdev(p.first, p.second);
}

template <typename Region, typename T>
double CoverageTracker<Region, T>::median() const
{
    if (coverage_.empty()) return 0;
    return maths::median(coverage_);
}

template <typename Region, typename T>
double CoverageTracker<Region, T>::median(const Region& region) const
{
    auto range_coverage = this->get(region);
    if (range_coverage.empty()) return 0;
    return maths::median(range_coverage);
}

template <typename Region, typename T>
template <typename OutputIt>
OutputIt CoverageTracker<Region, T>::get(const Region& region, OutputIt result) const
{
    if (coverage_.empty() || !overlaps(region, encompassing_region_)) {
        return std::fill_n(result, size(region), 0);
    }
    const auto p = range(region);
    if (contains(encompassing_region_, region)) {
        return std::copy(p.first, p.second, result);
    } else {
        using D = typename Region::Distance;
        const auto lhs_pad = std::max(begin_distance(region, encompassing_region_), D {0});
        result = std::fill_n(result, lhs_pad, 0);
        result = std::copy(p.first, p.second, result);
        const auto rhs_pad = std::max(end_distance(encompassing_region_, region), D {0});
        return std::fill_n(result, rhs_pad, 0);
    }
}

template <typename Region, typename T>
std::vector<T> CoverageTracker<Region, T>::get(const Region& region) const
{
    std::vector<T> result(size(region));
    this->get(region, std::begin(result));
    return result;
}

template <typename Region, typename T>
boost::optional<Region> CoverageTracker<Region, T>::encompassing_region() const
{
    if (!is_empty()) {
        return encompassing_region_;
    } else {
        return boost::none;
    }
}

template <typename Region, typename T>
bool CoverageTracker<Region, T>::is_empty() const noexcept
{
    return num_tracked_ == 0;
}

template <typename Region, typename T>
std::size_t CoverageTracker<Region, T>::num_tracked() const noexcept
{
    return num_tracked_;
}

template <typename Region, typename T>
void CoverageTracker<Region, T>::clear() noexcept
{
    coverage_.clear();
    coverage_.shrink_to_fit();
    num_tracked_ = 0;
}

// private methods

namespace detail {

inline bool is_same_contig_helper(const ContigRegion& lhs, const ContigRegion& rhs) noexcept
{
    return true;
}

inline bool is_same_contig_helper(const GenomicRegion& lhs, const GenomicRegion& rhs) noexcept
{
    return is_same_contig(lhs, rhs);
}

} // namespace detail

template <typename Region, typename T>
void CoverageTracker<Region, T>::do_add(const Region& region)
{
    if (octopus::is_empty(region)) return;
    if (num_tracked_ == 0) {
        coverage_.assign(size(region), 1);
        encompassing_region_ = region;
    } else {
        if (!detail::is_same_contig_helper(region, encompassing_region_)) {
            throw std::runtime_error {"CoverageTracker: contig mismatch"};
        }
        bool region_change {false};
        if (begins_before(region, encompassing_region_)) {
            coverage_.insert(std::cbegin(coverage_), left_overhang_size(region, encompassing_region_), 0);
            region_change = true;
        }
        if (ends_before(encompassing_region_, region)) {
            coverage_.insert(std::cend(coverage_), right_overhang_size(region, encompassing_region_), 0);
            region_change = true;
        }
        if (region_change) {
            encompassing_region_ = octopus::encompassing_region(encompassing_region_, region);
        }
        const auto first = std::next(std::begin(coverage_), begin_distance(encompassing_region_, region));
        assert(first < std::end(coverage_));
        assert(std::next(first, size(region)) <= std::end(coverage_));
        const auto last = std::next(first, size(region));
        if (check_overflow_) {
            if (std::find(first, last, std::numeric_limits<DepthType>::max()) != last) {
                throw std::runtime_error {"CoverageTracker::add failed due to depth overflow"};
            }
        }
        std::transform(first, last, first, [] (auto count) noexcept { return count + 1; });
    }
    ++num_tracked_;
}

template <typename Region, typename T>
typename CoverageTracker<Region, T>::IteratorPair CoverageTracker<Region, T>::range(const Region& region) const
{
    if (coverage_.empty() || !overlaps(region, encompassing_region_)) {
        return {std::end(coverage_), std::end(coverage_)};
    }
    auto range_start_itr = std::begin(coverage_);
    if (begins_before(encompassing_region_, region)) {
        std::advance(range_start_itr, begin_distance(encompassing_region_, region));
    }
    return {range_start_itr, std::next(range_start_itr, overlap_size(region, encompassing_region_))};
}

} // namespace octopus

#endif


// All content Copyright (C) 2018 Genomics plc
#include "variant/variantNormalizer.hpp"
//#include <boost/numeric/ublas/io.hpp>
#include <stack>
#include "utils/interval.hpp"

namespace wecall
{
namespace variant
{
    variantSet_t VariantNormalizer::getNormalized( const caller::Region & paddedRegion,
                                                   const utils::BasePairSequence & sequence,
                                                   const boost::optional< variantSet_t > unnormalizedVariants ) const
    {
        caller::Region region = paddedRegion;
        utils::BasePairSequence referenceString = m_referenceSequence->subseq( paddedRegion ).sequence();
        utils::BasePairSequence altString = sequence;

        if ( unnormalizedVariants.is_initialized() )
        {
            const auto variants = unnormalizedVariants.get();

            if ( variants.empty() )
            {
                return {};
            }

            const auto startComp = []( const varPtr_t & a, const varPtr_t & b )
            {
                return a->start() < b->start();
            };
            const auto minElement = std::min_element( variants.cbegin(), variants.cend(), startComp );
            const auto start = ( *minElement )->start();

            const auto endComp = []( const varPtr_t & a, const varPtr_t & b )
            {
                return a->end() < b->end();
            };
            const auto maxElement = std::max_element( variants.cbegin(), variants.cend(), endComp );
            const auto end = ( *maxElement )->end();
            const auto frontPadding = start - paddedRegion.start();
            const auto backPadding = paddedRegion.end() - end;

            region = caller::Region( paddedRegion.contig(), utils::Interval( start, end ) );
            referenceString = m_referenceSequence->subseq( region ).sequence();
            altString = sequence.substr( frontPadding, sequence.size() - frontPadding - backPadding );
        }

        const std::size_t matrixSize = ( region.size() + 1 ) * ( altString.size() + 1 );
        if ( matrixSize > m_maxMatrixSize )
        {
            WECALL_LOG( DEBUG, "Skipping normalization because the matrix has size "
                                    << region.size() + 1 << "x" << altString.size() + 1 << " = " << matrixSize
                                    << ", which is greater than the threshold of " << m_maxMatrixSize );
            if ( unnormalizedVariants.is_initialized() )
            {
                return unnormalizedVariants.get();
            }
            else
            {
                return {};
            }
        }

        const auto nWPenalties = utils::NWPenalties();
        auto needlemanWunsch = utils::NeedlemanWunsch( referenceString, altString, nWPenalties );

        needlemanWunsch.getScoreMatrix();

        auto nWVariants = needlemanWunsch.traceBack();

        variantSet_t variantSet = {};

        for ( auto & simpleVariant : nWVariants )
        {
            variantSet.insert( simpleVariant.getVariant( region.contig(), region.start(), m_referenceSequence )
                               //                                      ->getLeftAligned( paddedRegion.start() )
                               );
        }

        return variantSet;
    }
}
}


/*
 * Copyright 2020 Adobe. All rights reserved.
 * This file is licensed to you under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License. You may obtain a copy
 * of the License at http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software distributed under
 * the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR REPRESENTATIONS
 * OF ANY KIND, either express or implied. See the License for the specific language
 * governing permissions and limitations under the License.
 */
#include <lagrange/common.h>
#include <lagrange/io/load_mesh.h>
#include <lagrange/io/save_mesh.h>
#include <lagrange/mesh_cleanup/close_small_holes.h>
#include <lagrange/mesh_cleanup/remove_degenerate_triangles.h>
#include <lagrange/mesh_cleanup/remove_duplicate_vertices.h>
#include <lagrange/mesh_cleanup/split_long_edges.h>

// clang-format off
#include <lagrange/utils/warnoff.h>
#include <igl/bounding_box_diagonal.h>
#include <lagrange/utils/warnon.h>
// clang-format on

#include <CLI/CLI.hpp>
#include <Eigen/Core>

int main(int argc, char** argv)
{
    struct
    {
        std::string input;
        std::string output = "output.obj";
        double tol = 0.001;
        size_t max_holes = 0;
        bool holes_only = false;
        bool relative = true;
    } args;

    CLI::App app{argv[0]};
    app.add_option("input", args.input, "Input mesh.")->required()->check(CLI::ExistingFile);
    app.add_option("output", args.output, "Output mesh.");
    app.add_option("-s,--max-holes", args.max_holes, "Max hole size to close.");
    app.add_flag("-H,--holes-only", args.holes_only, "Only fill holes.");
    app.add_option("-t,--tolerance", args.tol, "Tolerance.");
    app.add_option(
        "-r,--relative",
        args.relative,
        "Whether to use a tolerance relative to the bbox diagonal.");
    CLI11_PARSE(app, argc, argv)

    lagrange::logger().set_level(spdlog::level::trace);

    lagrange::logger().info("Loading input mesh: {}", args.input);
    auto mesh = lagrange::io::load_mesh<lagrange::TriangleMesh3D>(args.input);

    if (args.relative) {
        double diag = igl::bounding_box_diagonal(mesh->get_vertices());
        lagrange::logger().info(
            "Using a relative tolerance of {:.3f} x {:.3f} = {:.3f}",
            args.tol,
            diag,
            args.tol * diag);
        args.tol *= diag;
    }

    if (args.max_holes) {
        lagrange::logger().info("Closing small holes");
        mesh = lagrange::close_small_holes(*mesh, args.max_holes);
    }

    if (!args.holes_only) {
        lagrange::logger().info("Removing degenerate triangles");
        mesh = lagrange::remove_degenerate_triangles(*mesh);
        lagrange::logger().info("Removing duplicate vertices");
        mesh = lagrange::remove_duplicate_vertices(*mesh);
        lagrange::logger().info("Splitting long edges");
        mesh = lagrange::split_long_edges(*mesh, args.tol * args.tol, true);
    }

    lagrange::logger().info("Saving result: {}", args.output);
    lagrange::io::save_mesh(args.output, *mesh);

    return 0;
}


#ifndef STAN_MATH_REV_SCAL_FUN_BOOST_ISNAN_HPP
#define STAN_MATH_REV_SCAL_FUN_BOOST_ISNAN_HPP

#include <boost/math/special_functions/fpclassify.hpp>
#include <stan/math/rev/core.hpp>

namespace boost {
  namespace math {

    /**
     * Checks if the given number is NaN
     *
     * Return true if the specified variable
     * has a value that is NaN.
     *
     * @param v Variable to test.
     * @return true if variable is NaN.
     */
    template <>
    inline
    bool isnan(const stan::math::var& v) {
      return (boost::math::isnan)(v.val());
    }

  }
}
#endif


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

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

// This file was modified by Oracle on 2018-2020.
// Modifications copyright (c) 2018-2020 Oracle and/or its affiliates.
// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

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

// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_GEOMETRY_EXTENSIONS_ALGEBRA_CORE_COORDINATE_DIMENSION_HPP
#define BOOST_GEOMETRY_EXTENSIONS_ALGEBRA_CORE_COORDINATE_DIMENSION_HPP

#include <boost/geometry/core/coordinate_dimension.hpp>
#include <boost/geometry/core/static_assert.hpp>

#include <boost/geometry/extensions/algebra/core/tags.hpp>

namespace boost { namespace geometry {

namespace traits {

template <typename Geometry, std::size_t Index>
struct indexed_dimension
{
    BOOST_GEOMETRY_STATIC_ASSERT_FALSE(
        "Not implemented for this Geometry or Index",
        Geometry, std::integral_constant<std::size_t, Index>);
};

} // namespace traits

#ifndef DOXYGEN_NO_DISPATCH
namespace core_dispatch {

template <typename V>
struct dimension<vector_tag, V>
    : traits::dimension<typename detail::remove_cptrref<V>::type>
{};

template <typename G>
struct dimension<quaternion_tag, G>
    : traits::dimension<typename detail::remove_cptrref<G>::type>
{};

template <typename T, typename G, std::size_t Index>
struct indexed_dimension
{
    BOOST_GEOMETRY_STATIC_ASSERT_FALSE(
        "Not implemented for this Geometry or Index",
        G, std::integral_constant<std::size_t, Index>);
};

template <typename G, std::size_t Index>
struct indexed_dimension<matrix_tag, G, Index>
    : traits::indexed_dimension<typename detail::remove_cptrref<G>::type, Index>
{};

template <typename G>
struct dimension<rotation_quaternion_tag, G>
    : traits::dimension<typename detail::remove_cptrref<G>::type>
{};

template <typename G>
struct dimension<rotation_matrix_tag, G>
    : traits::dimension<typename detail::remove_cptrref<G>::type>
{};

} // namespace core_dispatch
#endif // DOXYGEN_NO_DISPATCH

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_EXTENSIONS_ALGEBRA_CORE_COORDINATE_DIMENSION_HPP


#include "STLSceneLoader.h"
#include "SceneShape.h"
#include <boost/algorithm/string.hpp>
#include <boost/lexical_cast.hpp>
#include <fstream>

using namespace std;
using namespace boost::algorithm;
using namespace cnoid;

const char* STLSceneLoader::format() const
{
    return "STL";
}

static void readVector3(string text, SgVectorArray<Vector3f>* array)
{
    trim(text);
    Vector3f value;
    int i = 0;
    split_iterator<string::iterator> iter = make_split_iterator(text, token_finder(is_space(), token_compress_on));
    while(iter != split_iterator<string::iterator>()){
        value[i++] = boost::lexical_cast<float>(*iter++);
        if(i == 3){
            array->push_back(value);
            break;
        }
    }
}

SgNode* STLSceneLoader::load(const std::string& fileName)
{
    std::ifstream ifs(fileName.c_str(), std::ios::in);

    SgVertexArrayPtr vertices = new SgVertexArray;
    SgNormalArrayPtr normals = new SgNormalArray;

    std::string line;
    while(!ifs.eof() && getline(ifs, line)){
        trim(line);
        if(boost::istarts_with(line, "vertex")){
            readVector3(line.substr(6), vertices);
        } else if(boost::istarts_with(line, "facet normal")){
            readVector3(line.substr(12), normals);
        }
    }

    SgShape* shape = 0;

    if(!vertices->empty()){
        shape = new SgShape;
        SgMesh* mesh = shape->getOrCreateMesh();
        mesh->setVertices(vertices);
        const int numTriangles = vertices->size() / 3;
        mesh->reserveNumTriangles(numTriangles);
        for(int i = 0; i < numTriangles; ++i){
            const int j = i * 3;
            mesh->addTriangle(j, j + 1, j + 2);
        }
        if(!normals->empty()){
            mesh->setNormals(normals);
            SgIndexArray& indices = mesh->normalIndices();
            indices.reserve(normals->size() * 3);
            for(int i = 0; i < normals->size(); ++i) {
                indices.push_back(i);
                indices.push_back(i);
                indices.push_back(i);
            }
        }
    }

    return shape;
}


//==============================================================================
//         Copyright 2003 - 2011   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#include <boost/simd/memory/shifted_iterator.hpp>
#include <boost/simd/memory/output_iterator.hpp>
#include <boost/simd/include/functions/plus.hpp>
#include <boost/simd/sdk/simd/pack.hpp>
#include <boost/simd/sdk/simd/io.hpp>
#include <boost/simd/sdk/meta/cardinal_of.hpp>
#include <boost/simd/memory/allocator.hpp>
#include <nt2/sdk/unit/tests/relation.hpp>
#include <nt2/sdk/unit/module.hpp>
#include <vector>

template<class T>
struct average
{
  template<class Region>
  boost::simd::pack<T> operator()(Region const& r)
  {
    return (r[0]+r[1]+r[2]);
  }
};

template<typename In, typename Out, typename F>
inline Out mytransform(In const& b, In const& e, Out const& o, F f)
{
  In  lb = b;
  Out lo = o;
  for(;lb!=e;) *lo++ = f(*lb++);
  return lo;
}

NT2_TEST_CASE_TPL(shifted_vector_iterator_scalar, BOOST_SIMD_SIMD_TYPES )
{
  using boost::simd::output_iterator;
  using boost::simd::output_begin;
  using boost::simd::shifted_iterator;
  using boost::simd::shifted_begin;
  using boost::simd::shifted_end;
  using boost::simd::allocator;
  using boost::simd::region_;
  using boost::simd::around;
  using boost::simd::pack;
  using boost::simd::meta::cardinal_of;

  typedef typename std::vector<T, allocator<T> >::iterator it_t;
  typedef shifted_iterator<it_t,region_<-1,1> > sit_;
  typedef pack<T> p_t;

  static const std::size_t card = cardinal_of<p_t>::value;
  std::vector<T,allocator<T> > data(card*6);
  std::vector<T,allocator<T> > ref(card*6);
  std::vector<T,allocator<T> > result(card*6);

  for(std::size_t i=0; i<data.size(); ++i)
  {
    data[i] = T(i+1-card);
  }

  for(std::size_t i=card; i<ref.size()-card; ++i) ref[i] = T((i+1-card)*3);

  sit_ sb = boost::simd::shifted_begin(data.begin(), around<-1,1>());
  sit_ se = boost::simd::shifted_end(data.end(), around<-1,1>());

  mytransform( sb, se, output_begin(result.begin())+1, average<T>() );

  NT2_TEST_EQUAL(result,ref);
}