//=======================================================================
// Copyright 2013 University of Warsaw.
// Authors: Piotr Wygocki
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//=======================================================================
//
//This algorithm is described in "Network Flows: Theory, Algorithms, and Applications"
// by Ahuja, Magnanti, Orlin.

#ifndef BOOST_GRAPH_SUCCESSIVE_SHORTEST_PATH_HPP
#define BOOST_GRAPH_SUCCESSIVE_SHORTEST_PATH_HPP

#include <numeric>

#include <boost/property_map/property_map.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/graph_concepts.hpp>
#include <boost/pending/indirect_cmp.hpp>
#include <boost/pending/relaxed_heap.hpp>
#include <boost/graph/dijkstra_shortest_paths.hpp>
#include <boost/graph/properties.hpp>
#include <boost/graph/iteration_macros.hpp>
#include <boost/graph/detail/augment.hpp>

namespace boost {

namespace detail {

template <class Graph, class Weight, class Distance, class Reversed>
class MapReducedWeight :
    public put_get_helper<typename property_traits<Weight>::value_type, MapReducedWeight<Graph, Weight, Distance, Reversed> > {
    typedef graph_traits<Graph> gtraits;
public:
    typedef boost::readable_property_map_tag category;
    typedef typename property_traits<Weight>::value_type value_type;
    typedef value_type reference;
    typedef typename gtraits::edge_descriptor key_type;
    MapReducedWeight(const Graph & g, Weight w, Distance d, Reversed r) :
        g_(g), weight_(w), distance_(d), rev_(r) {}

    reference operator[](key_type v) const {
        return get(distance_, source(v, g_)) - get(distance_,target(v, g_)) + get(weight_, v);
    }
private:
    const Graph & g_;
    Weight weight_;
    Distance distance_;
    Reversed rev_;
};

template <class Graph, class Weight, class Distance, class Reversed>
MapReducedWeight<Graph, Weight, Distance, Reversed>
make_mapReducedWeight(const Graph & g, Weight w, Distance d, Reversed r)  {
    return MapReducedWeight<Graph, Weight, Distance, Reversed>(g, w, d, r);
}

}//detail

template <class Graph, class Capacity, class ResidualCapacity, class Reversed, class Pred, class Weight, class Distance, class Distance2, class VertexIndex>
void successive_shortest_path_nonnegative_weights(
        const Graph &g,
        typename graph_traits<Graph>::vertex_descriptor s,
        typename graph_traits<Graph>::vertex_descriptor t,
        Capacity capacity,
        ResidualCapacity residual_capacity,
        Weight weight,
        Reversed rev,
        VertexIndex index,
        Pred pred,
        Distance distance,
        Distance2 distance_prev) {
    filtered_graph<const Graph, is_residual_edge<ResidualCapacity> >
        gres = detail::residual_graph(g, residual_capacity);
    typedef typename graph_traits<Graph>::edge_descriptor edge_descriptor;

    BGL_FORALL_EDGES_T(e, g, Graph) {
        put(residual_capacity, e, get(capacity, e));
    }

    BGL_FORALL_VERTICES_T(v, g, Graph) {
        put(distance_prev, v, 0);
    }

    while(true) {
        BGL_FORALL_VERTICES_T(v, g, Graph) {
            put(pred, v, edge_descriptor());
        }
        dijkstra_shortest_paths(gres, s,
                weight_map(detail::make_mapReducedWeight(gres, weight, distance_prev, rev)).
                distance_map(distance).
                vertex_index_map(index).
                visitor(make_dijkstra_visitor(record_edge_predecessors(pred, on_edge_relaxed()))));

        if(get(pred, t) == edge_descriptor()) {
            break;
        }

        BGL_FORALL_VERTICES_T(v, g, Graph) {
            put(distance_prev, v, get(distance_prev, v) + get(distance, v));
        }

        detail::augment(g, s, t, pred, residual_capacity, rev);
    }
}

//in this namespace argument dispatching tak place
namespace detail {

template <class Graph, class Capacity, class ResidualCapacity, class Weight, class Reversed, class Pred, class Distance, class Distance2, class VertexIndex>
void successive_shortest_path_nonnegative_weights_dispatch3(
        const Graph &g,
        typename graph_traits<Graph>::vertex_descriptor s,
        typename graph_traits<Graph>::vertex_descriptor t,
        Capacity capacity,
        ResidualCapacity residual_capacity,
        Weight weight,
        Reversed rev,
        VertexIndex index,
        Pred pred,
        Distance dist,
        Distance2 dist_pred) {
    successive_shortest_path_nonnegative_weights(g, s, t, capacity, residual_capacity, weight, rev, index, pred, dist, dist_pred);
}

//setting default distance map
template <class Graph, class Capacity, class ResidualCapacity, class Weight, class Reversed, class Pred, class Distance, class VertexIndex>
void successive_shortest_path_nonnegative_weights_dispatch3(
        Graph &g,
        typename graph_traits<Graph>::vertex_descriptor s,
        typename graph_traits<Graph>::vertex_descriptor t,
        Capacity capacity,
        ResidualCapacity residual_capacity,
        Weight weight,
        Reversed rev,
        VertexIndex index,
        Pred pred,
        Distance dist,
        param_not_found) {
    typedef typename property_traits<Weight>::value_type D;

    std::vector<D> d_map(num_vertices(g));

    successive_shortest_path_nonnegative_weights(g, s, t, capacity, residual_capacity, weight, rev, index, pred, dist,
                             make_iterator_property_map(d_map.begin(), index));
}

template <class Graph, class P, class T, class R, class Capacity, class ResidualCapacity, class Weight, class Reversed, class Pred, class Distance, class VertexIndex>
void successive_shortest_path_nonnegative_weights_dispatch2(
        Graph &g,
        typename graph_traits<Graph>::vertex_descriptor s,
        typename graph_traits<Graph>::vertex_descriptor t,
        Capacity capacity,
        ResidualCapacity residual_capacity,
        Weight weight,
        Reversed rev,
        VertexIndex index,
        Pred pred,
        Distance dist,
        const bgl_named_params<P, T, R>& params) {
    successive_shortest_path_nonnegative_weights_dispatch3(g, s, t, capacity, residual_capacity, weight, rev, index, pred, dist, get_param(params, vertex_distance2));
}

//setting default distance map
template <class Graph, class P, class T, class R, class Capacity, class ResidualCapacity, class Weight, class Reversed, class Pred, class VertexIndex>
void successive_shortest_path_nonnegative_weights_dispatch2(
        Graph &g,
        typename graph_traits<Graph>::vertex_descriptor s,
        typename graph_traits<Graph>::vertex_descriptor t,
        Capacity capacity,
        ResidualCapacity residual_capacity,
        Weight weight,
        Reversed rev,
        VertexIndex index,
        Pred pred,
        param_not_found,
        const bgl_named_params<P, T, R>& params) {
    typedef typename property_traits<Weight>::value_type D;

    std::vector<D> d_map(num_vertices(g));

    successive_shortest_path_nonnegative_weights_dispatch3(g, s, t, capacity, residual_capacity, weight, rev, index, pred,
            make_iterator_property_map(d_map.begin(), index),
            get_param(params, vertex_distance2));
}

template <class Graph, class P, class T, class R, class Capacity, class ResidualCapacity, class Weight, class Reversed, class Pred, class VertexIndex>
void successive_shortest_path_nonnegative_weights_dispatch1(
        Graph &g,
        typename graph_traits<Graph>::vertex_descriptor s,
        typename graph_traits<Graph>::vertex_descriptor t,
        Capacity capacity,
        ResidualCapacity residual_capacity,
        Weight weight,
        Reversed rev,
        VertexIndex index,
        Pred pred,
        const bgl_named_params<P, T, R>& params) {
    successive_shortest_path_nonnegative_weights_dispatch2(g, s, t, capacity, residual_capacity, weight,  rev, index, pred,
                                get_param(params, vertex_distance), params);
}

//setting default predecessors map
template <class Graph, class P, class T, class R, class Capacity, class ResidualCapacity, class Weight, class Reversed, class VertexIndex>
void successive_shortest_path_nonnegative_weights_dispatch1(
        Graph &g,
        typename graph_traits<Graph>::vertex_descriptor s,
        typename graph_traits<Graph>::vertex_descriptor t,
        Capacity capacity,
        ResidualCapacity residual_capacity,
        Weight weight,
        Reversed rev,
        VertexIndex index,
        param_not_found,
        const bgl_named_params<P, T, R>& params) {
    typedef typename graph_traits<Graph>::edge_descriptor edge_descriptor;
    std::vector<edge_descriptor> pred_vec(num_vertices(g));

    successive_shortest_path_nonnegative_weights_dispatch2(g, s, t, capacity, residual_capacity, weight, rev, index,
            make_iterator_property_map(pred_vec.begin(), index),
            get_param(params, vertex_distance), params);
}

}//detail

template <class Graph, class P, class T, class R>
void successive_shortest_path_nonnegative_weights(
        Graph &g,
        typename graph_traits<Graph>::vertex_descriptor s,
        typename graph_traits<Graph>::vertex_descriptor t,
        const bgl_named_params<P, T, R>& params) {

    return detail::successive_shortest_path_nonnegative_weights_dispatch1(g, s, t,
           choose_const_pmap(get_param(params, edge_capacity), g, edge_capacity),
           choose_pmap(get_param(params, edge_residual_capacity),
                       g, edge_residual_capacity),
           choose_const_pmap(get_param(params, edge_weight), g, edge_weight),
           choose_const_pmap(get_param(params, edge_reverse), g, edge_reverse),
           choose_const_pmap(get_param(params, vertex_index), g, vertex_index),
           get_param(params, vertex_predecessor),
           params);
}

template <class Graph>
void successive_shortest_path_nonnegative_weights(
        Graph &g,
        typename graph_traits<Graph>::vertex_descriptor s,
        typename graph_traits<Graph>::vertex_descriptor t) {
    bgl_named_params<int, buffer_param_t> params(0);
    successive_shortest_path_nonnegative_weights(g, s, t, params);
}

}//boost
#endif /* BOOST_GRAPH_SUCCESSIVE_SHORTEST_PATH_HPP */


//
//  Copyright Toon Knapen, Karl Meerbergen
//
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//

#include "../../blas/test/random.hpp"

#include <boost/numeric/bindings/lapack/computational/sytrd.hpp>
#include <boost/numeric/bindings/ublas/matrix.hpp>
#include <boost/numeric/bindings/ublas/symmetric.hpp>
#include <boost/numeric/bindings/ublas/vector.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <algorithm>
#include <limits>
#include <iostream>

namespace ublas = boost::numeric::ublas;
namespace lapack = boost::numeric::bindings::lapack;
namespace bindings = boost::numeric::bindings;

template <typename T, typename UPLO>
int do_value_type() {
   const int n = 10 ;

   typedef typename bindings::remove_imaginary<T>::type real_type ;
   typedef std::complex< real_type >                                            complex_type ;

   typedef ublas::matrix<T, ublas::column_major> matrix_type ;
   typedef ublas::symmetric_adaptor<matrix_type, UPLO> symmetric_type ;
   typedef ublas::vector<T>                      vector_type ;

   // Set matrix
   matrix_type a( n, n );
   vector_type d( n ), e ( n - 1 ), tau( n - 1 ) ;

   for (int i=0; i<n; ++i ) {
     for (int j=0; j<n; ++j ) {
       a(j,i) = 0.0 ;
     }
   }

   a(0,0) = 2.0 ;
   for (int i=1; i<n; ++i ) {
      a(i,i) = 2.0 ;
      a(i-1,i) = -1.0 ;
   }

   // Compute eigendecomposition.
   symmetric_type s_a( a );
   lapack::sytrd( s_a, d, e, tau ) ;

   for ( int i=0; i<d.size(); ++i) {
      if (std::abs( d(i) - 2.0 ) > 10 * std::numeric_limits<T>::epsilon() ) return 1 ;
   }
   for ( int i=0; i<e.size(); ++i) {
      if (std::abs( e(i) + 1.0 ) > 10 * std::numeric_limits<T>::epsilon() ) return 1 ;
   }

   return 0 ;
} // do_value_type()

int main() {
   // Run tests for different value_types
   if (do_value_type<float, ublas::upper>()) return 255;
   if (do_value_type<double, ublas::upper>()) return 255;

   std::cout << "Regression test succeeded\n" ;
   return 0;
}


// -----------------------------------------------------------------------------
// Fern © Geoneric
//
// This file is part of Geoneric Fern which is available under the terms of
// the GNU General Public License (GPL), version 2. If you do not want to
// be bound by the terms of the GPL, you may purchase a proprietary license
// from Geoneric (http://www.geoneric.eu/contact).
// -----------------------------------------------------------------------------
#define BOOST_TEST_MODULE fern expression_tree evaluate_visitor
#include <boost/multi_array.hpp>
#include <boost/test/unit_test.hpp>
#include <boost/variant/get.hpp>
#include "fern/expression_tree/plus.h"
#include "fern/expression_tree/sqrt.h"
#include "fern/expression_tree/times.h"
#include "fern/expression_tree/evaluate_visitor.h"
#include "fern/feature/core/masked_array.h"

namespace fern {
namespace expression_tree {

// template<
//     class U,
//     class V>
// Operation<typename Plus<Raster<U>, Raster<V>>::result_type> operator+(
//     Raster<U> const& lhs,
//     Raster<V> const& rhs)
// {
//     return Operation<typename Plus<Raster<U>, Raster<V>>::result_type>(
//         "plus",
//         Implementation(Plus<Raster<U>, Raster<V>>()),
//         {
//             lhs,
//             rhs
//         }
//     );
// }

// template<
//     class U,
//     class V>
// Operation<typename Plus<Raster<U>, Raster<V>>::result_type> operator/(
//     Operation<Raster<U>> const& lhs,
//     Raster<V> const& rhs)
// {
//     return Operation<typename Plus<Raster<U>, Raster<V>>::result_type>(
//         "plus",
//         Implementation(Plus<Raster<U>, Raster<V>>()),
//         {
//             lhs,
//             rhs
//         }
//     );
// }

template<
    class U,
    class V>
Operation<typename Plus<U, V>::result_type> operator+(
    U const& lhs,
    V const& rhs)
{
    return Operation<typename Plus<U, V>::result_type>(
        "plus",
        Implementation(Plus<U, V>()),
        {
            lhs,
            rhs
        }
    );
}

} // namespace expression_tree
} // namespace fern

BOOST_AUTO_TEST_CASE(visit_constants)
{
    namespace fet = fern::expression_tree;

    // 2
    {
        fet::Constant<int32_t> constant(2);
        auto expression(constant);

        using Result = decltype(expression)::result_type;

        fet::Data result(fet::evaluate(expression));

        BOOST_REQUIRE_NO_THROW(boost::get<Result>(result));
        BOOST_CHECK_EQUAL(boost::get<Result>(result).value, 2);
    }

    // 2 + 1
    {
        fet::Constant<int32_t> expression1(2);
        using Result1 = decltype(expression1)::result_type;

        fet::Constant<int32_t> expression2(1);
        using Result2 = decltype(expression2)::result_type;

        using Result3 = typename fet::Plus<Result1, Result2>::result_type;

        fet::Operation<Result3> expression3(
            "plus",
            fet::Implementation(
                fet::Plus<Result1, Result2>()),
            {
                expression1,
                expression2
            }
        );

        fet::Data result(fet::evaluate(expression3));

        BOOST_REQUIRE_NO_THROW(boost::get<Result3>(result));
        BOOST_CHECK_EQUAL(boost::get<Result3>(result).value, 3);
    }

    // sqrt((2 + 1) * 3.0)
    {
        // 1: 2
        fet::Constant<int32_t> expression1(2);
        using Result1 = decltype(expression1)::result_type;

        // 2: 1
        fet::Constant<int32_t> expression2(1);
        using Result2 = decltype(expression2)::result_type;

        // 3: 2 + 1
        using Result3 = typename fet::Plus<Result1, Result2>::result_type;

        fet::Operation<Result3> expression3(
            "plus",
            fet::Implementation(
                fet::Plus<Result1, Result2>()),
            {
                expression1,
                expression2
            }
        );

        // 4: 3.0
        fet::Constant<double> expression4(3.0);
        using Result4 = decltype(expression4)::result_type;

        // 5: (2 + 1) * 3.0
        using Result5 = typename fet::Times<Result3, Result4>::result_type;

        fet::Operation<Result5> expression5(
            "times",
            fet::Implementation(
                fet::Times<Result3, Result4>()),
            {
                expression3,
                expression4
            }
        );

        // 6: sqrt((2 + 1) * 3.0)
        using Result6 = typename fet::Sqrt<Result5>::result_type;

        fet::Operation<Result6> expression6(
            "sqrt",
            fet::Implementation(
                fet::Sqrt<Result5>()),
            {
                expression5
            }
        );

        fet::Data result(fet::evaluate(expression6));

        BOOST_REQUIRE_NO_THROW(boost::get<Result6>(result));
        BOOST_CHECK_CLOSE(boost::get<Result6>(result).value, 3.0, 1e-6);
    }
}

template<
    class T>
using Raster = fern::MaskedArray<T, 2>;

BOOST_AUTO_TEST_CASE(visit_raster)
{
    namespace fet = fern::expression_tree;

    size_t const nr_rows = 3;
    size_t const nr_cols = 4;
    auto extents(fern::extents[nr_rows][nr_cols]);

    Raster<int32_t> raster1(extents);
    raster1[0][0] = -2;
    raster1[0][1] = -1;
    raster1[1][0] = 0;
    raster1.mask()[1][1] = true;
    raster1[2][0] = 1;
    raster1[2][1] = 2;

    auto expression1 = fet::Raster<int32_t>(raster1);
    using Result1 = decltype(expression1)::result_type;
    static_assert(std::is_same<Result1, fet::Raster<int32_t>>::value, "");

    Raster<int32_t> raster2(extents);
    raster2[0][0] = -20;
    raster2[0][1] = -10;
    raster2[1][0] = 0;
    raster2.mask()[1][1] = true;
    raster2[2][0] = 10;
    raster2[2][1] = 20;

    auto expression2 = fet::Raster<int32_t>(raster2);
    using Result2 = decltype(expression2)::result_type;
    static_assert(std::is_same<Result2, fet::Raster<int32_t>>::value, "");

    Raster<double> raster3(extents);
    raster3[0][0] = 2.0;
    raster3[0][1] = 4.0;
    raster3[1][0] = 6.0;
    raster3[1][1] = 8.0;
    raster3[2][0] = 10.0;
    raster3[2][1] = 12.0;

    auto expression3 = fet::Raster<double>(raster3);
    using Result3 = decltype(expression3)::result_type;
    static_assert(std::is_same<Result3, fet::Raster<double>>::value, "");

    // raster + raster
    {
        using namespace fern::expression_tree;

        auto operation = expression1 + expression2;

        fet::evaluate(operation);
        fet::Data result(fet::evaluate(operation));

        BOOST_REQUIRE_NO_THROW(boost::get<fet::Raster<int32_t> const&>(result));
        fet::Raster<int32_t> const& result_raster(
            boost::get<fet::Raster<int32_t> const&>(result));
        Raster<int32_t> const& raster(result_raster.value);

        BOOST_REQUIRE_EQUAL(raster.num_dimensions(), 2);
        BOOST_REQUIRE_EQUAL(raster.shape()[0], nr_rows);
        BOOST_REQUIRE_EQUAL(raster.shape()[1], nr_cols);

        BOOST_CHECK(!raster.mask()[0][0]);
        BOOST_CHECK_EQUAL(raster[0][0], -22);

        BOOST_CHECK(!raster.mask()[0][1]);
        BOOST_CHECK_EQUAL(raster[0][1], -11);

        BOOST_CHECK(!raster.mask()[1][0]);
        BOOST_CHECK_EQUAL(raster[1][0],  0);

        BOOST_CHECK( raster.mask()[1][1]);

        BOOST_CHECK(!raster.mask()[2][0]);
        BOOST_CHECK_EQUAL(raster[2][0],  11);

        BOOST_CHECK(!raster.mask()[2][1]);
        BOOST_CHECK_EQUAL(raster[2][1],  22);
    }

    // raster + raster + raster
    {
        auto operation = expression1 + expression2 + expression3;

        // std::cout << "evaluate..." << std::endl;
        fet::evaluate(operation);
        // std::cout << "/evaluate..." << std::endl;
        // fet::Data result(fet::evaluate(operation));

        // BOOST_REQUIRE_NO_THROW(boost::get<fet::Raster<double> const&>(result));
        // fet::Raster<double> const& result_raster(
        //     boost::get<fet::Raster<double> const&>(result));
        // Raster<double> const& raster(result_raster.value);
        // return;

        // BOOST_REQUIRE_EQUAL(raster.num_dimensions(), 2);
        // BOOST_REQUIRE_EQUAL(raster.shape()[0], nr_rows);
        // BOOST_REQUIRE_EQUAL(raster.shape()[1], nr_cols);

        // BOOST_CHECK(!raster.mask()[0][0]);
        // BOOST_CHECK_EQUAL(raster[0][0], -22);

        // BOOST_CHECK(!raster.mask()[0][1]);
        // BOOST_CHECK_EQUAL(raster[0][1], -11);

        // BOOST_CHECK(!raster.mask()[1][0]);
        // BOOST_CHECK_EQUAL(raster[1][0],  0);

        // BOOST_CHECK( raster.mask()[1][1]);

        // BOOST_CHECK(!raster.mask()[2][0]);
        // BOOST_CHECK_EQUAL(raster[2][0],  11);

        // BOOST_CHECK(!raster.mask()[2][1]);
        // BOOST_CHECK_EQUAL(raster[2][1],  22);
    }
}

BOOST_AUTO_TEST_CASE(visit_vector)
{
    {
        /// std::vector<int32_t> vector1({1, 2, 3, 4, 5});
        /// fern::Array<int32_t> expression1(vector1);
        /// using Result1 = decltype(expression1)::result_type;

        /// std::vector<int32_t> vector2({10, 11, 12, 13, 14});
        /// fern::Array<int32_t> expression2(vector2);
        /// using Result2 = decltype(expression2)::result_type;

        /// using Result3 = typename fern::Plus<Result1, Result2>::result_type;

        /// fern::Operation<Result3> expression3(
        ///     "plus",
        ///     fern::Implementation(
        ///         fern::Plus<Result1, Result2>()),
        ///     {
        ///         expression1,
        ///         expression2
        ///     }
        /// );

        // TODO How to create the resulting collection. Do it in the evaluate?
        //      We need the type of the result and the extent(s) of the
        //      input(s).

        // fern::Data result(fern::evaluate(expression3));

        // BOOST_REQUIRE_NO_THROW(boost::get<Result3>(result));
        // BOOST_CHECK_EQUAL(boost::get<Result3>(result).collection,
        //     std::vector<int32_t>({1, 2, 3, 4, 6}));
    }
}

// BOOST_AUTO_TEST_CASE(visit_boost_multi_array)
// {
//     // 2 + 1
//     {
//         using Array = boost::multi_array<int32_t, 2>;
//         // using Index = Array::index;
//         auto extents(boost::extents[30000][40000]);
//
//         Array array1(extents);
//         fern::Array<int32_t> expression1(array1);
//         using Result1 = decltype(expression1)::result_type;
//
//         Array array2(extents);
//         fern::Array<int32_t> expression2(array2);
//         using Result2 = decltype(expression2)::result_type;
//
//         using Result3 = typename fern::Plus<Result1, Result2>::result_type;
//
//         fern::Operation<Result3> expression3(
//             "plus",
//             fern::Implementation(
//                 fern::Plus<Result1, Result2>()),
//             {
//                 expression1,
//                 expression2
//             }
//         );
//
//         // fern::Data result(fern::evaluate(expression3));
//
//         // BOOST_REQUIRE_NO_THROW(boost::get<Result3>(result));
//         // BOOST_CHECK_EQUAL(boost::get<Result3>(result).value, 3);
//     }
// }


#ifndef PYTHONIC_INCLUDE_NUMPY_SQUARE_HPP
#define PYTHONIC_INCLUDE_NUMPY_SQUARE_HPP

#include "pythonic/include/types/numpy_op_helper.hpp"
#include "pythonic/include/utils/meta.hpp"
#include "pythonic/include/utils/numpy_traits.hpp"
#include "pythonic/include/utils/functor.hpp"

#include <boost/simd/function/sqr.hpp>
#include <complex>

#ifdef USE_GMP
#include "pythonic/include/types/long.hpp"
#endif

namespace wrapper
{
}

namespace pythonic
{

  namespace numpy
  {

    namespace wrapper
    {
#ifdef USE_GMP
      template <class T, class U>
      auto square(__gmp_expr<T, U> const &a) -> decltype(a *a)
      {
        return a * a;
      }
#endif
      template <class T>
      std::complex<T> square(std::complex<T> const &arg)
      {
        return arg * arg;
      }
      template <class T>
      auto square(T const &arg) -> decltype(boost::simd::sqr(arg))
      {
        return boost::simd::sqr(arg);
      }
    }

#define NUMPY_NARY_FUNC_NAME square
#define NUMPY_NARY_FUNC_SYM wrapper::square
#include "pythonic/include/types/numpy_nary_expr.hpp"
  }
}

#endif


/*
@copyright Louis Dionne 2015
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
 */

#include <boost/hana/applicative.hpp>
#include <boost/hana/assert.hpp>
#include <boost/hana/comparable.hpp>
#include <boost/hana/config.hpp>
#include <boost/hana/foldable.hpp>
#include <boost/hana/functional/curry.hpp>
#include <boost/hana/functional/flip.hpp>
#include <boost/hana/functional/partial.hpp>
#include <boost/hana/functor.hpp>
#include <boost/hana/monad.hpp>
#include <boost/hana/sequence.hpp>
#include <boost/hana/traversable.hpp>
#include <boost/hana/tuple.hpp>

#include <utility>

struct Tree;

template <typename X, typename Subforest>
struct node_type {
    struct hana { using datatype = Tree; };
    X value;
    Subforest subforest;
};

auto forest = boost::hana::make_tuple;

auto node = [](auto x, auto subforest) {
    return node_type<decltype(x), decltype(subforest)>{x, subforest};
};

namespace boost { namespace hana {
    //////////////////////////////////////////////////////////////////////////
    // Comparable
    //////////////////////////////////////////////////////////////////////////
    template <>
    struct equal_impl<Tree, Tree> {
        template <typename N1, typename N2>
        static constexpr decltype(auto) apply(N1&& n1, N2&& n2) {
            return and_(
                equal(std::forward<N1>(n1).value, std::forward<N2>(n2).value),
                equal(std::forward<N1>(n1).subforest, std::forward<N2>(n2).subforest)
            );
        }
    };

    //////////////////////////////////////////////////////////////////////////
    // Functor
    //////////////////////////////////////////////////////////////////////////
    template <>
    struct transform_impl<Tree> {
        template <typename N, typename F>
        static constexpr decltype(auto) apply(N&& n, F f) {
            auto g = [=](auto&& subtree) -> decltype(auto) {
                return transform(std::forward<decltype(subtree)>(subtree), f);
            };
            return node(
                f(std::forward<N>(n).value),
                transform(std::forward<N>(n).subforest, g)
            );
        }
    };

    //////////////////////////////////////////////////////////////////////////
    // Applicative
    //////////////////////////////////////////////////////////////////////////
    template <>
    struct lift_impl<Tree> {
        template <typename X>
        static constexpr decltype(auto) apply(X&& x)
        { return node(std::forward<X>(x), forest()); }
    };

    template <>
    struct ap_impl<Tree> {
        template <typename F, typename X>
        static constexpr decltype(auto) apply(F&& f, X&& x) {
            return node(
                f.value(x.value),
                concat(
                    transform(x.subforest, partial(flip(transform), f.value)),
                    transform(f.subforest, partial(flip(ap), x))
                )
            );
        }
    };

    //////////////////////////////////////////////////////////////////////////
    // Monad
    //////////////////////////////////////////////////////////////////////////
    template <>
    struct flatten_impl<Tree> {
        template <typename N>
        static constexpr decltype(auto) apply(N&& n) {
            return node(
                std::forward<N>(n).value.value,
                concat(
                    std::forward<N>(n).value.subforest,
                    transform(std::forward<N>(n).subforest, flatten)
                )
            );
        }
    };

    //////////////////////////////////////////////////////////////////////////
    // Foldable
    //////////////////////////////////////////////////////////////////////////
    template <>
    struct foldl_impl<Tree> {
        template <typename N, typename S, typename F>
        static constexpr decltype(auto) apply(N&& n, S&& s, F f) {
            return foldl(
                std::forward<N>(n).subforest,
                f(std::forward<S>(s), std::forward<N>(n).value),
                [=](auto&& state, auto&& subtree) -> decltype(auto) {
                    return foldl(
                        std::forward<decltype(subtree)>(subtree),
                        std::forward<decltype(state)>(state),
                        f
                    );
                }
            );
        }
    };

    template <>
    struct foldr_impl<Tree> {
        template <typename N, typename S, typename F>
        static constexpr decltype(auto) apply(N&& n, S&& s, F f) {
            return f(
                std::forward<N>(n).value,
                foldr(std::forward<N>(n).subforest, std::forward<S>(s),
                    [=](auto&& subtree, auto&& state) -> decltype(auto) {
                        return foldr(
                            std::forward<decltype(subtree)>(subtree),
                            std::forward<decltype(state)>(state),
                            f
                        );
                    }
                )
            );
        }
    };

    //////////////////////////////////////////////////////////////////////////
    // Traversable
    //////////////////////////////////////////////////////////////////////////
    template <>
    struct traverse_impl<Tree> {
        template <typename A, typename N, typename F>
        static constexpr decltype(auto) apply(N&& n, F&& f) {
            return hana::ap(
                hana::transform(f(std::forward<N>(n).value), curry<2>(node)),
                traverse<A>(
                    std::forward<N>(n).subforest,
                    hana::partial(hana::flip(traverse<A>), f)
                )
            );
        }
    };
}}

int main() {
    BOOST_HANA_CONSTEXPR_LAMBDA auto tree = node(1, forest(
        node(2, forest()),
        node(3, forest()),
        node(4, forest())
    ));

    BOOST_HANA_CONSTEXPR_CHECK(boost::hana::sum(tree) == 10);
}


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

MSI applications for interactive analysis in MITK (M2aia)

Copyright (c) Jonas Cordes

All rights reserved.

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

See LICENSE.txt for details.

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

#include "mitkIOUtil.h"
#include <algorithm>
#include <m2Normalization.h>
#include <m2TestingConfig.h>
#include <mitkTestFixture.h>
#include <mitkTestingMacros.h>
#include <numeric>
#include <random>

//#include <boost/algorithm/string.hpp>

class m2CalibrationTestSuite : public mitk::TestFixture
{
  CPPUNIT_TEST_SUITE(m2CalibrationTestSuite);
  MITK_TEST(ApplyTIC_RandomGaussianSignal_shouldReturnTrue);

  CPPUNIT_TEST_SUITE_END();

  /*
  Signal was generated once using the following procedure
    -----
  const double mean = 1.0;
    const double stddev = 0.33;
    std::default_random_engine generator;
    generator.seed(142191);
    std::normal_distribution<double> dist(mean, stddev);

    std::vector<double> signal;
    auto it = std::inserter(signal, std::begin(signal));
    for (int i = 0; i < 1000; ++i)
      it = dist(generator);
    -----
  */

private:
  std::vector<double> ReadDoubleVector(const std::string &fileNameInM2aiaDir, char delim = '\n')
  {
    std::vector<double> signal;
    std::ifstream f(GetTestDataFilePath(fileNameInM2aiaDir, M2AIA_DATA_DIR));
    std::string line;
    while (std::getline(f, line, delim))
    {
      signal.push_back(std::stod(line));
    }
    return signal;
  }

  template <class T>
  void WriteVector(
    const std::vector<T> & vec, const std::string &fileNameInM2aiaDir, char delim = '\n', std::function<double(T &)> &func = [](T &t) {
    return t; })
  {
    std::ofstream f(GetTestDataFilePath(fileNameInM2aiaDir, M2AIA_DATA_DIR));

    for (int i = 0; i < vec.size(); i++)
    {
      f << std::setprecision(128) << vec[i];
      if (i < vec.size() - 1)
        f << delim;
    }
  }

public:
  void ApplyTIC_RandomGaussianSignal_shouldReturnTrue()
  {
    /*{
      std::ofstream f(GetTestDataFilePath("quadratic.data", M2AIA_DATA_DIR));

      for (int i = 0; i < 1000; i++)
      {
        f << std::setprecision(128) << -0.00012 * i * i + 0.32 * i + 800;
        if (i < 1000 - 1)
          f << "\n";
      }
    }*/
    std::vector<double> signal = ReadDoubleVector("signal.data");
    std::vector<double> mzs = ReadDoubleVector("quadratic.data");
    std::vector<double> expected = ReadDoubleVector("quadratic_tic_calibration.result");

    std::vector<double> result(signal.size());
    double tic = m2::Signal::TotalIonCurrent(std::begin(mzs), std::end(mzs), std::begin(signal));
    MITK_INFO << tic;
    std::transform(std::begin(signal), std::end(signal), std::begin(result), [&tic](const auto &v) { return v / tic; });

    CPPUNIT_ASSERT(std::equal(std::begin(result), std::end(result), std::begin(expected)));
  }
};

MITK_TEST_SUITE_REGISTRATION(m2Calibration)


#ifndef HELPER_FUNCTIONS_HPP_INCLUDED
#define HELPER_FUNCTIONS_HPP_INCLUDED

#include <iostream>
#include <OpenMesh/Core/IO/MeshIO.hh>
#include <OpenMesh/Core/Mesh/TriMesh_ArrayKernelT.hh>
//#include <OpenMesh/Core/IO/reader/OBJReader.hh>
//#include <OpenMesh/Core/IO/writer/OBJWriter.hh>
#include <nanoflann.hpp>
#include <Eigen/Dense>
#include <Eigen/SparseCore>
#include "../global.hpp"

typedef OpenMesh::TriMesh_ArrayKernelT<>  TriMesh;
typedef Eigen::SparseMatrix<float, 0, int> SparseMat;
typedef Eigen::Matrix< int, Eigen::Dynamic, Eigen::Dynamic> MatDynInt; //matrix MxN of type unsigned int
typedef Eigen::Matrix< int, Eigen::Dynamic, 3> FacesMat; //matrix Mx3 of type unsigned int
typedef Eigen::VectorXf VecDynFloat;
typedef Eigen::Matrix< float, Eigen::Dynamic, Eigen::Dynamic> MatDynFloat; //matrix MxN of type float
typedef Eigen::Matrix< float, Eigen::Dynamic, registration::NUM_FEATURES> FeatureMat; //matrix Mx6 of type float
typedef Eigen::MatrixX3f Vec3Mat;

namespace registration {

void fuse_affinities(SparseMat &ioAffinity1,
                    const SparseMat &inAffinity2);

void normalize_sparse_matrix(SparseMat &ioMat);

template <typename VecMatType>
void radius_nearest_neighbours(const VecMatType &inQueriedPoints,
                                const VecMatType &inSourcePoints,
                                MatDynInt &outNeighbourIndices,
                                MatDynFloat &outNeighbourSquaredDistances,
                                const float paramRadius = 3.0,
                                const size_t paramLeafsize = 15);

void convert_mesh_to_matrices(const TriMesh &inMesh,
                                FeatureMat &outFeatures,
                                FacesMat &outFaces);

void convert_mesh_to_matrices(const TriMesh &inMesh,
                                FeatureMat &outFeatures);

void convert_mesh_to_matrices(const TriMesh &inMesh,
                                FeatureMat &outFeatures,
                                FacesMat &outFaces,
                                VecDynFloat &outFlags);

void convert_matrices_to_mesh(const Vec3Mat &inPositions,
                                const FacesMat &inFaces,
                                TriMesh &outMesh);

void convert_matrices_to_mesh(const FeatureMat &inFeatures,
                                const FacesMat &inFaces,
                                TriMesh &outMesh);

void convert_matrices_to_mesh(const FeatureMat &inFeatures,
                                const FacesMat &inFaces,
                                const VecDynFloat &inFlags,
                                TriMesh &outMesh);

//void load_obj_to_eigen(const std::string inObjFilename,
//                                TriMesh &outMesh,
//                                FeatureMat &outFeatureMatrix);

//void write_eigen_to_obj(const FeatureMat &inFeatures,
//                                TriMesh &inMesh,
//                                const std::string inObjFilename);

//bool import_data(const std::string inFloatingMeshPath,
//                 const std::string inTargetMeshPath,
//                 FeatureMat &outFloatingFeatures,
//                 FeatureMat &outTargetFeatures,
//                 FacesMat &outFloatingFaces,
//                 FacesMat &outTargetFaces);

//bool export_data(FeatureMat &inResultFeatures,
//                 FacesMat &inResultFaces,
//                 const std::string inResultMeshPath);

void update_normals_for_altered_positions(TriMesh &ioMesh,
                                        FeatureMat &ioFeatures);

void update_normals_for_altered_positions(const Vec3Mat &inPositions,
                                        const FacesMat &inFaces,
                                        Vec3Mat &outNormals);

void update_normals_safely(const FeatureMat &features, TriMesh &mesh);

}//namespace registration
#endif // HELPER_FUNCTIONS_HPP_INCLUDED


#pragma once

#include <armadillo>
#include <algorithm>
#include <random>
#include <cassert>

#include "net.hpp"

template <typename activation = Logistic, typename error = Squared_Error>
void train_online(FeedForward_Network<activation, error>& network,
    arma::Mat<float> inputs, arma::Mat<float> targets, float learning_rate) {
    for (int i = 0; i < targets.n_rows; ++i) {
      calculate_activation(network, inputs.row(i));
      backprop(network, targets.row(i), learning_rate);
    }
}

template <typename activation, typename error>
void train_batch(FeedForward_Network<activation, error>& network,
    arma::Mat<float> inputs, arma::Mat<float> targets, int batch_size, float learning_rate) {
    network.resize_activation(batch_size);

    int batches_in_train = targets.n_rows/batch_size - 1;
    for (int i = 0; i < batches_in_train; ++i) {
      arma::Mat<float> input_slice = inputs.rows(i*batch_size, (i+1) * batch_size-1);
      calculate_activation(network, input_slice);
      arma::Mat<float> target_slice = targets.rows(i*batch_size, (i+1) * batch_size-1);
      backprop(network, target_slice, learning_rate);
    }
}

//Randomize weights in a network.
template <typename activation, typename error>
void randomize(FeedForward_Network<activation, error>& network, float standard_deviation = 0.05) {
  std::default_random_engine generator;
  std::normal_distribution<float> distribution(0, standard_deviation);

  auto random_num = [&]() {return distribution(generator);};
  for (int i=0; i < network.weights.size(); ++i) {
    network.weights[i].imbue(random_num);
    //TODO figure out which one is right
    //avoid local nearby local maximum
    network.last_weights[i].imbue(random_num);
    //network.last_weights[i] = network.weights[i];
  }
}

template <typename arma_t, typename activation, typename error>
void backprop(FeedForward_Network<activation, error> &network,
    arma_t target, float learning_rate = 0.8f, float momentum = 0.8f) {
  //Calculate deltas

  //output delta first
  network.deltas.back() = error::error_dir(target, network.activations.back()) % activation::activation_dir(network.activations.back());

  //rest of the delta
  for (int i = network.deltas.size() - 2; i >= 0; --i) {
    network.deltas[i] = (network.deltas[i+1] * network.weights[i+1].t()) % activation::activation_dir(network.activations[i+1]);
  }

  //update weights
  for (int i=0; i < network.weights.size(); ++i) {
    auto & standard_piece = (1 - momentum) * learning_rate * (network.deltas[i].t() * network.activations[i]).t();
    auto & momentum_piece = momentum * (network.weights[i] - network.last_weights[i]);
    arma::Mat<float> delta_weights = standard_piece + momentum_piece;
    network.last_weights[i] = network.weights[i];
    network.weights[i] += delta_weights;
  }
}

template <typename arma_t, typename activation, typename error>
void calculate_activation(FeedForward_Network<activation, error>& network,
    arma_t input) {

  network.activations[0] = input;
  for(int i=1; i < network.activations.size(); ++i) {
    network.activations[i] = network.activations[i-1] * network.weights[i-1];
    network.activations[i] = activation::activation(network.activations[i]);
  }
}

//TODO remove this function??
template <typename activation, typename error>
arma::Mat<float> predict(FeedForward_Network<activation, error>& network,
    arma::Mat<float> input) {
  calculate_activation(network, input);
  return network.activations.back();
}

//Scoring function for classification.
inline double classify_percent_score(arma::Mat<float> result, arma::Mat<float> correct) {
  assert(result.n_cols == correct.n_cols);
  int num_correct = 0;
  for (int i=0; i < result.n_rows; ++i) {
    auto sort_vec = arma::sort_index(result.row(i), 1);
    if (correct.row(i)[sort_vec[0]] == 1) {
      num_correct += 1;
    }
  }
  return static_cast<float>(num_correct) / static_cast<float>(result.n_rows);
}

//Scoring function that calculates the difference in squares between two matrices.
inline float squared_diff(arma::Mat<float> result, arma::Mat<float> correct) {
  assert(result.n_cols == correct.n_cols);
  auto error_diff = correct - result;
  return arma::accu(error_diff % error_diff);
}


//
// Created by yalavrinenko on 18.06.2021.
//

#ifndef TRASH_ROGAINE_SOLVER_FULL_LINKED_GRAPH_HPP
#define TRASH_ROGAINE_SOLVER_FULL_LINKED_GRAPH_HPP

#include <array>
#include <boost/graph/astar_search.hpp>
#include <boost/graph/grid_graph.hpp>
#include <boost/functional/hash.hpp>
#include <opencv2/opencv.hpp>
#include <tuple>
#include <utility>

namespace trs{
  class full_linked_grid {
  public:
    using vertex_t = std::array<size_t, 2>;
    using edge_t = std::pair<vertex_t, vertex_t>;

    struct target_visitor : public boost::default_astar_visitor {
      struct target_reached {
      };

      explicit target_visitor(vertex_t to) : target_{std::move(to)} {}

      void examine_vertex(vertex_t u, const full_linked_grid &) {
        if (u == target_)
          throw target_visitor::target_reached();
      }

    private:
      vertex_t target_;
    };
    using visitor_t = target_visitor;

    struct graph_vertex_hash {
      size_t operator()(vertex_t const &u) const {
        size_t hash = 0;
        boost::hash_combine(hash, u[0]);
        boost::hash_combine(hash, u[1]);
        return hash;
      }
    };
    using hash_t = graph_vertex_hash;

    explicit full_linked_grid(cv::Mat  mat): road_map_(std::move(mat)) {}

    [[nodiscard]] bool check_vertex(vertex_t const& v) const {
      return v[0] < static_cast<size_t>(road_map_.rows) && v[1] < static_cast<size_t>(road_map_.cols) &&
             road_map_.at<char>(static_cast<int>(v[0]), static_cast<int>(v[1]));
    }

    struct edge_iterator : public boost::iterator_facade<edge_iterator, edge_t, boost::forward_traversal_tag, edge_t>{
      edge_iterator(vertex_t vertex, full_linked_grid const &graph): graph_{graph}, current_(vertex){
        next_ = next_vertex();
      }

      edge_t operator* () const {
        return {current_, next_};
      }

      bool operator == (edge_iterator const& rhs) const {
        return current_ == rhs.current_ && next_ == rhs.next_;
      }

      bool operator != (edge_iterator const& rhs) const {
        return !(*this == rhs);
      }

      [[nodiscard]] bool equal(edge_iterator const& rhs) const {
        return this->operator==(rhs);
      }

      void operator++ () {
        next_ = next_vertex();
      }

      explicit operator bool() const {
        return has_vertex_;
      }

      void increment() {
        ++(*this);
      }

    private:
      vertex_t next_vertex() {
        has_vertex_ = false;
        next_ = current_;
        while (!has_vertex_ && current_shift_ < shifts_.size()){
          next_[0] = current_[0] + shifts_[current_shift_].first;
          next_[1] = current_[1] + shifts_[current_shift_].second;
          ++current_shift_;

          if (graph_.check_vertex(next_))
            has_vertex_ = true;
        }
        return next_;
      }

      size_t current_shift_ = 0;
      bool has_vertex_;
      full_linked_grid const &graph_;
      vertex_t current_;
      vertex_t next_;

      std::array<std::pair<int, int>, 8> shifts_ = {
          std::pair{-1, 1}, {0, 1}, {1, 1},
          {-1, 0}, {1, 0},
          {-1, -1}, {0, -1}, {1, -1}
      };
    };

    [[nodiscard]] auto& graph() const{
      return *this;
    }

    static visitor_t visitor(vertex_t const& v) {
      return target_visitor(v);
    }

    static vertex_t vertex2d(size_t row, size_t col) {
      return vertex_t {row, col};
    }

  private:
    cv::Mat road_map_;
  };

  template<>
  struct rogain_graph_traits<full_linked_grid>{
    using graph_t = full_linked_grid;
    using vertex_t = typename full_linked_grid::vertex_t;
    using visitor_t = typename full_linked_grid::visitor_t;
    using hash_t = typename full_linked_grid::hash_t;
    using stop_exception_t = full_linked_grid::visitor_t::target_reached;
  };

  template<>
  struct vertex_accessor<rogain_graph_traits<full_linked_grid>::vertex_t> {
    static size_t field(rogain_graph_traits<full_linked_grid>::vertex_t const& v, size_t id) {
      return v[id];
    }
  };
}

namespace boost{
  template<>
  struct graph_traits<trs::full_linked_grid>{
    using vertex_descriptor = trs::full_linked_grid::vertex_t;
    using edge_descriptor = trs::full_linked_grid::edge_t;
    using directed_category = boost::undirected_tag;
    using edge_parallel_category = boost::disallow_parallel_edge_tag;
    using traversal_category = boost::incidence_graph_tag;

    using out_edge_iterator = trs::full_linked_grid::edge_iterator;
    using degree_size_type = size_t;
    using edges_size_type = size_t;
  };
}

#endif//TRASH_ROGAINE_SOLVER_FULL_LINKED_GRAPH_HPP


#ifndef PYTHONIC_INCLUDE_NUMPY_EXP_HPP
#define PYTHONIC_INCLUDE_NUMPY_EXP_HPP

#include "pythonic/include/utils/functor.hpp"
#include "pythonic/include/types/ndarray.hpp"
#include "pythonic/include/utils/numpy_traits.hpp"

#include <boost/simd/function/exp.hpp>
#include <cmath>

PYTHONIC_NS_BEGIN

namespace numpy
{
  namespace wrapper
  {
    template <class T>
    std::complex<T> exp(std::complex<T> const &val)
    {
      return std::exp(val);
    }
    template <class T>
    auto exp(T const &val) -> decltype(boost::simd::exp(val))
    {
      return boost::simd::exp(val);
    }
  }
#define NUMPY_NARY_FUNC_NAME exp
#define NUMPY_NARY_FUNC_SYM wrapper::exp
#include "pythonic/include/types/numpy_nary_expr.hpp"
}
PYTHONIC_NS_END

#endif


// -------------------------------------------------------------------------------------------------
//                              Copyright 2016 - NumScale SAS
//
//                   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 <ns.bench.hpp>
#include <boost/simd/function/simd/is_nlt.hpp>
#include <cmath>

namespace bs = boost::simd;
namespace nsb = ns::bench;

template <typename T>
struct is_nlt_simd
{
   template <typename U>
   void operator()(U min0, U max0, U min1, U max1)
   {
         { return bs::is_nlt(x0, x1); }
       );
   }
};

int main(int argc, char **argv) {
   nsb::parse_args(argc, argv);
   nsb::make_for_each<is_nlt_simd, NS_BENCH_SIGNED_NUMERIC_TYPES>( -10,  10,  -10,  10);
   nsb::make_for_each<is_nlt_simd, NS_BENCH_UNSIGNED_NUMERIC_TYPES>(0,  10, 0,  10);
   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.

// This file was modified by Oracle on 2014.
// Modifications copyright (c) 2014 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)

#include <geometry_test_common.hpp>

#include <boost/concept_check.hpp>

#include <boost/geometry/strategies/geographic/distance_vincenty.hpp>
#include <boost/geometry/algorithms/detail/vincenty_inverse.hpp>
#include <boost/geometry/algorithms/detail/vincenty_direct.hpp>

#include <boost/geometry/core/srs.hpp>
#include <boost/geometry/strategies/strategies.hpp>
#include <boost/geometry/algorithms/assign.hpp>
#include <boost/geometry/geometries/point.hpp>
#include <test_common/test_point.hpp>

#ifdef HAVE_TTMATH
#  include <boost/geometry/extensions/contrib/ttmath_stub.hpp>
#endif

template <typename T>
void normalize_deg(T & deg)
{
    while ( deg > T(180) )
        deg -= T(360);
    while ( deg <= T(-180) )
        deg += T(360);
}

template <typename T>
T difference_deg(T const& a1, T const& a2)
{
    T d = a1 - a2;
    normalize_deg(d);
    return d;
}

template <typename T>
void check_deg(std::string const& name, T const& a1, T const& a2, T const& percent, T const& error)
{
    T diff = bg::math::abs(difference_deg(a1, a2));

    if ( bg::math::equals(a1, T(0)) || bg::math::equals(a2, T(0)) )
    {
        if ( diff > error )
        {
            BOOST_ERROR(name << " - the difference {" << diff << "} between {" << a1 << "} and {" << a2 << "} exceeds {" << error << "}");
        }
    }
    else
    {
        T greater = (std::max)(bg::math::abs(a1), bg::math::abs(a2));

        if ( diff > greater * percent / T(100) )
        {
            BOOST_ERROR(name << " the difference {" << diff << "} between {" << a1 << "} and {" << a2 << "} exceeds {" << percent << "}%");
        }
    }
}

double azimuth(double deg, double min, double sec)
{
    min = fabs(min);
    sec = fabs(sec);

    if ( deg < 0 )
    {
        min = -min;
        sec = -sec;
    }

    return deg + min/60.0 + sec/3600.0;
}

double azimuth(double deg, double min)
{
    return azimuth(deg, min, 0.0);
}

template <typename P>
bool non_precise_ct()
{
    typedef typename bg::coordinate_type<P>::type ct;
    return boost::is_integral<ct>::value || boost::is_float<ct>::value;
}

template <typename P1, typename P2, typename Spheroid>
void test_vincenty(double lon1, double lat1, double lon2, double lat2,
                   double expected_distance,
                   double expected_azimuth_12,
                   double expected_azimuth_21,
                   Spheroid const& spheroid)
{
    typedef typename bg::promote_floating_point
        <
            typename bg::select_calculation_type<P1, P2, void>::type
        >::type calc_t;

    calc_t tolerance = non_precise_ct<P1>() || non_precise_ct<P2>() ?
                       5.0 : 0.001;
    calc_t error = non_precise_ct<P1>() || non_precise_ct<P2>() ?
                   1e-5 : 1e-12;

    // formula
    {
        bg::detail::vincenty_inverse<calc_t> vi(lon1 * bg::math::d2r,
                                                lat1 * bg::math::d2r,
                                                lon2 * bg::math::d2r,
                                                lat2 * bg::math::d2r,
                                                spheroid);
        calc_t dist = vi.distance();
        calc_t az12 = vi.azimuth12();
        calc_t az21 = vi.azimuth21();

        calc_t az12_deg = az12 * bg::math::r2d;
        calc_t az21_deg = az21 * bg::math::r2d;

        BOOST_CHECK_CLOSE(dist, calc_t(expected_distance), tolerance);
        check_deg("az12_deg", az12_deg, calc_t(expected_azimuth_12), tolerance, error);
        check_deg("az21_deg", az21_deg, calc_t(expected_azimuth_21), tolerance, error);

        bg::detail::vincenty_direct<calc_t> vd(lon1 * bg::math::d2r,
                                               lat1 * bg::math::d2r,
                                               dist,
                                               az12,
                                               spheroid);
        calc_t direct_lon2 = vd.lon2();
        calc_t direct_lat2 = vd.lat2();
        calc_t direct_az21 = vd.azimuth21();

        calc_t direct_lon2_deg = direct_lon2 * bg::math::r2d;
        calc_t direct_lat2_deg = direct_lat2 * bg::math::r2d;
        calc_t direct_az21_deg = direct_az21 * bg::math::r2d;

        check_deg("direct_lon2_deg", direct_lon2_deg, calc_t(lon2), tolerance, error);
        check_deg("direct_lat2_deg", direct_lat2_deg, calc_t(lat2), tolerance, error);
        check_deg("direct_az21_deg", direct_az21_deg, az21_deg, tolerance, error);
    }

    // strategy
    {
        typedef bg::strategy::distance::vincenty<Spheroid> vincenty_type;

        BOOST_CONCEPT_ASSERT(
            (
                bg::concept::PointDistanceStrategy<vincenty_type, P1, P2>)
            );

        vincenty_type vincenty(spheroid);
        typedef typename bg::strategy::distance::services::return_type<vincenty_type, P1, P2>::type return_type;

        P1 p1;
        P2 p2;

        bg::assign_values(p1, lon1, lat1);
        bg::assign_values(p2, lon2, lat2);

        BOOST_CHECK_CLOSE(vincenty.apply(p1, p2), return_type(expected_distance), tolerance);
        BOOST_CHECK_CLOSE(bg::distance(p1, p2, vincenty), return_type(expected_distance), tolerance);
    }
}

template <typename P1, typename P2>
void test_vincenty(double lon1, double lat1, double lon2, double lat2,
                   double expected_distance,
                   double expected_azimuth_12,
                   double expected_azimuth_21)
{
    test_vincenty<P1, P2>(lon1, lat1, lon2, lat2,
                          expected_distance, expected_azimuth_12, expected_azimuth_21,
                          bg::srs::spheroid<double>());
}

template <typename P1, typename P2>
void test_all()
{
    // See:
    //  - http://www.ga.gov.au/geodesy/datums/vincenty_inverse.jsp
    //  - http://www.ga.gov.au/geodesy/datums/vincenty_direct.jsp
    // Values in the comments below was calculated using the above pages
    // in some cases distances may be different, previously used values was left

    // use km
    double gda_a = 6378.1370;
    double gda_f = 1.0 / 298.25722210;
    double gda_b = gda_a * ( 1.0 - gda_f );
    bg::srs::spheroid<double> gda_spheroid(gda_a, gda_b);

    // Test fractional coordinates only for non-integral types
    if ( BOOST_GEOMETRY_CONDITION(
            ! boost::is_integral<typename bg::coordinate_type<P1>::type>::value
         && ! boost::is_integral<typename bg::coordinate_type<P2>::type>::value  ) )
    {
        // Flinders Peak -> Buninyong
        test_vincenty<P1, P2>(azimuth(144,25,29.52440), azimuth(-37,57,3.72030),
                              azimuth(143,55,35.38390), azimuth(-37,39,10.15610),
                              54.972271, azimuth(306,52,5.37), azimuth(127,10,25.07),
                              gda_spheroid);
    }

    // Lodz -> Trondheim
    test_vincenty<P1, P2>(azimuth(19,28), azimuth(51,47),
                          azimuth(10,21), azimuth(63,23),
                          1399.032724, azimuth(340,54,25.14), azimuth(153,10,0.19),
                          gda_spheroid);
    // London -> New York
    test_vincenty<P1, P2>(azimuth(0,7,39), azimuth(51,30,26),
                          azimuth(-74,0,21), azimuth(40,42,46),
                          5602.044851, azimuth(288,31,36.82), azimuth(51,10,33.43),
                          gda_spheroid);

    // Shanghai -> San Francisco
    test_vincenty<P1, P2>(azimuth(121,30), azimuth(31,12),
                          azimuth(-122,25), azimuth(37,47),
                          9899.698550, azimuth(45,12,44.76), azimuth(309,50,20.88),
                          gda_spheroid);

    test_vincenty<P1, P2>(0, 0, 0, 50, 5540.847042, 0, 180, gda_spheroid); // N
    test_vincenty<P1, P2>(0, 0, 0, -50, 5540.847042, 180, 0, gda_spheroid); // S
    test_vincenty<P1, P2>(0, 0, 50, 0, 	5565.974540, 90, -90, gda_spheroid); // E
    test_vincenty<P1, P2>(0, 0, -50, 0, 5565.974540, -90, 90, gda_spheroid); // W

    test_vincenty<P1, P2>(0, 0, 50, 50, 7284.879297, azimuth(32,51,55.87), azimuth(237,24,50.12), gda_spheroid); // NE

    // The original distance values, azimuths calculated using the web form mentioned above
    // Using default spheroid units (meters)
    test_vincenty<P1, P2>(0, 89, 1, 80, 1005153.5769, azimuth(178,53,23.85), azimuth(359,53,18.35)); // sub-polar
    test_vincenty<P1, P2>(4, 52, 4, 52, 0.0, 0, 0); // no point difference
    test_vincenty<P1, P2>(4, 52, 3, 40, 1336039.890, azimuth(183,41,29.08), azimuth(2,58,5.13)); // normal case
}

template <typename P>
void test_all()
{
    test_all<P, P>();
}

int test_main(int, char* [])
{
    //test_all<float[2]>();
    //test_all<double[2]>();
    test_all<bg::model::point<double, 2, bg::cs::geographic<bg::degree> > >();
    test_all<bg::model::point<float, 2, bg::cs::geographic<bg::degree> > >();
    test_all<bg::model::point<int, 2, bg::cs::geographic<bg::degree> > >();

#if defined(HAVE_TTMATH)
    test_all<bg::model::point<ttmath::Big<1,4>, 2, bg::cs::geographic<bg::degree> > >();
    test_all<bg::model::point<ttmath_big, 2, bg::cs::geographic<bg::degree> > >();
#endif

    return 0;
}


#ifndef CIRCLE_DETECTION_INCLUDE_GAURD
#define CIRCLE_DETECTION_INCLUDE_GAURD
#include <Eigen/Dense>
#include <vector>
#include "rigid2d/rigid2d.hpp"

using std::vector;
using Eigen::Vector3d;
using rigid2d::Vector2D;

namespace circleDetection
{
struct Point2d
{
  Point2d(double x, double y);
  double x, y;
};

Vector3d fitCircle(vector<Vector2D> circlePoints);
double calculateError(const vector<Vector2D>& observedPoints, Vector3d circleCoeff);
}

#endif


//=======================================================================
// Copyright 2007 Aaron Windsor
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//=======================================================================
#include <iostream>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/properties.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/property_map/property_map.hpp>
#include <boost/ref.hpp>
#include <vector>

#include <boost/graph/biconnected_components.hpp>
#include <boost/graph/make_biconnected_planar.hpp>
#include <boost/graph/boyer_myrvold_planar_test.hpp>

using namespace boost;

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

  typedef adjacency_list
    < vecS,
      vecS,
      undirectedS,
      property<vertex_index_t, int>,
      property<edge_index_t, int>
    >
    graph;

  graph g(11);
  add_edge(0,1,g);
  add_edge(2,3,g);
  add_edge(3,0,g);
  add_edge(3,4,g);
  add_edge(4,5,g);
  add_edge(5,3,g);
  add_edge(5,6,g);
  add_edge(6,7,g);
  add_edge(7,8,g);
  add_edge(8,5,g);
  add_edge(8,9,g);
  add_edge(0,10,g);

  //Initialize the interior edge index
  property_map<graph, edge_index_t>::type e_index = get(edge_index, g);
  graph_traits<graph>::edges_size_type edge_count = 0;
  graph_traits<graph>::edge_iterator ei, ei_end;
  for(tie(ei, ei_end) = edges(g); ei != ei_end; ++ei)
    put(e_index, *ei, edge_count++);

  //Test for planarity; compute the planar embedding as a side-effect
  typedef std::vector< graph_traits<graph>::edge_descriptor > vec_t;
  std::vector<vec_t> embedding(num_vertices(g));
  if (boyer_myrvold_planarity_test(boyer_myrvold_params::graph = g,
                                   boyer_myrvold_params::embedding =
                                     &embedding[0]
                                   )
      )
    std::cout << "Input graph is planar" << std::endl;
  else
    std::cout << "Input graph is not planar" << std::endl;

  typedef std::vector< graph_traits<graph>::edges_size_type >
    component_storage_t;
  typedef iterator_property_map
    < component_storage_t::iterator,
      property_map<graph, edge_index_t>::type
    >
    component_map_t;

  component_storage_t component_storage(num_edges(g));
  component_map_t component(component_storage.begin(), get(edge_index, g));

  std::cout << "Before calling make_biconnected_planar, the graph has "
            << biconnected_components(g, component)
            << " biconnected components" << std::endl;

  make_biconnected_planar(g, &embedding[0]);

  // Re-initialize the edge index, since we just added a few edges
  edge_count = 0;
  for(tie(ei, ei_end) = edges(g); ei != ei_end; ++ei)
    put(e_index, *ei, edge_count++);

  // Re-size the storage for the biconnected components, since we
  // just added a few edges

  component_storage.resize(num_edges(g));
  component = component_map_t(component_storage.begin(), get(edge_index,g));

  std::cout << "After calling make_biconnected_planar, the graph has "
            << biconnected_components(g, component)
            << " biconnected components" << std::endl;

  if (boyer_myrvold_planarity_test(g))
    std::cout << "Also, the graph is still planar." << std::endl;
  else
    std::cout << "But the graph is not still planar." << std::endl;

  return 0;
}


/**
Good tutorial: http://www.informit.com/articles/article.aspx?p=673259
*/

#include <algorithm>
#include <fstream>
#include <iostream>
#include <set>
#include <vector>

#include <boost/config.hpp>
//-lboost_graph
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/dijkstra_shortest_paths.hpp>
#include <boost/property_map/property_map.hpp>

int main() {

    /*
    #Graph

        The following class hierarchy exists:

            BidirectionalGraph -------- Incience ---------+
                                                          |
                                        Adjacency --------+
                                                          |
            VertexAndEdgeList ----+---- VertexList -------+---- Graph
                                  |                       |
                                  +---- EdgeList ---------+
                                                          |
                                        AdjacenyMatrix ---+
    */
    {
        /*
        #properties

            Properties are values associated to edges and vertices.
        */
        {
            /*
            There are a few predefined properties which you should use whenever possible
            as they are already used in many algorithms, but you can also define your own properties.

            Predefined properties include:

            - `edge_weight_t`. Used for most algorithms that have a single value associated to each
                edge such as Dijikstra.

            - `vertex_name_t`
            */
            {
                typedef boost::property<boost::vertex_name_t, std::string> VertexProperties;
                typedef boost::property<boost::edge_weight_t, int> EdgeProperties;
            }

            /*
            Multiple properties can be specified either by:

            - using a custom class as the property type. TODO is there any limitation to this?
            - chaining multile properties
            */
            {
            }

            /*
            The absense of a property is speficied by boost::no_property.
            */
            {
                typedef boost::no_property VertexProperties;
            }

        }

        typedef boost::property<boost::vertex_name_t, std::string> VertexProperties;
        typedef boost::property<boost::edge_weight_t, int> EdgeProperties;
        typedef boost::adjacency_list<
            // Data structure to represent the out edges for each vertex.
            // Possibilities:
            //
            // #vecS selects std::vector.
            // #listS selects std::list.
            // #slistS selects std::slist.
            // #setS selects std::set.
            // #multisetS selects std::multiset.
            // #hash_setS selects std::hash_set.
            //
            // `S` standas for Selector.
            boost::vecS,

            // Data structure to represent the vertex set.
            boost::vecS,

            // Directed type.
            // #bidirectionalS: directed graph with access to in and out edges
            // #directedS:      directed graph with access only to out-edges
            // #undirectedS:    undirected graph
            boost::bidirectionalS,

            // Optional.
            VertexProperties,

            // Optional.
            EdgeProperties
        > Graph;
        //typedef boost::graph_traits<Graph>::vertex_iterator VertexIter;
        //typedef boost::graph_traits<Graph>::vertex_descriptor Vertex;
        //typedef boost::property_map<Graph, boost::vertex_index_t>::type IndexMap;

        // Fix number of vertices, and add one edge at a time.
        int num_vertices = 3;
        Graph g(num_vertices);
        boost::add_edge(0, 1, g);
        boost::add_edge(1, 2, g);

        // Fix number of vertices, and add one edge array.
        {
            int num_vertices = 3;
            typedef std::pair<int, int> Edge;
            std::vector<Edge> edges{
                {0, 1},
                {1, 2},
            };
            Graph g(edges.data(), edges.data() + edges.size(), num_vertices);
        }

        // It is also possible to add vertices with #add_vertex.

        //#vertices
        {
            // Number of vertices.
            boost::graph_traits<Graph>::vertices_size_type num_vertices = boost::num_vertices(g);
            assert(num_vertices == 3u);

            //#vertices() Returns a begin() end() vertex iterator pair so we know where to stop.
            {
                typedef std::vector<boost::graph_traits<Graph>::vertex_descriptor> Vertices;
                Vertices vertices;
                vertices.reserve(num_vertices);
                //IndexMap
                auto index = boost::get(boost::vertex_index, g);
                //std::pair<vertex_iter, vertex_iter> vp
                for (auto vp = boost::vertices(g); vp.first != vp.second; ++vp.first) {
                    // Vertex
                    auto v = *vp.first;
                    vertices.push_back(index[v]);
                }
                assert((vertices == Vertices{0, 1, 2}));
            }

            // The iterator is a ranom access iterator.
            {
                auto index = boost::get(boost::vertex_index, g);
                auto it = boost::vertices(g).first;
                assert(index[it[2]] == 2);
                assert(index[it[1]] == 1);
            }
        }

        //#edges
        {
            // It seems that only AdjencyMatrix has a method to get an edge given two vertices:
            //edge(u, v, g)
        }
    }

    //#source is also a global function: <http://stackoverflow.com/questions/16114616/why-is-boost-graph-librarys-source-a-global-function>

    //#dijikstra
    std::cout << "#dijkstra" << std::endl;
    {
        typedef boost::adjacency_list<
            boost::listS,
            boost::vecS,
            boost::directedS,
            boost::no_property,
            boost::property<boost::edge_weight_t, int>
        > Graph;
        typedef boost::graph_traits<Graph>::vertex_descriptor vertex_descriptor;
        typedef boost::graph_traits<Graph>::edge_descriptor edge_descriptor;
        typedef std::pair<int, int> Edge;

        // Model inputs.
        const int num_nodes = 5;
        const int sorce = 0;
        std::vector<Edge> edges{
            {0, 2}, {1, 1}, {1, 3}, {1, 4}, {2, 1},
            {2, 3}, {3, 4}, {4, 0}, {4, 1}
        };
        std::vector<int> weights{
            1, 2, 1, 2, 7,
            3, 1, 1, 1
        };

        // Solve.
        Graph g(edges.data(), edges.data() + edges.size(), weights.data(), num_nodes);
        std::vector<vertex_descriptor> p(num_vertices(g));
        std::vector<int> d(num_vertices(g));
        vertex_descriptor s = vertex(sorce, g);
        dijkstra_shortest_paths(g, s,
            predecessor_map(boost::make_iterator_property_map(
                p.begin(),
                boost::get(boost::vertex_index, g)
            )).distance_map(boost::make_iterator_property_map(
                d.begin(),
                boost::get(boost::vertex_index, g)
            ))
        );

        // Print solution to stdout.
        std::cout << "node | distance from source | parent" << std::endl;
        boost::graph_traits<Graph>::vertex_iterator vi, vend;
        for (boost::tie(vi, vend) = vertices(g); vi != vend; ++vi)
            std::cout << *vi << " " << d[*vi] << " " << p[*vi] << std::endl;
        std::cout <<std::endl;

        // Generate a .dot graph file with shortest path highlighted.
        // To PNG with: dot -Tpng -o outfile.png input.dot
        boost::property_map<Graph, boost::edge_weight_t>::type weightmap = boost::get(boost::edge_weight, g);
        std::ofstream dot_file("dijkstra.dot");
        dot_file << "digraph D {\n"      << "  rankdir=LR\n"           << "  size=\"4,3\"\n"
                 << "  ratio=\"fill\"\n" << "  edge[style=\"bold\"]\n" << "  node[shape=\"circle\"]\n";
        boost::graph_traits <Graph>::edge_iterator ei, ei_end;
        for (std::tie(ei, ei_end) = boost::edges(g); ei != ei_end; ++ei) {
            edge_descriptor e = *ei;
            boost::graph_traits<Graph>::vertex_descriptor
                u = boost::source(e, g), v = boost::target(e, g);
            dot_file << u << " -> " << v << "[label=\"" << boost::get(weightmap, e) << "\"";
            if (p[v] == u)
                dot_file << ", color=\"black\"";
            else
                dot_file << ", color=\"grey\"";
            dot_file << "]";
        }
        dot_file << "}";

        // Construct forward path to a destination.
        int dest = 4;
        int cur = dest;
        std::vector<int> path;
        path.push_back(cur);
        while(cur != sorce) {
            cur = p[cur];
            path.push_back(cur);
        }
        std::reverse(path.begin(), path.end());
        // Print.
        std::cout << "Path to node " << std::to_string(dest) << ":" << std::endl;
        for(auto& node : path) {
            std::cout << node << std::endl;
        }
    }
}


#include <iostream>
#include <Eigen/Dense>
#include "RigidBodyManipulator.h"

using namespace std;

int main(int argc, char* argv[])
{
  if (argc < 2) {
    cerr << "Usage: urdfManipulatorDynamicsTest urdf_filename" << endl;
    exit(-1);
  }
  RigidBodyManipulator* model = new RigidBodyManipulator(argv[1]);
  if (!model) {
    cerr << "ERROR: Failed to load model from " << argv[1] << endl;
    return -1;
  }
  cout << "=======" << endl;

  // the order of the bodies may be different in matlab, so print it out once here
  cout << model->num_bodies << endl;
  for (int i = 0; i < model->num_bodies; i++) {
    cout << model->bodies[i]->linkname << endl;
  }

  VectorXd q = VectorXd::Zero(model->num_positions);
  VectorXd v = VectorXd::Zero(model->num_velocities);
  int i;

  if (argc >= 2 + model->num_positions) {
    for (i = 0; i < model->num_positions; i++)
      sscanf(argv[2 + i], "%lf", &q(i));
  }

  if (argc >= 2 + model->num_positions + model->num_velocities) {
    for (i = 0; i < model->num_velocities; i++)
      sscanf(argv[2 + model->num_positions + i], "%lf", &v(i));
  }

  model->doKinematicsNew(q, v, true, true);

  auto H = model->massMatrix<double>();
  cout << H.value() << endl;

  map<int, unique_ptr<GradientVar<double, TWIST_SIZE, 1>> > f_ext;
  auto C = model->inverseDynamics(f_ext);
  cout << C.value() << endl;

  cout << model->B << endl;

  if (model->loops.size()>0) {
    auto phi = model->positionConstraintsNew<double>(1);
    cout << phi.value() << endl;
    cout << phi.gradient().value() << endl;
  }

  delete model;
  return 0;
}


// Solving a puzzle below
// https://sist8.com/yougun

#include <cstdlib>
#include <cmath>
#include <fstream>
#include <iomanip>
#include <iostream>
#include <limits>
#include <random>
#include <sstream>
#include <string>
#include <vector>
#include <boost/optional.hpp>
#include <boost/program_options.hpp>
#include <boost/multi_array.hpp>

namespace {
    using Count = long long int;
    // Bitmap (1 to alive) of survivors 0..(N_PLAYERS-1)
    using State = unsigned int;
    using Index = int;
    using Survivors = std::vector<Index>;

    struct Action {
        Index player;
        State state;
        Index target;
    };
    using ActionChain = std::vector<Action>;

    // Rule of the game
    constexpr Index N_PLAYERS = 3;
    constexpr Index N_STATES = 1 << N_PLAYERS;  // 2^N_PLAYERS combinations
    constexpr Index N_ACTIONS = N_PLAYERS + 1;  // Shoot Player 0, ... (N_PLAYERS-1), or nobody
    constexpr Index N_WINNERS = N_PLAYERS;
    constexpr Index ALIVE = 1;
    const std::vector<double> HIT_RATE {0.3, 0.5, 1.0};

    // Optimal actions (responses)
    enum class ExpectedAction {
        A,
        B,
        C,
        Nobody,
        Undefined,
    };

    const ExpectedAction ExpectedActions[N_PLAYERS][N_STATES] = {
        {ExpectedAction::Undefined, ExpectedAction::Undefined, ExpectedAction::Undefined, ExpectedAction::B,
         ExpectedAction::Undefined, ExpectedAction::C, ExpectedAction::Undefined, ExpectedAction::Nobody},
        {ExpectedAction::Undefined, ExpectedAction::Undefined, ExpectedAction::Undefined, ExpectedAction::A,
         ExpectedAction::Undefined, ExpectedAction::Undefined, ExpectedAction::C, ExpectedAction::C},
        {ExpectedAction::Undefined, ExpectedAction::Undefined, ExpectedAction::Undefined, ExpectedAction::Undefined,
         ExpectedAction::Undefined, ExpectedAction::A, ExpectedAction::B, ExpectedAction::B}};

    // Hyper parameters
    constexpr Count DEFAULT_SAMPLES_SOFTMAX = 1000000ll;
    constexpr Count DEFAULT_SAMPLES_LINEAR = 10000000ll;
    constexpr double DEFAULT_LEARNING_RATE_LINEAR = 0.00001;
    constexpr double DEFAULT_LEARNING_RATE_SOFTMAX = 0.0001;
    constexpr double DEFAULT_EXPLORATION_RATIO_LINEAR = 0.1;
    constexpr double DEFAULT_EXPLORATION_RATIO_SOFTMAX = 0.2;

    constexpr Count MIN_TRIALS = 1ll;
    constexpr bool DEFAULT_USE_SOFTMAX = false;
    constexpr bool IID_CHOICE = false;
    constexpr Index MAX_DEPTH = 15;
}

#define OPTION_NAME_TRIALS "trials"
#define OPTION_NAME_SAMPLES "samples"
#define OPTION_NAME_SOFTMAX "softmax"
#define OPTION_NAME_LEARNING_RATE "learning_rate"
#define OPTION_NAME_EXPLORATION_RATIO "exploration_ratio"
#define OPTION_NAME_LOGFILE "logfile"

class OptimalAction {
public:
    struct Setting {
        bool useSoftmax {DEFAULT_USE_SOFTMAX};
        boost::optional<Count> numSamples;
        boost::optional<double> learningRate;
        boost::optional<double> explorationRatio;
        std::string logFilename;
    };

    OptimalAction(const Setting& setting) :
        rand_gen_(rand_dev_()), unit_distribution_(0.0, 1.0),
        value_(boost::extents[N_PLAYERS][N_STATES][N_ACTIONS]),
        useSoftmax_(setting.useSoftmax) {
        numSamples_ = (setting.numSamples) ? (*setting.numSamples) :
            ((useSoftmax_) ? DEFAULT_SAMPLES_SOFTMAX : DEFAULT_SAMPLES_LINEAR);
        learningRate_ = (setting.learningRate) ? (*setting.learningRate) :
            ((useSoftmax_) ? DEFAULT_LEARNING_RATE_SOFTMAX : DEFAULT_LEARNING_RATE_LINEAR);
        explorationRatio_ = (setting.explorationRatio) ? (*setting.explorationRatio) :
            ((useSoftmax_) ? DEFAULT_EXPLORATION_RATIO_SOFTMAX : DEFAULT_EXPLORATION_RATIO_LINEAR);

        if (!setting.logFilename.empty()) {
            outStream_ = std::make_unique<std::ofstream>(setting.logFilename);
            *outStream_ << "B,C,Nobody\n";
        }

        for (Index player = 0; player < N_PLAYERS; ++player) {
            for (Index state = 0; state < N_STATES; ++state) {
                initialize(player, state);
            }
        }

        std::cout << "UseSoftmax=" << useSoftmax_ << ", # of Samples=" << numSamples_;
        std::cout << ", Learning Rate=" << learningRate_;
        std::cout << ", Exploration Ratio=" << explorationRatio_ << "\n";
        return;
    }

    virtual ~OptimalAction(void) = default;

    void Exec(void) {
        for (Count i = 0; i < numSamples_; ++i) {
            exec();
            if (!outStream_) {
                continue;
            }

            Index player = static_cast<decltype(player)>(ExpectedAction::A);
            for (Index action = 0; action < N_ACTIONS; ++action) {
                if (player != action) {
                    *outStream_ << value_[player][N_STATES-1][action];
                    if ((action + 1) < N_ACTIONS) {
                        *outStream_ << ",";
                    }
                }
            }
            *outStream_ << "\n";
        }
        return;
    }

    void Print(void) {
        for (Index player = 0; player < N_PLAYERS; ++player) {
            std::cout << printValue(player);
        }
        return;
    }

    bool Check(void) {
        bool result = true;

        for (Index player = 0; player < N_PLAYERS; ++player) {
            for (Index state = 0; state < N_STATES; ++state) {
                const auto expectedAction = ExpectedActions[player][state];
                if (expectedAction == ExpectedAction::Undefined) {
                    continue;
                }

                Index actual = N_ACTIONS;
                double maxValue = -std::numeric_limits<double>::infinity();
                for (Index action = 0; action < N_ACTIONS; ++action) {
                    auto value = value_[player][state][action];
                    if (maxValue < value) {
                        actual = action;
                        maxValue = value;
                    }
                }

                const auto expected = static_cast<decltype(actual)>(expectedAction);
                if (static_cast<decltype(actual)>(expected) != actual) {
                    result = false;
                    std::cout << "! Player=" << player << ", State=" << state;
                    std::cout << ", Expected=" << expected << ", Actual=" << actual << "\n";
                }
            }
        }
        return result;
    }

    void initialize(Index player, State state) {
        for (Index action = 0; action < N_ACTIONS; ++action) {
            // Do not shoot yourself!
            auto count = checkAliveOrNobody(player, state);
            if (useSoftmax_) {
                value_[player][state][action] = count && checkAliveOrNobody(action, state) &&
                    (player != action) ? 0.0 : -std::numeric_limits<double>::infinity();
            } else {
                value_[player][state][action] = count && checkAliveOrNobody(action, state) &&
                    (player != action) ? (1.0 / static_cast<double>(count)) : 0.0;
            }
        }

        if (useSoftmax_) {
            normalizeSoftmaxProbabilities(player, state);
        }
        return;
    }

    void exec(void) {
        Survivors survivors(N_PLAYERS, ALIVE);
        ActionChain actionChain;
        aimAndShoot(0, 0, survivors, actionChain);
        return;
    }

    char printIndexChar(Index player) {
        return player + 'A';
    }

    std::string printValue(Index player) {
        std::ostringstream os;

        os << std::setprecision(5) << "[States, actions and values for Player " << printIndexChar(player) << "]\n";
        for (Index state = 0; state < N_STATES; ++state) {
            // Exclude when the player is not alive or only alive
            if (checkPlayerAlive(player, state) < 2) {
                continue;
            }

            for (Index action = 0; action < N_ACTIONS; ++action) {
                if ((player != action) && checkPlayerAlive(action, state)) {
                    os << "Target " << printIndexChar(action) << ":" << value_[player][state][action] << ", ";
                }
                if (action >= N_PLAYERS) {
                    os << "Nobody:" << value_[player][state][action] << "\n" ;
                }
            }
        }

        os << "\n";
        return os.str();
    }

    // Converts an array to a bitmap
    State survivorsToState(const Survivors& survivors) {
        State state = 0;
        State index = 1;
        for(const auto value : survivors) {
            state += index * (value ? 1 : 0);
            index <<= 1;
        }
        return state;
    }

    // Notice that nobody is always not alive (population - player(1) + nobady(1))
    template<typename T>
    auto countPopulation(T state) {
        return __builtin_popcount(state);
    }

    // Return population of the state if the player is alive in the state, 0 othewise
    Index checkAliveOrNobody(Index player, State state) {
        return ((player >= N_PLAYERS) || (state & (1 << player))) ? countPopulation(state) : 0;
    }

    Index checkPlayerAlive(Index player, State state) {
        return (state & (1 << player)) ? countPopulation(state) : 0;
    }

    // exp(elements) /= exp(max(elements)) to avoid overflow
    void normalizeSoftmaxProbabilities(Index player, State state) {
        double max_log = -std::numeric_limits<double>::infinity();
        for (Index action = 0; action < N_ACTIONS; ++action) {
            max_log = std::max(max_log, value_[player][state][action]);
        }

        // Adjust max(log(probabilities)) to zero
        for (Index action = 0; action < N_ACTIONS; ++action) {
            value_[player][state][action] -= max_log;
        }
        return;
    }

    std::vector<double> getSoftmaxProbabilities(Index player, State state) {
        std::vector<double> probabilities(N_ACTIONS, 0.0);
        double sum = 0.0;

        for (Index action = 0; action < N_ACTIONS; ++action) {
            const auto value = ::exp(value_[player][state][action]);
            probabilities.at(action) = value;
            sum += value;
        }

        for (Index action = 0; action < N_ACTIONS; ++action) {
            probabilities.at(action) /= sum;
        }

        return probabilities;
    }

    // Overwrites survivors
    void aimAndShoot(Index player, Index depth, Survivors& survivors, const ActionChain& actionChain) {
        if (depth >= MAX_DEPTH) {
            return;
        }

        const auto targets = getTargets(player, survivors);
        const auto state = survivorsToState(survivors);
        const auto target = getActionTarget(player, state, survivors, targets);

        ActionChain nextActionChain = actionChain;
        Action nextAction {player, state, target};
        nextActionChain.push_back(nextAction);
        shoot(player, survivors, target);

        if (std::accumulate(survivors.begin(), survivors.end(), 0) == 1) {
            const auto winner = std::distance(survivors.begin(),
                                              std::find(survivors.begin(), survivors.end(), ALIVE));

            // Pick up one sample to i.i.d.
            if (IID_CHOICE) {
                auto raw_index = unit_distribution_(rand_gen_) * static_cast<double>(nextActionChain.size()) - 0.5;
                auto index = std::min(nextActionChain.size() - 1,
                                      static_cast<decltype(nextActionChain.size())>(
                                          std::max(0, static_cast<int>(raw_index))));
                backpropagate(nextActionChain.at(index), winner);
            } else {
                // Reverse if you deduct rewards
                for(const auto& action : nextActionChain) {
                    backpropagate(action, winner);
                }
            }
        } else {
            aimAndShoot((player + 1) % N_PLAYERS, depth + 1, survivors, nextActionChain);
        }

        return;
    }

    std::vector<Index> getTargets(Index player, const Survivors& survivors) {
        std::vector<Index> targets;
        for(Index target = 0; target < N_PLAYERS; ++target) {
            if ((target != player) && survivors.at(target)) {
                targets.push_back(target);
            }
        }

        if (targets.size() > 1) {
            // Can shoot nobody
            targets.push_back(N_PLAYERS);
        }

        return targets;
    }

    std::vector<double> getProportions(Index player, State state) {
        // Number of targets = number of survivors - player(1) + nobody(1)
        std::vector<double> proportions(N_ACTIONS, 0.0);

        // Epsilon-greedy
        const auto rand_proportional = unit_distribution_(rand_gen_);
        const bool proportional = (rand_proportional < explorationRatio_);

        if (proportional) {
            // Number of targets = number of survivors - player(1) + nobody(1)
            const auto population = checkPlayerAlive(player, state);
            const double proportion = (population > 0) ? (1.0 / static_cast<double>(population)) : 0.0;
            for (Index action = 0; action < N_ACTIONS; ++action) {
                proportions.at(action) = (checkPlayerAlive(player, state) &&
                                          checkAliveOrNobody(action, state) &&
                                          (player != action)) ? proportion : 0.0;
            }
        } else {
            if (useSoftmax_) {
                proportions = getSoftmaxProbabilities(player, state);
            } else {
                for (Index action = 0; action < N_ACTIONS; ++action) {
                    proportions.at(action) = value_[player][state][action];
                }
            }
        }

        return proportions;
    }

    Index getActionTarget(Index player, State state, const Survivors& survivors, const std::vector<Index>& targets) {
        const auto proportions = getProportions(player, state);
        auto rand_value = unit_distribution_(rand_gen_);
        Index target = 0;

        while(rand_value >= 0.0) {
            rand_value -= proportions.at(target);
            target += 1;
            if (target >= N_ACTIONS) {
                break;
            }
        }

        return target - 1;
    }

    // Overwrites survivors
    void shoot(Index player, Survivors& survivors, Index target) {
        if (target < N_PLAYERS) {
            if (unit_distribution_(rand_gen_) < HIT_RATE.at(player)) {
                survivors.at(target) = 0;
            }
        }

        return;
    }

    void backpropagate(const Action& action, Index final_surviver) {
        // Normalizes such that the sum of values is 1
        if (useSoftmax_) {
            // backpropagate Y-T * delta(error)/delta(y)
            auto exp_proportions = getSoftmaxProbabilities(action.player, action.state);
            for (Index i = 0; i < N_ACTIONS; ++i) {
                const auto delta = (exp_proportions[i] - ((action.player == final_surviver) ? 1.0 : 0.0)) * learningRate_;
                value_[action.player][action.state][action.target] -= delta;
            }
            normalizeSoftmaxProbabilities(action.player, action.state);
        } else {
            const auto target_value = value_[action.player][action.state][action.target];
            const auto delta = target_value * learningRate_ * ((action.player == final_surviver) ? 1.0 : -1.0);
            value_[action.player][action.state][action.target] += delta;
            double sum = 0.0;
            for (Index i = 0; i < N_ACTIONS; ++i) {
                sum += value_[action.player][action.state][i];
            }
            for (Index i = 0; i < N_ACTIONS; ++i) {
                value_[action.player][action.state][i] /= sum;
            }
        }
    }

private:
    using StateActionValue = boost::multi_array<double, 3>;
    std::random_device rand_dev_;
    std::mt19937 rand_gen_;
    std::uniform_real_distribution<double> unit_distribution_;
    StateActionValue value_;

    bool useSoftmax_ {DEFAULT_USE_SOFTMAX};
    Count numSamples_ {0};
    double learningRate_ {0.0};
    double explorationRatio_ {0.0};
    std::unique_ptr<std::ofstream> outStream_;
};

int main(int argc, char* argv[]) {
    Count numTrials = MIN_TRIALS;
    OptimalAction::Setting setting;

    boost::program_options::options_description description("Options");
    description.add_options()
        (OPTION_NAME_TRIALS",t",
         boost::program_options::value<decltype(numTrials)>(),
         "Number of searches")
        (OPTION_NAME_SAMPLES",s",
         boost::program_options::value<decltype(setting.numSamples)>(),
         "Number of samples in a search")
        (OPTION_NAME_SOFTMAX",x",
         boost::program_options::value<decltype(setting.useSoftmax)>()->default_value(false),
         "Use softmax instead of linear")
        (OPTION_NAME_LEARNING_RATE",l",
         boost::program_options::value<decltype(setting.learningRate)::value_type>(),
         "Learning rate")
        (OPTION_NAME_EXPLORATION_RATIO",e",
         boost::program_options::value<decltype(setting.explorationRatio)::value_type>(),
         "Probability of explorations")
        (OPTION_NAME_LOGFILE",o",
         boost::program_options::value<decltype(setting.logFilename)>(),
         "Output log file name")
        ;

    boost::program_options::variables_map varMap;
    boost::program_options::store(parse_command_line(argc, argv, description), varMap);
    boost::program_options::notify(varMap);

    if (varMap.count(OPTION_NAME_TRIALS)) {
        numTrials = std::max(MIN_TRIALS, varMap[OPTION_NAME_TRIALS].
                             as<decltype(numTrials)>());
    }

    if (varMap.count(OPTION_NAME_SAMPLES)) {
        setting.numSamples = varMap[OPTION_NAME_SAMPLES].
            as<decltype(setting.numSamples)>();
    }

    if (varMap.count(OPTION_NAME_SOFTMAX)) {
        setting.useSoftmax = varMap[OPTION_NAME_SOFTMAX].
            as<decltype(setting.useSoftmax)>();
    }

    if (varMap.count(OPTION_NAME_LEARNING_RATE)) {
        setting.learningRate = varMap[OPTION_NAME_LEARNING_RATE].
            as<decltype(setting.learningRate)::value_type>();
    }

    if (varMap.count(OPTION_NAME_EXPLORATION_RATIO)) {
        setting.explorationRatio = varMap[OPTION_NAME_EXPLORATION_RATIO]
            .as<decltype(setting.explorationRatio)::value_type>();
    }

    if (varMap.count(OPTION_NAME_LOGFILE)) {
        setting.logFilename = varMap[OPTION_NAME_LOGFILE]
            .as<decltype(setting.logFilename)>();
    }

    OptimalAction optimalAction(setting);
    Count n_correct = 0;
    Count n_wrong = 0;
    for (Count trial = 0; trial < numTrials; ++trial) {
        optimalAction.Exec();
        const auto correct = optimalAction.Check();
        n_correct += (correct ? 1 : 0);
        n_wrong += (correct ? 0 : 1);

        if (!trial || !correct) {
            optimalAction.Print();
        } else {
            std::cerr << ".";
        }
    }

    std::cout << "Correct cases=" << n_correct << ", Wrong cases=" << n_wrong << "\n";
    return 0;
}

/*
Local Variables:
mode: c++
coding: utf-8-dos
tab-width: nil
c-file-style: "stroustrup"
End:
*/


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

  @copyright 2016 NumScale SAS

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

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

 /*!

    @ingroup group-predicates
    Function object implementing majority capabilities

    Returns @ref True if at least two inputs are not @ref Zero else @ref False.

    @par Semantic:

    @code
    auto r = majority(x,y,z);
    @endcode

    is similar to:

    @code
    auto r = (x!= 0)+(y!= 0)+(z!= 0) >= 2;
    @endcode

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

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

#endif


// Copyright 2020 Advanced Remanufacturing and Technology Centre
// Copyright 2020 ROS-Industrial Consortium Asia Pacific Team
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#ifndef PLANNING_FUNCTIONS_HPP_
#define PLANNING_FUNCTIONS_HPP_

#include <boost/filesystem.hpp>
#include <ament_index_cpp/get_package_share_directory.hpp>
#include <opencv2/opencv.hpp>
#include <cv_bridge/cv_bridge.h>

#include <iostream>
#include <vector>
#include <fstream>
#include <sstream>
#include <algorithm>
#include "cmath"
#include "sensor_msgs/msg/camera_info.hpp"
#include "sensor_msgs/image_encodings.hpp"
#include "yaml-cpp/yaml.h"

float PI = 3.14159265;
/**
 * Calculate length of two points in 2D space
 */
float length(int x1, int y1, int x2, int y2)
{
  return sqrt(pow(static_cast<float>(x1 - x2), 2) + pow(static_cast<float>(y1 - y2), 2));
}

/**
 * Checks if a point is in a circle.
 */
bool in_circle(float radius, std::vector<int> centre, std::vector<int> point)
{
  if (pow((point[0] - centre[0]), 2) + pow((point[1] - centre[1]), 2) - pow(radius, 2) < 0) {
    return true;
  } else {
    return false;
  }
}

/**
 * Quadratic equation solver
 */
std::vector<float> quadratic_equation(float a, float b, float c)
{
  std::vector<float> result;
  float discriminant = pow(b, 2) - (4 * a * c);
  if (discriminant < 0) {
    return result;
  } else if (discriminant == 0) {
    result.push_back(-b / (2 * a));
  } else {
    result.push_back(((-b + sqrt(discriminant)) / (2 * a)));
    result.push_back(((-b - sqrt(discriminant)) / (2 * a)));
  }
  return result;
}

/**
 * Get the intersecting coordinates between a circle and a line
 */
std::vector<std::vector<int>> circle_line_intersect(
  float gradient,
  float intersect,
  float radius,
  std::vector<float> circle_centre)
{
  std::vector<std::vector<int>> result;

  if (circle_centre.size() == 2 && radius > 0) {
    // Get the quadratic equation to determine the x coordinates of point
    float a = 1 + pow(gradient, 2);
    float b = 2 * ((gradient * (intersect - circle_centre[1])) - circle_centre[0]);
    float c = pow(circle_centre[0], 2) +
      pow((intersect - circle_centre[1]), 2) - pow(radius, 2);

    std::vector<float> x_coords = quadratic_equation(a, b, c);

    // Solve the equation to get the x coordinates
    if (static_cast<int>(x_coords.size()) == 2) {
      for (int i = 0; i < static_cast<int>(x_coords.size()); i++) {
        std::vector<int> temp_coords {static_cast<int>(round(x_coords[i])),
          static_cast<int>(round(gradient * x_coords[i] + intersect))};
        result.push_back(temp_coords);
      }
    } else {
      return result;
    }
  }

  return result;
}

/**
 * Get a bounding box based on coodinate centre and radius of box area.
 */

std::vector<std::vector<int>> get_border_corners(std::vector<int> centre, float radius)
{
  std::vector<std::vector<int>> result;
  if (centre.size() == 2 && radius > 0 && centre[0] >= 0 && centre[1] >= 0) {
    std::vector<int> top_left;
    std::vector<int> bottom_right;
    if (centre[0] - radius < 0) {
      top_left.push_back(0);
    } else {
      top_left.push_back(static_cast<int>(round(centre[0] - radius)));
    }
    if (centre[1] - radius < 0) {
      top_left.push_back(0);
    } else {
      top_left.push_back(static_cast<int>(round(centre[1] - radius)));
    }
    bottom_right = {static_cast<int>(round(centre[0] + radius)),
      static_cast<int>(round(centre[1] + radius))};
    result = {top_left, bottom_right};
  } else {
    return result;
  }
  return result;
}

/**
 * Make sure input is within 0 to Pi
 */

float keep_angle_in_bounds(float input)
{
  if (input < 0 || input >= PI) {
    if (input < 0) {
      while (input < 0) {
        input += PI;
      }
    }
    if (input >= PI) {
      while (input >= PI) {
        input -= PI;
      }
    }
  }
  return input;
}

#endif  // PLANNING_FUNCTIONS_HPP_


#pragma once

#include "enum/enum.hpp"
#include "matrix/sparsity_pattern/helpers.hpp"

#include <deal.II/lac/dynamic_sparsity_pattern.h>

namespace boltzmann {

template <enum METHOD>
struct vsparsity
{
};

template <>
struct vsparsity<METHOD::MODLEASTSQUARES>
{
  template <typename VELOCITY_VARFORM>
  static void make(dealii::SparsityPattern& lhs_sparsity,
                   dealii::SparsityPattern& rhs_sparsity,
                   int N,
                   const VELOCITY_VARFORM& var_form);
};

template <typename VELOCITY_VARFORM>
void
vsparsity<METHOD::MODLEASTSQUARES>::make(dealii::SparsityPattern& lhs_vsparsity,
                                         dealii::SparsityPattern& rhs_vsparsity,
                                         int N,
                                         const VELOCITY_VARFORM& var_form)
{
  dealii::DynamicSparsityPattern csp(N, N);
  sparsity_helper::add_to_csp(csp, var_form.get_s0());
  sparsity_helper::add_to_csp(csp, var_form.get_t2());
  lhs_vsparsity.copy_from(csp);
  lhs_vsparsity.compress();

  dealii::DynamicSparsityPattern csp2(N, N);
  sparsity_helper::add_to_csp(csp2, var_form.get_s0());
  sparsity_helper::add_to_csp(csp2, var_form.get_t1());
  rhs_vsparsity.copy_from(csp2);
  rhs_vsparsity.compress();
}

}  // boltzmann