/*
 [auto_generated]
 boost/numeric/odeint/stepper/detail/generic_rk_algorithm.hpp

 [begin_description]
 Implementation of the generic Runge-Kutta method.
 [end_description]

 Copyright 2009-2011 Karsten Ahnert
 Copyright 2009-2011 Mario Mulansky

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

#ifndef BOOST_NUMERIC_ODEINT_STEPPER_DETAIL_GENERIC_RK_ALGORITHM_HPP_INCLUDED
#define BOOST_NUMERIC_ODEINT_STEPPER_DETAIL_GENERIC_RK_ALGORITHM_HPP_INCLUDED

#include <boost/static_assert.hpp>

#include <boost/mpl/vector.hpp>
#include <boost/mpl/push_back.hpp>
#include <boost/mpl/for_each.hpp>
#include <boost/mpl/range_c.hpp>
#include <boost/mpl/copy.hpp>
#include <boost/mpl/size_t.hpp>

#include <boost/fusion/algorithm.hpp>
#include <boost/fusion/iterator.hpp>
#include <boost/fusion/mpl.hpp>
#include <boost/fusion/sequence.hpp>

#include <boost/array.hpp>

#include <boost/numeric/odeint/algebra/range_algebra.hpp>
#include <boost/numeric/odeint/algebra/default_operations.hpp>
#include <boost/numeric/odeint/stepper/detail/generic_rk_call_algebra.hpp>
#include <boost/numeric/odeint/stepper/detail/generic_rk_operations.hpp>
#include <boost/numeric/odeint/util/bind.hpp>

namespace boost {
namespace numeric {
namespace odeint {
namespace detail {

template< class T , class Constant >
struct array_wrapper
{
    typedef const typename boost::array< T , Constant::value > type;
};

template< class T , size_t i >
struct stage
{
    T c;
    boost::array< T , i > a;
};

template< class T , class Constant >
struct stage_wrapper
{
    typedef stage< T , Constant::value > type;
};

template<
size_t StageCount,
class Value ,
class Algebra ,
class Operations
>
class generic_rk_algorithm {

public:
    typedef mpl::range_c< size_t , 1 , StageCount > stage_indices;

    typedef typename boost::fusion::result_of::as_vector
            <
            typename boost::mpl::copy
            <
            stage_indices ,
            boost::mpl::inserter
            <
            boost::mpl::vector0< > ,
            boost::mpl::push_back< boost::mpl::_1 , array_wrapper< Value , boost::mpl::_2 > >
    >
    >::type
    >::type coef_a_type;

    typedef boost::array< Value , StageCount > coef_b_type;
    typedef boost::array< Value , StageCount > coef_c_type;

    typedef typename boost::fusion::result_of::as_vector
            <
            typename boost::mpl::push_back
            <
            typename boost::mpl::copy
            <
            stage_indices,
            boost::mpl::inserter
            <
            boost::mpl::vector0<> ,
            boost::mpl::push_back< boost::mpl::_1 , stage_wrapper< Value , boost::mpl::_2 > >
    >
    >::type ,
    stage< Value , StageCount >
    >::type
    >::type stage_vector_base;

    struct stage_vector : public stage_vector_base
    {
        struct do_insertion
        {
            stage_vector_base &m_base;
            const coef_a_type &m_a;
            const coef_c_type &m_c;

            do_insertion( stage_vector_base &base , const coef_a_type &a , const coef_c_type &c )
            : m_base( base ) , m_a( a ) , m_c( c ) { }

            template< class Index >
            void operator()( Index ) const
            {
                //boost::fusion::at< Index >( m_base ) = stage< double , Index::value+1 , intermediate_stage >( m_c[ Index::value ] , boost::fusion::at< Index >( m_a ) );
                boost::fusion::at< Index >( m_base ).c  = m_c[ Index::value ];
                boost::fusion::at< Index >( m_base ).a = boost::fusion::at< Index >( m_a );
            }
        };

        struct print_butcher
        {
            const stage_vector_base &m_base;
            std::ostream &m_os;

            print_butcher( const stage_vector_base &base , std::ostream &os )
            : m_base( base ) , m_os( os )
            { }

            template<class Index>
            void operator()(Index) const {
                m_os << boost::fusion::at<Index>(m_base).c << " | ";
                for( size_t i=0 ; i<Index::value ; ++i )
                    m_os << boost::fusion::at<Index>(m_base).a[i] << " ";
                m_os << std::endl;
            }
        };

        stage_vector( const coef_a_type &a , const coef_b_type &b , const coef_c_type &c )
        {
            typedef boost::mpl::range_c< size_t , 0 , StageCount-1 > indices;
            boost::mpl::for_each< indices >( do_insertion( *this , a , c ) );
            boost::fusion::at_c< StageCount - 1 >( *this ).c = c[ StageCount - 1 ];
            boost::fusion::at_c< StageCount - 1 >( *this ).a = b;
        }

        void print( std::ostream &os ) const
        {
            typedef boost::mpl::range_c< size_t , 0 , StageCount > indices;
            boost::mpl::for_each< indices >( print_butcher( *this , os ) );
        }
    };

    template< class System , class StateIn , class StateTemp , class DerivIn , class Deriv ,
    class StateOut , class Time >
    struct calculate_stage
    {
        Algebra &algebra;
        System &system;
        const StateIn &x;
        StateTemp &x_tmp;
        StateOut &x_out;
        const DerivIn &dxdt;
        Deriv *F;
        Time t;
        Time dt;

        calculate_stage( Algebra &_algebra , System &_system , const StateIn &_x , const DerivIn &_dxdt , StateOut &_out ,
                StateTemp &_x_tmp , Deriv *_F , Time _t , Time _dt )
        : algebra( _algebra ) , system( _system ) , x( _x ) , x_tmp( _x_tmp ) , x_out( _out) , dxdt( _dxdt ) , F( _F ) , t( _t ) , dt( _dt )
        {}

        template< typename T , size_t stage_number >
        void inline operator()( stage< T , stage_number > const &stage ) const
        //typename stage_fusion_wrapper< T , mpl::size_t< stage_number > , intermediate_stage >::type const &stage ) const
        {
            if( stage_number > 1 )
            {
#ifdef BOOST_MSVC
#pragma warning( disable : 4307 34 )
#endif
                system( x_tmp , F[stage_number-2].m_v , t + stage.c * dt );
#ifdef BOOST_MSVC
#pragma warning( default : 4307 34 )
#endif
            }
            //std::cout << stage_number-2 << ", t': " << t + stage.c * dt << std::endl;

            if( stage_number < StageCount )
                detail::template generic_rk_call_algebra< stage_number , Algebra >()( algebra , x_tmp , x , dxdt , F ,
                        detail::generic_rk_scale_sum< stage_number , Operations , Value , Time >( stage.a , dt) );
            //                  algebra_type::template for_eachn<stage_number>( x_tmp , x , dxdt , F ,
            //                          typename operations_type::template scale_sumn< stage_number , time_type >( stage.a , dt ) );
            else
                detail::template generic_rk_call_algebra< stage_number , Algebra >()( algebra , x_out , x , dxdt , F ,
                        detail::generic_rk_scale_sum< stage_number , Operations , Value , Time >( stage.a , dt ) );
            //                algebra_type::template for_eachn<stage_number>( x_out , x , dxdt , F ,
            //                            typename operations_type::template scale_sumn< stage_number , time_type >( stage.a , dt ) );
        }

    };

    generic_rk_algorithm( const coef_a_type &a , const coef_b_type &b , const coef_c_type &c )
    : m_stages( a , b , c )
    { }

    template< class System , class StateIn , class DerivIn , class Time , class StateOut , class StateTemp , class Deriv >
    void inline do_step( Algebra &algebra , System system , const StateIn &in , const DerivIn &dxdt ,
            Time t , StateOut &out , Time dt ,
            StateTemp &x_tmp , Deriv F[StageCount-1] ) const
    {
        typedef typename odeint::unwrap_reference< System >::type unwrapped_system_type;
        unwrapped_system_type &sys = system;
        boost::fusion::for_each( m_stages , calculate_stage<
                unwrapped_system_type , StateIn , StateTemp , DerivIn , Deriv , StateOut , Time >
        ( algebra , sys , in , dxdt , out , x_tmp , F , t , dt ) );
    }

private:
    stage_vector m_stages;
};

}
}
}
}

#endif // BOOST_NUMERIC_ODEINT_STEPPER_DETAIL_GENERIC_RK_ALGORITHM_HPP_INCLUDED


#ifndef FLOW_H_
#define FLOW_H_

#include <Eigen/Core>
#include <list>
#include <map>
#include <vector>

#include "qflow/config.hpp"

#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/boykov_kolmogorov_max_flow.hpp>
#include <boost/graph/edmonds_karp_max_flow.hpp>
#include <boost/graph/push_relabel_max_flow.hpp>

#include <lemon/network_simplex.h>
#include <lemon/preflow.h>
#include <lemon/smart_graph.h>

using namespace boost;
using namespace Eigen;

namespace qflow {

class MaxFlowHelper {
   public:
    MaxFlowHelper() {}
    virtual ~MaxFlowHelper(){};
    virtual void resize(int n, int m) = 0;
    virtual void addEdge(int x, int y, int c, int rc, int v, int cost = 1) = 0;
    virtual int compute() = 0;
    virtual void applyTo(std::vector<Vector2i>& edge_diff) = 0;
};

class BoykovMaxFlowHelper : public MaxFlowHelper {
   public:
    typedef int EdgeWeightType;
    typedef adjacency_list_traits<vecS, vecS, directedS> Traits;
    // clang-format off
    typedef adjacency_list < vecS, vecS, directedS,
        property < vertex_name_t, std::string,
        property < vertex_index_t, long,
        property < vertex_color_t, boost::default_color_type,
        property < vertex_distance_t, long,
        property < vertex_predecessor_t, Traits::edge_descriptor > > > > >,

        property < edge_capacity_t, EdgeWeightType,
        property < edge_residual_capacity_t, EdgeWeightType,
        property < edge_reverse_t, Traits::edge_descriptor > > > > Graph;
    // clang-format on

   public:
    BoykovMaxFlowHelper() { rev = get(edge_reverse, g); }
    void resize(int n, int m) {
        vertex_descriptors.resize(n);
        for (int i = 0; i < n; ++i) vertex_descriptors[i] = add_vertex(g);
    }
    int compute() {
        EdgeWeightType flow =
            boykov_kolmogorov_max_flow(g, vertex_descriptors.front(), vertex_descriptors.back());
        return flow;
    }
    void addDirectEdge(Traits::vertex_descriptor& v1, Traits::vertex_descriptor& v2,
                       property_map<Graph, edge_reverse_t>::type& rev, const int capacity,
                       const int inv_capacity, Graph& g, Traits::edge_descriptor& e1,
                       Traits::edge_descriptor& e2) {
        e1 = add_edge(v1, v2, g).first;
        e2 = add_edge(v2, v1, g).first;
        put(edge_capacity, g, e1, capacity);
        put(edge_capacity, g, e2, inv_capacity);

        rev[e1] = e2;
        rev[e2] = e1;
    }
    void addEdge(int x, int y, int c, int rc, int v, int cost = 1) {
        Traits::edge_descriptor e1, e2;
        addDirectEdge(vertex_descriptors[x], vertex_descriptors[y], rev, c, rc, g, e1, e2);
        if (v != -1) {
            edge_to_variables[e1] = std::make_pair(v, -1);
            edge_to_variables[e2] = std::make_pair(v, 1);
        }
    }
    void applyTo(std::vector<Vector2i>& edge_diff) {
        property_map<Graph, edge_capacity_t>::type capacity = get(edge_capacity, g);
        property_map<Graph, edge_residual_capacity_t>::type residual_capacity =
            get(edge_residual_capacity, g);

        graph_traits<Graph>::vertex_iterator u_iter, u_end;
        graph_traits<Graph>::out_edge_iterator ei, e_end;
        for (tie(u_iter, u_end) = vertices(g); u_iter != u_end; ++u_iter)
            for (tie(ei, e_end) = out_edges(*u_iter, g); ei != e_end; ++ei)
                if (capacity[*ei] > 0) {
                    int flow = (capacity[*ei] - residual_capacity[*ei]);
                    if (flow > 0) {
                        auto it = edge_to_variables.find(*ei);
                        if (it != edge_to_variables.end()) {
                            edge_diff[it->second.first / 2][it->second.first % 2] +=
                                it->second.second * flow;
                        }
                    }
                }
    }

   private:
    Graph g;
    property_map<Graph, edge_reverse_t>::type rev;
    std::vector<Traits::vertex_descriptor> vertex_descriptors;
    std::map<Traits::edge_descriptor, std::pair<int, int>> edge_to_variables;
};

class NetworkSimplexFlowHelper : public MaxFlowHelper {
   public:
    using Weight = int;
    using Capacity = int;
    using Graph = lemon::SmartDigraph;
    using Node = Graph::Node;
    using Arc = Graph::Arc;
    template <typename ValueType>
    using ArcMap = lemon::SmartDigraph::ArcMap<ValueType>;
    using Preflow = lemon::Preflow<lemon::SmartDigraph, ArcMap<Capacity>>;
    using NetworkSimplex = lemon::NetworkSimplex<lemon::SmartDigraph, Capacity, Weight>;

   public:
    NetworkSimplexFlowHelper() : cost(graph), capacity(graph), flow(graph), variable(graph) {}
    ~NetworkSimplexFlowHelper(){};
    void resize(int n, int m) {
        nodes.reserve(n);
        for (int i = 0; i < n; ++i) nodes.push_back(graph.addNode());
    }
    void addEdge(int x, int y, int c, int rc, int v, int cst = 1) {
        assert(x >= 0);
        assert(v >= -1);
        if (c) {
            auto e1 = graph.addArc(nodes[x], nodes[y]);
            cost[e1] = cst;
            capacity[e1] = c;
            variable[e1] = std::make_pair(v, 1);
        }

        if (rc) {
            auto e2 = graph.addArc(nodes[y], nodes[x]);
            cost[e2] = cst;
            capacity[e2] = rc;
            variable[e2] = std::make_pair(v, -1);
        }
    }
    int compute() {
        Preflow pf(graph, capacity, nodes.front(), nodes.back());
        NetworkSimplex ns(graph);

        // Run preflow to find maximum flow
        lprintf("push-relabel flow... ");
        pf.runMinCut();
        int maxflow = pf.flowValue();

        // Run network simplex to find minimum cost maximum flow
        ns.costMap(cost).upperMap(capacity).stSupply(nodes.front(), nodes.back(), maxflow);
        auto status = ns.run();
        switch (status) {
            case NetworkSimplex::OPTIMAL:
                ns.flowMap(flow);
                break;
            case NetworkSimplex::INFEASIBLE:
                lputs("NetworkSimplex::INFEASIBLE");
                assert(0);
                break;
            default:
                lputs("Unknown: NetworkSimplex::Default");
                assert(0);
                break;
        }

        return maxflow;
    }
    void applyTo(std::vector<Vector2i>& edge_diff) {
        for (Graph::ArcIt e(graph); e != lemon::INVALID; ++e) {
            int var = variable[e].first;
            if (var == -1) continue;
            int sgn = variable[e].second;
            edge_diff[var / 2][var % 2] -= sgn * flow[e];
        }
    }

   private:
    Graph graph;
    ArcMap<Weight> cost;
    ArcMap<Capacity> capacity;
    ArcMap<Capacity> flow;
    ArcMap<std::pair<int, int>> variable;
    std::vector<Node> nodes;
    std::vector<Arc> edges;
};

#ifdef WITH_GUROBI

#include <gurobi_c++.h>

class GurobiFlowHelper : public MaxFlowHelper {
   public:
    GurobiFlowHelper() {}
    virtual ~GurobiFlowHelper(){};
    virtual void resize(int n, int m) {
        nodes.resize(n * 2);
        edges.resize(m);
    }
    virtual void addEdge(int x, int y, int c, int rc, int v, int cost = 1) {
        nodes[x * 2 + 0].push_back(vars.size());
        nodes[y * 2 + 1].push_back(vars.size());
        vars.push_back(model.addVar(0, c, 0, GRB_INTEGER));
        edges.push_back(std::make_pair(v, 1));

        nodes[y * 2 + 0].push_back(vars.size());
        nodes[x * 2 + 1].push_back(vars.size());
        vars.push_back(model.addVar(0, rc, 0, GRB_INTEGER));
        edges.push_back(std::make_pair(v, -1));
    }
    virtual int compute() {
        std::cerr << "compute" << std::endl;
        int ns = nodes.size() / 2;

        int flow;
        for (int i = 1; i < ns - 1; ++i) {
            GRBLinExpr cons = 0;
            for (auto n : nodes[2 * i + 0]) cons += vars[n];
            for (auto n : nodes[2 * i + 1]) cons -= vars[n];
            model.addConstr(cons == 0);
        }

        // first pass, maximum flow
        GRBLinExpr outbound = 0;
        {
            lprintf("first pass\n");
            for (auto& n : nodes[0]) outbound += vars[n];
            for (auto& n : nodes[1]) outbound -= vars[n];
            model.setObjective(outbound, GRB_MAXIMIZE);
            model.optimize();

            flow = (int)model.get(GRB_DoubleAttr_ObjVal);
            lprintf("Gurobi result: %d\n", flow);
        }

        // second pass, minimum cost flow
        {
            lprintf("second pass\n");
            model.addConstr(outbound == flow);
            GRBLinExpr cost = 0;
            for (auto& v : vars) cost += v;
            model.setObjective(cost, GRB_MINIMIZE);
            model.optimize();

            double optimal_cost = (int)model.get(GRB_DoubleAttr_ObjVal);
            lprintf("Gurobi result: %.3f\n", optimal_cost);
        }
        return flow;
    }
    virtual void applyTo(std::vector<Vector2i>& edge_diff) { assert(0); };

   private:
    GRBEnv env = GRBEnv();
    GRBModel model = GRBModel(env);
    std::vector<GRBVar> vars;
    std::vector<std::pair<int, int>> edges;
    std::vector<std::vector<int>> nodes;
};

#endif

class ECMaxFlowHelper : public MaxFlowHelper {
   public:
    struct FlowInfo {
        int id;
        int capacity, flow;
        int v, d;
        FlowInfo* rev;
    };
    struct SearchInfo {
        SearchInfo(int _id, int _prev_id, FlowInfo* _info)
            : id(_id), prev_id(_prev_id), info(_info) {}
        int id;
        int prev_id;
        FlowInfo* info;
    };
    ECMaxFlowHelper() { num = 0; }
    int num;
    std::vector<FlowInfo*> variable_to_edge;
    void resize(int n, int m) {
        graph.resize(n);
        variable_to_edge.resize(m, 0);
        num = n;
    }
    void addEdge(int x, int y, int c, int rc, int v, int cost = 0) {
        FlowInfo flow;
        flow.id = y;
        flow.capacity = c;
        flow.flow = 0;
        flow.v = v;
        flow.d = -1;
        graph[x].push_back(flow);
        auto& f1 = graph[x].back();
        flow.id = x;
        flow.capacity = rc;
        flow.flow = 0;
        flow.v = v;
        flow.d = 1;
        graph[y].push_back(flow);
        auto& f2 = graph[y].back();
        f2.rev = &f1;
        f1.rev = &f2;
    }
    int compute() {
        int total_flow = 0;
        int count = 0;
        while (true) {
            count += 1;
            std::vector<int> vhash(num, 0);
            std::vector<SearchInfo> q;
            q.push_back(SearchInfo(0, -1, 0));
            vhash[0] = 1;
            int q_front = 0;
            bool found = false;
            while (q_front < q.size()) {
                int vert = q[q_front].id;
                for (auto& l : graph[vert]) {
                    if (vhash[l.id] || l.capacity <= l.flow) continue;
                    q.push_back(SearchInfo(l.id, q_front, &l));
                    vhash[l.id] = 1;
                    if (l.id == num - 1) {
                        found = true;
                        break;
                    }
                }
                if (found) break;
                q_front += 1;
            }
            if (q_front == q.size()) break;
            int loc = q.size() - 1;
            while (q[loc].prev_id != -1) {
                q[loc].info->flow += 1;
                q[loc].info->rev->flow -= 1;
                loc = q[loc].prev_id;
                // int prev_v = q[loc].id;
                // applyFlow(prev_v, current_v, 1);
                // applyFlow(current_v, prev_v, -1);
            }
            total_flow += 1;
        }
        return total_flow;
    }
    void applyTo(std::vector<Vector2i>& edge_diff) {
        for (int i = 0; i < graph.size(); ++i) {
            for (auto& flow : graph[i]) {
                if (flow.flow > 0 && flow.v != -1) {
                    if (flow.flow > 0) {
                        edge_diff[flow.v / 2][flow.v % 2] += flow.d * flow.flow;
                        if (abs(edge_diff[flow.v / 2][flow.v % 2]) > 2) {
                        }
                    }
                }
            }
        }
    }
    void applyFlow(int v1, int v2, int flow) {
        for (auto& it : graph[v1]) {
            if (it.id == v2) {
                it.flow += flow;
                break;
            }
        }
    }
    std::vector<std::list<FlowInfo>> graph;
};

} // namespace qflow

#endif


/* Copyright (c) 2021 Grumpy Cat Software S.L.
 *
 * This Source Code is licensed under the MIT 2.0 license.
 * the terms can be found in  LICENSE.md at the root of
 * this project, or at http://mozilla.org/MPL/2.0/.
 */

#include <gauss/dimensionality.h>
#include <gauss/internal/scopedHostPtr.h>

#include <algorithm>
#include <boost/math/distributions/normal.hpp>
#include <cmath>
#include <iterator>
#include <limits>
#include <map>
#include <numeric>
#include <set>
#include <stdexcept>
#include <utility>
#include <vector>

using namespace gauss::dimensionality;

namespace {

struct VisvalingamSummaryPoint {
    float x;
    float y;
    int64_t area;
};

std::vector<float> computeBreakpoints(int alphabet_size, float mean_value, float std_value) {
    std::vector<float> res;
    boost::math::normal dist(mean_value, std_value);

    for (int i = 1; i < alphabet_size; i++) {
        float value = static_cast<float>(quantile(dist, (float)i * (1 / (float)alphabet_size)));
        res.push_back(value);
    }
    return res;
}

std::vector<int> generateAlphabet(int alphabet_size) {
    std::vector<int> res;
    for (int i = 0; i < alphabet_size; i++) {
        res.push_back(i);
    }
    return res;
}

double PerpendicularDistance(const Point &pt, const Point &lineStart, const Point &lineEnd) {
    double dx = static_cast<double>(lineEnd.first) - lineStart.first;
    double dy = static_cast<double>(lineEnd.second) - lineStart.second;

    // Normalise
    double mag = pow(pow(dx, 2.0) + pow(dy, 2.0), 0.5);
    if (mag > 0.0) {
        dx /= mag;
        dy /= mag;
    }

    double pvx = static_cast<double>(pt.first) - lineStart.first;
    double pvy = static_cast<double>(pt.second) - lineStart.second;

    // Get dot product (project pv onto normalized direction)
    double pvdot = dx * pvx + dy * pvy;

    // Scale line direction vector
    double dsx = pvdot * dx;
    double dsy = pvdot * dy;

    // Subtract this from pv
    double ax = pvx - dsx;
    double ay = pvy - dsy;

    return pow(pow(ax, 2.0) + pow(ay, 2.0), 0.5);
}

/**
 * @brief We compute the vertical distance of the point p w.r.t. the line that connects start and end points.
 * We use the point-slope equiation to solve it.
 */
float verticalDistance(const Point &p, const Point &start, const Point &end) {
    float dy = end.second - start.second;
    float dx = end.first - start.first;
    float m = dy / dx;
    float line_y = (p.first - start.first) * m + start.second;
    return (p.second - line_y) * (p.second - line_y);
}

/**
 * @brief This function just inserts a point in the given position of a vector.
 */
void insertPointBetweenSelected(const Point &p, int position, std::vector<Point> &selected) {
    auto it = selected.begin();
    selected.insert(it + position, p);
}

/**
 * @brief This function just checks if the given point is already in the vector.
 */
bool isPointInDesiredList(const Point &point, const std::vector<Point> &selected) {
    auto it =
        std::find_if(selected.cbegin(), selected.cend(), [&point](const Point &p) { return p.first == point.first; });
    return it != selected.end();
}

/**
 * @brief This function calculates the segment indices that we need to compare the point.
 */
std::pair<int, int> getSegmentFromSelected(const Point &point, const std::vector<Point> &selected) {
    int lower = 0, upper = 0;
    // We do not check the first element, as it is fixed and set to the first element of the time series
    size_t i = 0;
    bool rebased = false;
    while (!rebased && (i < selected.size() - 1)) {
        // We check points until point.first > selected[i]
        if (point.first > selected[i].first) {
            lower = static_cast<int>(i);
            upper = static_cast<int>(i + 1);
        } else {
            rebased = true;
        }
        i++;
    }
    return std::make_pair(lower, upper);
}

template <typename T>
af::array PAA_CPU(const af::array &a, int bins) {
    af::array result;
    auto n = a.dims(0);

    auto reducedColumn = std::vector<T>(bins);

    // Find out the number of elements per bin
    T elemPerBin = static_cast<T>(n) / static_cast<T>(bins);

    // For each column
    for (int i = 0; i < a.dims(1); i++) {
        auto column = gauss::utils::makeScopedHostPtr(a.col(i).host<T>());
        T start = 0.0;
        T end = elemPerBin - 1;

        // For each column
        for (int j = 0; j < bins; j++) {
            T avg = 0.0;
            int count = 0;

            // Compute avg for this segment
            for (int k = start; k <= end; k++) {
                avg = avg + column[k];
                count++;
            }
            avg = avg / count;
            reducedColumn[j] = avg;

            // Compute next segment
            start = std::ceil(end);
            end = end + elemPerBin;
            end = (end > n) ? n : end;
        }

        // First Column
        af::array col(bins, 1, reducedColumn.data());
        if (i == 0) {
            result = col;
        } else {
            result = af::join(1, result, col);
        }
    }
    return result;
}

float calculateError(const std::vector<Point> &ts, int start, int end) {
    Point p1 = ts[start];
    Point p2 = ts[end];

    // We use the point-slope equation for the middle points between start and end: y = mx - mx_1 + y_1
    // where m = (y_2 - y_1) / (x_2 - x_1)
    float m = (p2.second - p1.second) / (p2.first - p1.first);

    return std::accumulate(ts.cbegin() + start, ts.cbegin() + end + 1, 0.0f, [&p1, m](float acc, Point p) {
        return acc + std::pow(p.second - (m * (p.first - p1.first) + p1.second), 2);
    });
}

Segment merge(Segment s1, Segment s2) { return {s1.first, s2.second}; }

int64_t computeTriangleArea(const VisvalingamSummaryPoint &a, const VisvalingamSummaryPoint &b,
                            const VisvalingamSummaryPoint &c, const long scale = 1e9) {
    float f1 = a.x * (b.y - c.y);
    float f2 = b.x * (c.y - a.y);
    float f3 = c.x * (a.y - b.y);
    return static_cast<int64_t>(std::abs((static_cast<double>(f1) + f2 + f3) / 2.0f) * scale);
}

template <typename Iter, typename Distance>
Iter shiftIterator(Iter iter, Distance positions) {
    std::advance(iter, positions);
    return iter;
}

class mapComparator {
   public:
    bool operator()(const std::pair<int64_t, int64_t> &p1, const std::pair<int64_t, int64_t> &p2) const {
        return ((p1.first < p2.first) || ((p1.first == p2.first) && (p1.second < p2.second)));
    }
};

void recomputeAreaNeighbor(std::map<int64_t, VisvalingamSummaryPoint>::iterator &iterator_point,
                           std::set<std::pair<int64_t, int64_t>, mapComparator> &point_indexer,
                           std::map<int64_t, VisvalingamSummaryPoint> &points, const int64_t scale) {
    auto im1m1 = shiftIterator(iterator_point, -1);
    auto im1p1 = shiftIterator(iterator_point, 1);
    auto original_position_minus1 = iterator_point->first;

    auto old_area_minus1 = iterator_point->second.area;
    auto new_area_minus1 = computeTriangleArea(im1m1->second, iterator_point->second, im1p1->second, scale);
    points[iterator_point->first] =
        VisvalingamSummaryPoint{iterator_point->second.x, iterator_point->second.y, new_area_minus1};

    auto it = point_indexer.find(std::make_pair(old_area_minus1, original_position_minus1));
    point_indexer.erase(it);

    point_indexer.insert(std::pair<int64_t, int64_t>(std::make_pair(new_area_minus1, original_position_minus1)));
}

}  // namespace

std::vector<Point> gauss::dimensionality::PAA(const std::vector<Point> &points, int bins) {
    float xrange = points.back().first - points.front().first;
    float width_bin = xrange / bins;
    float reduction = bins / xrange;

    std::vector<float> sum(bins, 0.0);
    std::vector<int> counter(bins, 0);

    // Iterating over the time series
    for (const auto &p : points) {
        auto pos = static_cast<size_t>(std::min(p.first * reduction, (float)(bins - 1)));
        sum[pos] += p.second;
        counter[pos] += 1;
    }

    std::vector<Point> result;
    result.reserve(bins);
    // Compute the average per bin
    for (int i = 0; i < bins; ++i) {
        result.emplace_back((width_bin * i) + (width_bin / 2.0f), sum[i] / counter[i]);
    }
    return result;
}

af::array gauss::dimensionality::PAA(const af::array &a, int bins) {
    // Resulting array
    af::array result;

    // Check dimensions are divisible, if not, call CPU version
    if (a.dims(0) % bins == 0) {
        auto n = a.dims(0);
        auto elem_row = n / bins;

        af::array b = af::moddims(a, elem_row, bins, a.dims(1));
        af::array addition = af::sum(b, 0);
        result = af::reorder(addition / elem_row, 1, 2, 0, 3);
    } else {
        // Call the CPU version
        if (a.type() == af::dtype::f64) {
            result = PAA_CPU<double>(a, bins);
        } else if (a.type() == af::dtype::f32) {
            result = PAA_CPU<float>(a, bins);
        }
    }

    return result;
}

af::array gauss::dimensionality::PIP(const af::array &ts, int numberIPs) {
    if (ts.dims(1) != 2) {
        throw std::invalid_argument("Invalid dims. Khiva array with two columns expected (x axis and y axis).");
    }
    auto n = ts.dims(0);
    auto end = n - 1;

    if (n < 2) {
        throw std::invalid_argument("We can't delete all those important points");
    } else if (n <= numberIPs) {
        return ts;
    }

    // Extracting info from af::array
    auto h_x = gauss::utils::makeScopedHostPtr(ts.col(0).host<float>());
    auto h_y = gauss::utils::makeScopedHostPtr(ts.col(1).host<float>());

    // Converting c-arrays to vector of points
    std::vector<Point> points;
    for (int i = 0; i < n; i++) {
        points.emplace_back(h_x[i], h_y[i]);
    }

    // Allocating vectors for selected points
    std::vector<Point> selected;
    selected.emplace_back(points[0]);
    selected.emplace_back(points[end]);

    // we have to find (numberIPs - 2) points, as we have already included P[0] and P[end].
    // Number of passes over the collection
    for (int p = 0; p < (numberIPs - 2); p++) {
        float dmax = -1.0;
        int index = -1;
        int position = -1;

        // Find the next PIP
        for (int i = 1; i < end; i++) {
            // We first check if the point is already in the list.
            if (!isPointInDesiredList(points[i], selected)) {
                // segment contains the indices of the selected points which are the boundaries for the point i.
                std::pair<int, int> segment = getSegmentFromSelected(points[i], selected);
                float d = verticalDistance(points[i], selected[segment.first], selected[segment.second]);
                // We store the point with the maximum distance to the line that connects the segment.
                if (d > dmax) {
                    index = i;
                    dmax = d;
                    position = segment.second;
                }
            }
        }
        insertPointBetweenSelected(points[index], position, selected);
    }

    // Converting from vector to array
    std::vector<float> x;
    x.reserve(selected.size());
    std::vector<float> y;
    y.reserve(selected.size());
    for (auto &i : selected) {
        x.emplace_back(i.first);
        y.emplace_back(i.second);
    }

    // from c-array to af::array
    af::array tsx(selected.size(), 1, x.data());
    af::array tsy(selected.size(), 1, y.data());
    af::array res = af::join(1, tsx, tsy);

    return res;
}

std::vector<Point> gauss::dimensionality::PLABottomUp(const std::vector<Point> &ts, float maxError) {
    std::vector<Segment> segments;
    segments.reserve(ts.size());

    // Allocating vector of segments
    for (size_t i = 0; i < ts.size() - 1; i = i + 2) {
        segments.emplace_back(i, i + 1);
    }

    std::vector<float> mergeCost;
    mergeCost.reserve(segments.size());
    for (size_t i = 0; i < segments.size() - 1; i++) {
        mergeCost.emplace_back(calculateError(ts, segments[i].first, segments[i + 1].second));
    }

    // Calculate minimum, calculating in advance
    auto minCost = std::min_element(std::begin(mergeCost), std::end(mergeCost));
    while ((segments.size() > 2) && (*minCost < maxError)) {
        // We have to merge
        auto index = std::distance(std::begin(mergeCost), minCost);

        // Merge candidate segments
        segments[index] = merge(segments[index], segments[index + 1]);

        // Delete fused segment
        segments.erase(segments.begin() + index + 1);
        mergeCost.erase(mergeCost.begin() + index + 1);

        // Calculate new cost
        mergeCost[index] = calculateError(ts, segments[index].first, segments[index + 1].second);
        mergeCost[index - 1] = calculateError(ts, segments[index - 1].first, segments[index].second);

        // Calculate new minimum
        minCost = std::min_element(std::begin(mergeCost), std::end(mergeCost));
    }

    // Build a polyline from a set of segments
    std::vector<Point> result;
    result.reserve(segments.size());
    for (auto &segment : segments) {
        result.emplace_back(ts[segment.first]);
        result.emplace_back(ts[segment.second]);
    }

    return result;
}

af::array gauss::dimensionality::PLABottomUp(const af::array &ts, float maxError) {
    if (ts.dims(1) != 2) {
        throw std::invalid_argument("Invalid dims. Khiva array with two columns expected (x axis and y axis).");
    }
    // Extracting info from af::array
    auto h_x = gauss::utils::makeScopedHostPtr(ts.col(0).host<float>());
    auto h_y = gauss::utils::makeScopedHostPtr(ts.col(1).host<float>());

    std::vector<Point> points;
    points.reserve(ts.dims(0));

    // Creating a vector of Points
    for (int i = 0; i < ts.dims(0); i++) {
        points.emplace_back(h_x[i], h_y[i]);
    }

    std::vector<Point> reducedPoints = PLABottomUp(points, maxError);
    std::vector<float> x;
    x.reserve(reducedPoints.size());
    std::vector<float> y;
    y.reserve(reducedPoints.size());

    // Converting from vector to array
    for (const auto &point : reducedPoints) {
        x.emplace_back(point.first);
        y.emplace_back(point.second);
    }

    // from c-array to af::array
    af::array tsx(reducedPoints.size(), 1, x.data());
    af::array tsy(reducedPoints.size(), 1, y.data());
    af::array res = af::join(1, tsx, tsy);

    return res;
}

std::vector<Point> gauss::dimensionality::PLASlidingWindow(const std::vector<Point> &ts, float maxError) {
    std::vector<Segment> segments;

    size_t anchor = 0;
    size_t i;

    // We haven´t explored the whole time series
    while (anchor < (ts.size() - 1)) {
        i = 1;
        while (((anchor + i) < ts.size()) &&
               (calculateError(ts, static_cast<int>(anchor), static_cast<int>(anchor + i)) < maxError)) {
            i = i + 1;
        }

        if ((anchor + i) == (ts.size() - 1)) {
            segments.emplace_back(anchor, anchor + i);
        } else {
            segments.emplace_back(anchor, anchor + i - 1);
        }
        anchor += i;
    }

    // Build a polyline from a set of segments
    std::vector<Point> result;
    result.reserve(segments.size());
    for (auto &segment : segments) {
        result.emplace_back(ts[segment.first]);
        result.emplace_back(ts[segment.second]);
    }

    return result;
}

af::array gauss::dimensionality::PLASlidingWindow(const af::array &ts, float maxError) {
    if (ts.dims(1) != 2) {
        throw std::invalid_argument("Invalid dims. Khiva array with two columns expected (x axis and y axis).");
    }
    // Extracting info from af::array
    auto h_x = gauss::utils::makeScopedHostPtr(ts.col(0).host<float>());
    auto h_y = gauss::utils::makeScopedHostPtr(ts.col(1).host<float>());

    std::vector<Point> points;
    points.reserve(ts.dims(0));

    // Creating a vector of Points
    for (int i = 0; i < ts.dims(0); i++) {
        points.emplace_back(h_x[i], h_y[i]);
    }

    std::vector<Point> reducedPoints = PLASlidingWindow(points, maxError);
    std::vector<float> x;
    x.reserve(reducedPoints.size());
    std::vector<float> y;
    y.reserve(reducedPoints.size());
    // Converting from vector to array
    for (const auto &point : reducedPoints) {
        x.emplace_back(point.first);
        y.emplace_back(point.second);
    }

    // from c-array to af::array
    af::array tsx(reducedPoints.size(), 1, x.data());
    af::array tsy(reducedPoints.size(), 1, y.data());
    af::array res = af::join(1, tsx, tsy);

    return res;
}

std::vector<Point> gauss::dimensionality::ramerDouglasPeucker(const std::vector<Point> &pointList, double epsilon) {
    std::vector<Point> out;

    if (pointList.size() < 2) throw std::invalid_argument("Not enough points to simplify ...");

    // Find the point with the maximum distance from line between start and end
    double dmax = 0.0;
    size_t index = 0;
    size_t end = pointList.size() - 1;

    for (size_t i = 1; i < end; i++) {
        double d = PerpendicularDistance(pointList[i], pointList[0], pointList[end]);
        if (d > dmax) {
            index = i;
            dmax = d;
        }
    }

    // If max distance is greater than epsilon, recursively simplify
    if (dmax > epsilon) {
        std::vector<Point> firstLine(pointList.begin(), pointList.begin() + index + 1);
        std::vector<Point> lastLine(pointList.begin() + index, pointList.end());
        auto recResults1 = ramerDouglasPeucker(firstLine, epsilon);
        auto recResults2 = ramerDouglasPeucker(lastLine, epsilon);

        // Build the result list
        out.assign(recResults1.begin(), recResults1.end() - 1);
        out.insert(out.end(), recResults2.begin(), recResults2.end());
        if (out.size() < 2) throw std::runtime_error("Problem assembling output");
    } else {
        // Just return start and end points
        out.clear();
        out.emplace_back(pointList[0]);
        out.emplace_back(pointList[end]);
    }
    return out;
}

af::array gauss::dimensionality::ramerDouglasPeucker(const af::array &pointList, double epsilon) {
    if (pointList.dims(1) != 2) {
        throw std::invalid_argument("Invalid dims. Khiva array with two columns expected (x axis and y axis).");
    }

    auto x = gauss::utils::makeScopedHostPtr(pointList.col(0).host<float>());
    auto y = gauss::utils::makeScopedHostPtr(pointList.col(1).host<float>());

    std::vector<Point> points;
    points.reserve(pointList.dims(0));
    for (int i = 0; i < pointList.dims(0); i++) {
        points.emplace_back(x[i], y[i]);
    }

    std::vector<Point> rPoints = ramerDouglasPeucker(points, epsilon);
    af::array out = af::constant(0, rPoints.size(), 2);

    std::vector<float> vx;
    vx.reserve(rPoints.size());
    std::vector<float> vy;
    vy.reserve(rPoints.size());

    for (const auto &rPoint : rPoints) {
        vx.emplace_back(rPoint.first);
        vy.emplace_back(rPoint.second);
    }

    af::array ox(rPoints.size(), vx.data());
    af::array oy(rPoints.size(), vy.data());

    return af::join(1, ox, oy);
}

af::array gauss::dimensionality::SAX(const af::array &a, int alphabet_size) {
    if (a.dims(1) != 2) {
        throw std::invalid_argument("Invalid dims. Khiva array with two columns expected (x axis and y axis).");
    }

    af::array result = af::constant(0.0, a.dims());
    // Let's store the x-axis.
    result(af::span, 0) += a.col(0);

    // Let's compute the y-axis.
    for (int k = 1; k < a.dims(1); k++) {
        af::array ts = a.col(k);
        auto mean_value = af::mean<float>(ts);
        auto std_value = af::stdev<float>(ts);
        dim_t n = ts.dims(0);
        std::vector<int> aux(n, 0);

        if (std_value > 0) {
            std::vector<float> breakingPoints = computeBreakpoints(alphabet_size, mean_value, std_value);
            std::vector<int> alphabet = generateAlphabet(alphabet_size);
            auto a_h = gauss::utils::makeScopedHostPtr(ts.host<float>());

            // Iterate across elements of ts
            for (int i = 0; i < n; i++) {
                size_t j = 0;
                while ((j < breakingPoints.size()) && (a_h[i] > breakingPoints[j])) {
                    j++;
                }
                aux[i] = alphabet[j];
            }
        }

        // from c-array to af::array
        af::array res(aux.size(), 1, aux.data());
        result(af::span, k) += res;
    }

    return result;
}

std::vector<Point> gauss::dimensionality::visvalingam(const std::vector<Point> &pointList, int64_t numPoints, int64_t scale) {
    std::map<int64_t, VisvalingamSummaryPoint> points;
    std::set<std::pair<int64_t, int64_t>, mapComparator> point_indexer;
    int64_t counter = 0;

    std::transform(
        pointList.cbegin(), pointList.cend(), std::inserter(points, points.end()), [&counter](const Point &point) {
            return std::make_pair(
                counter++, VisvalingamSummaryPoint{point.first, point.second, std::numeric_limits<int64_t>::max()});
        });

    auto points_to_be_deleted = pointList.size() - numPoints;
    auto point_iterator = point_indexer.begin();

    // Precompute areas
    for (auto it = shiftIterator(points.begin(), 1); it != shiftIterator(points.end(), -1); ++it) {
        auto area = computeTriangleArea(shiftIterator(it, -1)->second, it->second, shiftIterator(it, 1)->second, scale);
        it->second.area = area;
        point_iterator = point_indexer.insert(point_iterator, std::make_pair(area, it->first));
    }

    // One point to be deleted on each iteration
    for (size_t iter = 0; iter < points_to_be_deleted; iter++) {
        auto min_index_iterator = point_indexer.begin();
        int64_t min_element = min_index_iterator->second;
        point_indexer.erase(min_index_iterator);

        auto iterator_point = points.find(min_element);
        auto iterator_point_minus1 = shiftIterator(iterator_point, -1);
        auto iterator_point_plus1 = shiftIterator(iterator_point, 1);

        points.erase(iterator_point);

        if (iterator_point_minus1->first > 0) {
            recomputeAreaNeighbor(iterator_point_minus1, point_indexer, points, scale);
        }

        if (iterator_point_plus1->first < counter - 1) {
            recomputeAreaNeighbor(iterator_point_plus1, point_indexer, points, scale);
        }
    }

    std::vector<Point> out_vector;
    out_vector.reserve(numPoints);
    std::transform(points.cbegin(), points.cend(), std::back_inserter(out_vector),
                   [](const std::pair<int64_t, VisvalingamSummaryPoint> &p) {
                       return Point{p.second.x, p.second.y};
                   });

    return out_vector;
}

af::array gauss::dimensionality::visvalingam(const af::array &pointList, int numPoints) {
    if (pointList.dims(1) != 2) {
        throw std::invalid_argument("Invalid dims. Array with two columns expected (x axis and y axis).");
    }

    std::vector<Point> points;
    points.reserve(pointList.dims(0));
    auto x = gauss::utils::makeScopedHostPtr(pointList.col(0).host<float>());
    auto y = gauss::utils::makeScopedHostPtr(pointList.col(1).host<float>());

    for (int i = 0; i < pointList.dims(0); i++) {
        points.emplace_back(x[i], y[i]);
    }

    std::vector<Point> rPoints = visvalingam(points, numPoints);
    af::array out = af::constant(0, rPoints.size(), 2);

    std::vector<float> vx;
    vx.reserve(rPoints.size());
    std::vector<float> vy;
    vy.reserve(rPoints.size());

    for (const auto &rPoint : rPoints) {
        vx.emplace_back(rPoint.first);
        vy.emplace_back(rPoint.second);
    }

    af::array ox(rPoints.size(), vx.data());
    af::array oy(rPoints.size(), vy.data());

    return af::join(1, ox, oy);
}


/*********************************************************************
 * BSD 3-Clause License
 *
 * Copyright (c) 2018, Rawashdeh Research Group
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met:
 *
 * 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer.
 *
 * 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the
 *    documentation and/or other materials provided with the distribution.
 *
 * 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from
 *    this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
 * HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
 * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
 * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
 * THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 **********************************************************************/
// Author: Mohamed Aladem

#include "lvt_motion_model.h"
#include <Eigen/Geometry>

lvt_motion_model::lvt_motion_model()
{
    reset();
}

void lvt_motion_model::reset()
{
    m_last_q.setIdentity();
    m_angular_velocity.setIdentity();
    m_last_position.setZero();
    m_linear_velocity.setZero();
}

lvt_pose lvt_motion_model::predict_next_pose(const lvt_pose &current_pose)
{
    // compute new linear velocity
    lvt_vector3 new_lin_velocity = current_pose.get_position() - m_last_position;
    new_lin_velocity = (new_lin_velocity + m_linear_velocity) * 0.5; // smooth velocity over time

    // compute new angular velocity
    lvt_quaternion current_q = current_pose.get_orientation_quaternion();
    lvt_quaternion ang_vel_diff = current_q * m_last_q.inverse();
    lvt_quaternion new_ang_vel = ang_vel_diff.slerp(0.5, m_angular_velocity);
    new_ang_vel.normalize();

    // Update state
    m_last_q = current_q;
    m_angular_velocity = new_ang_vel;
    m_last_position = current_pose.get_position();
    m_linear_velocity = new_lin_velocity;

    // Integrate to compute predictions
    lvt_vector3 int_pos = m_last_position + m_linear_velocity;
    lvt_quaternion int_q = current_q * new_ang_vel;
    int_q.normalize();
    return lvt_pose(int_pos, int_q);
}


#include <iostream>
#include <algorithm>
#include <functional>
#include <iomanip> // std::setwのみ
#include <random>
#include <boost/range/algorithm.hpp>
#include <data_type.hpp>
#include <image_algorithm/genetic.hpp>

void genetic::shuffle(std::vector<int>& arr) const
{
    std::uniform_int_distribution<int> distribution(0, arr.size()-1);

    for(int i = 0; i < arr.size(); ++i)
    {
        int j = distribution(mt_);
        std::swap(arr[i], arr[j]);
    }
}

void genetic::shuffle_all()
{
    for(auto it = children_.begin(); it != children_.end(); ++it)
    {
        shuffle(it->children_gene);
    }
}

unsigned int genetic::assess(std::vector<int> const& arr) const
{
    auto const sepx = data_.split_num.first;
    auto const sepy = data_.split_num.second;

    unsigned int ass = 0;
    for(int i = 0; i < arr.size(); ++i)
    {
        if(i + 1 < arr.size())
        {
            if(arr[i] == arr[i + 1])
            {
                std::cout << "!!!" << arr[i] << arr[i + 1] << std::endl;
            }
            ass += comp_[arr[i] / sepx][arr[i] % sepx][arr[i + 1] / sepx][arr[i + 1] % sepx].right;
        }
        if(i + sepy < arr.size())
        {
            if(arr[i] == arr[i + sepy])
            {
                std::cout << "???" << arr[i] << arr[i + sepy] << std::endl;
            }
            ass += comp_[arr[i] / sepx][arr[i] % sepx][arr[i + sepy] / sepx][arr[i + sepy] % sepx].right;
        }
    }
    return ass;
}

void genetic::assess_all()
{
    for(auto it = children_.begin(); it != children_.end(); ++it)
    {
        //for(int i = 0; i < split_total; ++i)
        //{
        //    std::cout << it->children_gene[i] << " - ";
        //}
        it->assessment = assess(it->children_gene);
		//std::cout << "     " << it->assessment << std::endl;
    }
}

std::vector<std::vector<int>> genetic::select_parent(std::vector<int>& top10)
{
    top10.assign(PARENT_NUM, 0);

    std::vector<int> childred_copy;
    childred_copy.reserve(children_.size());

    boost::transform(
        children_, std::back_inserter(childred_copy),
        [](children_t const& base){ return base.assessment; }
        );
    boost::sort(childred_copy);
    //boost::for_each(childred_copy, [](int const base){ std::cout << base << std::endl; });

    for(int i=0; i<top10.size(); ++i)
    {
        for(int j=0; j<CHILDREN_NUM; ++j)
        {
            if(childred_copy.at(i) == children_[j].assessment)
            {
                top10[i] = j;
            }
        }
    }

    auto const split_total = data_.split_num.first * data_.split_num.second;
    std::vector<std::vector<int>> parent_gene(PARENT_NUM, std::vector<int>(split_total, 0));

    //親遺伝子配列にいいやつTop10を突っ込む
    for(int i=0; i<top10.size(); ++i)
    {
        for(int j=0; j<split_total; ++j)
        {
            parent_gene[i][j] = children_[top10[i]].children_gene[j];
        }
    }

    std::cout << "番号  " << "評価" << std::endl;
    for(auto const& elem : top10)
    {
        std::cout << elem << "   " << children_[elem].assessment << std::endl;
    }

    return parent_gene;
}

void genetic::cross_over(std::vector<std::vector<int>> const& parent_gene)
{
    auto const split_width  = data_.split_num.first;
    auto const split_height = data_.split_num.second;
    auto const split_total  = split_width * split_height;

    std::uniform_int_distribution<int> parent_dist(0, PARENT_NUM - 1);
    std::uniform_int_distribution<int> mutate_dist(0, 4 - 1);
    std::uniform_int_distribution<int> split_dist(0, split_total - 1);

    for(int i=0; i<CHILDREN_NUM; i+=2)
    {
        auto const& pra1 = parent_gene[parent_dist(mt_)];
        auto const& pra2 = parent_gene[parent_dist(mt_)];
        auto      & child1 = children_[i  ].children_gene;
        auto      & child2 = children_[i+1].children_gene;

        //std::cout << "par1 ";
        //boost::for_each(pra1, [](int const elem){ std::cout << elem << "*"; });
        //std::cout << std::endl;
        //
        //std::cout << "par2 ";
        //boost::for_each(pra2, [](int const elem){ std::cout << elem << "*"; });
        //std::cout << std::endl;

        child1.assign(child1.size(), -1);
        child2.assign(child2.size(), -1);

        int select_gene = split_dist(mt_);
        //std::cout << "  s" << select_gene;
        while(child1[select_gene] != pra1[select_gene])
        {
			child1[select_gene] = pra1[select_gene];
			child2[select_gene] = pra2[select_gene];

            for(int i=0; i<split_total; ++i)
            {
                if(child2[select_gene] == pra1[i])
                {
                    select_gene = i;
                }
            }
            //std::cout << "  s" << select_gene;
        }

        for(int i=0; i<split_total; ++i)
        {
            if(child1[i] == -1)
            {
				child1[i] = pra2[i];
				child2[i] = pra1[i];
            }
        }

        for(int n = parent_dist(mt_); n < 10; ++n) //このループ数をいじらないと収束しなかったりすぐに収束してしまったり
        {
            if(mutate_dist(mt_) == 0) //ここの確率で突然変異率を変化させる
            {
                int rand1 = split_dist(mt_);
                int rand2 = split_dist(mt_);
                std::swap(child1[rand1], child1[rand2]);
                //std::cout << "rand1" << std::endl;
            }
            if(mutate_dist(mt_) == 0) //ここの確率で突然変異率を変化させる
            {
                int rand1 = split_dist(mt_);
                int rand2 = split_dist(mt_);
                std::swap(child2[rand1], child2[rand2]);
				//std::cout << "rand2" << std::endl;
            }
        }

        //std::cout << std::endl;
        //std::cout << "chi1 ";
        //boost::for_each(child1, [](int const elem){ std::cout << elem << "+"; });
        //
        //std::cout << std::endl;
        //std::cout << "chi2 ";
        //boost::for_each(child2, [](int const elem){ std::cout << elem << "+"; });
        //
        //std::cout << std::endl;
    }
}

genetic::genetic(question_raw_data const& data, compared_type const& comp)
    : data_(data), comp_(comp)
{
    //メルセンヌ・ツイスタの初期化
    std::random_device rd;
    std::vector<std::uint_least64_t> random_seed(10);
    boost::generate(random_seed, std::ref(rd));

    std::seed_seq seed(random_seed.cbegin(), random_seed.cend());
    mt_.seed(seed);
}

std::vector<std::vector<std::vector<point_type>>> genetic::operator() ()
{
    auto const split_total = data_.split_num.first * data_.split_num.second;

    // children_の初期化
    {
        //連番リスト(0~split_total-1)の作製
        std::vector<int> tmp_vector;
        tmp_vector.reserve(split_total);
        for(int i = 0; i < split_total; ++i) tmp_vector.push_back(i);

        //連番リストでchildren_を初期化
        children_.assign(CHILDREN_NUM, {tmp_vector, 0});
    }

    shuffle_all();
    assess_all();

    int top;
    std::vector<int> top10(PARENT_NUM);
    std::vector<int> toppers(1000);
    for(int i = 0; i < toppers.size(); ++i)
    {
        for(int j = 0; j < 10000; ++j)
        {
            auto const parent_gene = select_parent(top10);
            top = children_.at(top10[0]).assessment;

            cross_over(parent_gene);
            assess_all();

			std::cout << "最上位遺伝子";
			for (int i = 0; i < split_total; i++)
            {
				std::cout << std::setw(3) << parent_gene[0][i]; //unspecified(std::setw)
			}

            std::cout << "番号" << top10[0] << std::endl;

            int const base = top10[0]; // VCのための苦肉の策
            if(
                std::all_of(
                    top10.begin(), top10.end(),
                    [base](int const elem){ return base == elem; }
                    //[base = top10[0]](int const elem){ return base == elem; }
                )
            )
            {
                //Top10の遺伝子がすべて同じものになったら収束したと判断する
                break;
            }
            std::cout << "世代=" << j << "ループ" << i << std::endl;

        }
        toppers[i] = top;

        shuffle_all();
    }

	int toptop = std::numeric_limits<int>::max();
	int topnum = 0;
	for(auto const elem : toppers)
    {
        if(elem < toptop)
        {
            toptop = elem;
            topnum = 0;
        }
        else if(elem == toptop)
        {
            ++topnum;
        }
		std::cout << elem << std::endl;
    }
	std::cout << "最適値" << toptop << "数" << topnum << std::endl;

    return {}; //ごめん，どこが答えか分からなかったから空白返す．
}


// Compile command: g++ -Wall -Wextra -std=c++17 -O2 -pthread -I/usr/include/python3.8 -I/usr/include/eigen3 -o triangular_cone_newdisloc triangular_cone_newdisloc.cpp -lpython3.8
#include <iostream>
#include <fstream>
#include <cmath>
#include <complex>
#include <Eigen/Dense>
#include <unsupported/Eigen/MatrixFunctions>
#include <vector>
#include <utility>
#include <functional>
#include <thread>
#include <map>
#include <string>
#include <algorithm>
#include <ctime>
#include <iomanip>
#include <filesystem>
#include "matplotlibcpp.h"

namespace plt = matplotlibcpp;
using Eigen::MatrixXd;
using Eigen::Matrix2cd;
using Eigen::Vector2cd;
using std::vector;
using std::sqrt;
using std::cos;
using std::acos;
using std::sin;
using std::atan2;
using namespace std::complex_literals;

typedef std::complex<double> cd;
typedef vector<vector<vector<cd>>> grid_t;
typedef std::function<Matrix2cd(int, double, double)> gen_coin_t;

// Settings here
int num_steps = 1000;
double dy = sqrt(3);
bool plotCone = true; // Set to true to plot the results in the cone coordinates
int ymin = -200, ymax = 200;
int xmin = 3*ymin/2-ymin%2-1, xmax = 3*ymax/2+ymax%2+1;
int ntriangles_x = xmax-xmin+1;
int ntriangles_y = ymax-ymin+1;
int center[2] = {-xmin, -ymin};
std::string prefix;
int initialState = 0;
vector<std::string> initialStateName = {"square", "shifttop", "shiftdl", "center", "bothsides", "everywhere"};

std::time_t now = time(0);
std::tm *ltm = localtime(&now);

grid_t zerogrid() {
    return vector<vector<vector<cd>>>(ntriangles_x, vector<vector<cd>> (ntriangles_y, vector<cd> (3, 0. + 0i)));
}
grid_t nangrid() {
    return vector<vector<vector<cd>>>(ntriangles_x, vector<vector<cd>> (ntriangles_y, vector<cd> (3, std::nan("") + 0i)));
}

grid_t grid = zerogrid();

Matrix2cd U, C0;
Matrix2cd sigmay, sigmaz;

double sumAmplitudes() {
    double ret = 0.0;
    for(int x = xmin; x <= xmax; x++)
        for(int y = ymin; y <= ymax; y++)
            for(int side = 0; side < 3; side++) {
                cd val = grid[x+center[0]][y+center[1]][side];
                ret += std::real(val*std::conj(val));
            }
    return ret;
}

// Simplifies initialization for some initial states
void normalizeGrid() {
    double target = sumAmplitudes();
    double mul = 1/sqrt(target);
    if(target == 0)
        return;
    for(int x = xmin; x <= xmax; x++)
        for(int y = ymin; y <= ymax; y++)
            for(int side = 0; side < 3; side++)
                grid[x+center[0]][y+center[1]][side] *= mul;
}

inline int modulo(int a, int b) {
    return (a%b+b)%b;
}

void init_HQ() {
    sigmaz <<
        1,0,
        0,-1;
    sigmay <<
        0, -1.0i,
        1.0i, 0;
    C0 = std::exp(1.0i*M_PI/3.0) * ((1.0i*M_PI/3.0)*sigmaz).exp();
    double alpha = -std::acos(sqrt(5)/3);
    Matrix2cd U0 = (-1.0i*alpha*sigmay/2.0).exp();
    U = U0*C0*C0;
}

inline int sign(double d) {
    if(d>=0)
        return 1;
    else
        return -1;
}

inline double sq(double d) {
    return d*d;
}

inline double correct_fmod(const double a, const double b) {
    return std::fmod(std::fmod(a,b)+b, b);
}

inline double principal_measure(const double theta) {
    double ret = correct_fmod(theta, 2*M_PI);
    if(ret > M_PI)
        ret -= 2*M_PI;
    return ret;
}

inline double sqmodulus(const std::complex<double> comp) {
    return std::real(comp*std::conj(comp));
}

grid_t shift(grid_t grid) {
    grid_t ngrid = zerogrid();
    for(int x = xmin; x <= xmax; x++) {
        for(int y = ymin; y <= ymax; y++) {
            for(int i = 0; i < 3; i++) {
                int iprec = ((i-1)%3+3)%3;
                ngrid[x+center[0]][y+center[1]][i] = grid[x+center[0]][y+center[1]][iprec];
            }
        }
    }
    return ngrid;
}

std::pair<double, double> real_coords(int iside, int x, int y, bool show = false) {
    double dec;
    if(show)
        dec = .4;
    else
        dec = .5;
    double xcoord, ycoord;
    // Dislocation: particular case
    if(x == 0 && y >= 0) { // Vertical line
        if((x+y)%2 == 0) { // Tip down
            if(iside == 0) {
                xcoord = x-dec;
                ycoord = (y+.5)*dy;
            }
            else if(iside == 1) {
                xcoord = x;
                ycoord = (y+.5)*dy;
            }
            else {
                xcoord = x-dec;
                ycoord = (y+.5+dec)*dy;
            }
        }
        else { // Tip up
            if(iside == 0) {
                xcoord = x;
                ycoord = (y+.5)*dy;
            }
            else if(iside == 1) {
                xcoord = x-dec;
                ycoord = (y+.5)*dy;
            }
            else {
                xcoord = x-dec;
                ycoord = (y+.5-dec)*dy;
            }
        }
    }
    else if(y >= 0 && (x == 3*y+1 || x == 3*y+2)) { // Diagonal line
        if((x+y)%2 == 0) { // Tip down
            if(iside == 0) {
                xcoord = x-dec/2.;
                ycoord = (y+.5-dec/2.)*dy;
            }
            else if(iside == 1) {
                xcoord = x+dec;
                ycoord = (y+.5)*dy;
            }
            else {
                xcoord = x+dec/2.;
                ycoord = (y+.5+dec/2.)*dy;
            }
        }
        else { // Tip up
            if(iside == 0) {
                xcoord = x+dec+dec/2.;
                ycoord = (y+.5-dec/2.)*dy;
            }
            else if(iside == 1) {
                xcoord = x-dec/2.;
                ycoord = (y+.5-dec/2.)*dy;
            }
            else {
                xcoord = x;
                ycoord = (y+.5-dec)*dy;
            }
        }
    }
    else if((x+y)%2==0) {
        if(iside == 0) {
            xcoord = x-dec;
            ycoord = (y+.5)*dy;
        }
        else if(iside == 1) {
            xcoord = x+dec;
            ycoord = (y+.5)*dy;
        }
        else {
            xcoord = x;
            ycoord = (y+.5+dec)*dy;
        }
    }
    else {
        if(iside == 0) {
            xcoord = x+dec;
            ycoord = (y+.5)*dy;
        }
        else if(iside == 1) {
            xcoord = x-dec;
            ycoord = (y+.5)*dy;
        }
        else {
            xcoord = x;
            ycoord = (y+.5-dec)*dy;
        }
    }
    return std::make_pair(xcoord, ycoord);
}

std::pair<double, double> cone_coords(int iside, int x, int y, bool show = false) {
    double rx, ry;
    std::tie(rx, ry) = real_coords(iside, x, y, show);
    double r = 5./6.*sqrt(rx*rx+ry*ry);
    double theta_before = atan2(ry, rx);
    double theta_rotate = principal_measure(theta_before+4*M_PI/6.);
    double theta = 6./5.*theta_rotate;
    return std::make_pair(r*cos(theta), r*sin(theta));
}

const int DELTAS[][2] = {{1,0}, {-1,0}, {0,-1}};
const int NUM_THREADS = 8;

/**
 * For the new dislocation:
 * - triangles (0,y), y >= 0, have triangle (3*y//2 + y%2 + 1, y//2) as a neighbor.
 *   Since we only consider tip up triangles, with (x+y)%2 == 1, we simply need to know that
 *   triangles (0, y), y >= 0 and y odd, have side 2 of triangle (3*(y-1)/2+2, (y-1)/2) as a neighbor at side 0.
 * - triangles (3y+1, y), y >= 0 have side 1 of triangle (0, 2y) as a neighbor at side 1.
 */
void applyCoinsPartial(grid_t &ngrid, grid_t &grid, const Matrix2cd &coin, int loc_xmin, int loc_xmax) {
    for(int x = loc_xmin; x < loc_xmax; x++) {
        for(int y = ymin; y <= ymax; y++) {
            if((x+y)%2==0 || (x>0 && y>(x-1)/3))
                continue;
            for(int iside = 0; iside < 3; iside++) {
                int otherside = iside;
                cd thisval = grid[x+center[0]][y+center[1]][iside];
                int xo = x + DELTAS[iside][0], yo = y + DELTAS[iside][1];
                if(xo > xmax || xo < xmin || yo > ymax || yo < ymin) { // No propagation at borders
                    ngrid[x+center[0]][y+center[1]][iside] = thisval;
                    continue;
                }
                if(x == 0 && y >= 0 && iside == 0) {
                    xo = 3*(y-1)/2+2;
                    yo = (y-1)/2;
                    otherside = 2;
                }
                else if(y >= 0 && x == 3*y+1 && iside == 1) {
                    xo = 0;
                    yo = 2*y;
                    otherside = 1;
                }
                cd otherval = grid[xo+center[0]][yo+center[1]][otherside];
                Vector2cd vect;
                vect << thisval, otherval;
                Vector2cd newvect = coin*vect;
                ngrid[x+center[0]][y+center[1]][iside] = newvect(0);
                ngrid[xo+center[0]][yo+center[1]][otherside] = newvect(1);
            }
        }
    }
}

grid_t applyCoins(grid_t grid, const Matrix2cd &coin, bool multithread = true) {
    grid_t ngrid = nangrid();
    if(!multithread) {
        for(int x = xmin; x <= xmax; x++) {
            for(int y = ymin; y <= ymax; y++) {
                if((x+y)%2==0 || (x>0 && y>(x-1)/3)) // dislocation : remove a 60-degree part of the lattie
                    continue;
                for(int iside = 0; iside < 3; iside++) {
                    int otherside = iside;
                    cd thisval = grid[x+center[0]][y+center[1]][iside];
                    int xo = x + DELTAS[iside][0], yo = y + DELTAS[iside][1];
                    if(xo > xmax || xo < xmin || yo > ymax || yo < ymin) { // No propagation at borders
                        ngrid[x+center[0]][y+center[1]][iside] = thisval;
                        continue;
                    }
                    if(x == 0 && y >= 0 && iside == 0) {
                        xo = 3*(y-1)/2+2;
                        yo = (y-1)/2;
                        otherside = 2;
                    }
                    else if(y >= 0 && x == 3*y+1 && iside == 1) {
                        xo = 0;
                        yo = 2*y;
                        otherside = 1;
                    }
                    cd otherval = grid[xo+center[0]][yo+center[1]][otherside];
                    Vector2cd vect;
                    vect << thisval, otherval;
                    Vector2cd newvect = coin*vect;
                    ngrid[x+center[0]][y+center[1]][iside] = newvect(0);
                    ngrid[xo+center[0]][yo+center[1]][otherside] = newvect(1);
                }
            }
        }
    }
    else {
        std::thread threads[NUM_THREADS];
        int delta_x = ntriangles_x/NUM_THREADS;
        for(int iThread = 0; iThread < NUM_THREADS-1; iThread++) {
            threads[iThread] = std::thread(applyCoinsPartial, std::ref(ngrid), std::ref(grid), std::ref(coin), xmin+iThread*delta_x, xmin+(iThread+1)*delta_x);
        }
        threads[NUM_THREADS-1] = std::thread(applyCoinsPartial, std::ref(ngrid), std::ref(grid), std::ref(coin), xmin+(NUM_THREADS-1)*delta_x, xmax+1);
        for(int iThread = 0; iThread < NUM_THREADS; iThread++)
            threads[iThread].join();
    }
    // We may miss some sides of 1 (tip up) triangles which are not adjacent to a valid 0 triangle
    for(int x = xmin; x <= xmax; x++)
        for(int y = ymin; y <= ymax; y++)
            for(int side = 0; side < 3; side++) {
                cd &newval = ngrid[x+center[0]][y+center[1]][side];
                if(std::isnan(std::real(newval)))
                    newval = grid[x+center[0]][y+center[1]][side];
            }
    return ngrid;
}

void plot(int iGrid = -1) {
    PyObject *fig;
    // Code to write the plot as Python matplotlib instructions. Use this if there are too many visible points to plot, since matplotlib-cpp segfaults in that case
    /*std::ostringstream scriptpath;
    scriptpath << prefix << "/script_" << iGrid << ".py";
    std::ofstream pyscript(scriptpath.str());
    pyscript << "from matplotlib import pyplot as plt\n";
    pyscript << "plt.figure(figsize=(10,10))\n";
    pyscript << "plt.xlim(" << xmin << "," << xmax << ")\n";
    pyscript << "plt.ylim(" << ymin*dy << "," << ymax*dy << ")\n";*/
    if(plotCone) {
        fig = plt::figure_size(1000,1000);
        plt::xlim(xmin, xmax);
        plt::ylim(ymin*dy, ymax*dy);
        plt::set_aspect_equal();
        vector<double> xlist, ylist, colorlist;
        vector<double> listvals;
        for(int x = xmin; x <= xmax; x++) {
            for(int y = ymin; y <= ymax; y++) {
                for(int iside = 0; iside < 3; iside++) {
                    cd val = grid[x+center[0]][y+center[1]][iside];
                    double col = std::real(val*std::conj(val));
                    listvals.push_back(col);
                }
            }
        }
        std::sort(listvals.rbegin(), listvals.rend());
        double maxi = (listvals[0]+listvals[1])/2;
        double threshold = maxi/10;
        for(int x = xmin; x <= xmax; x++) {
            for(int y = ymin; y <= ymax; y++) {
                for(int iside = 0; iside < 3; iside++) {
                    cd val = grid[x+center[0]][y+center[1]][iside];
                    double col = std::real(val*std::conj(val));
                    if(col > threshold) {
                        double rx, ry;
                        std::tie(rx, ry) = cone_coords(iside, x, y, true);
                        xlist.push_back(rx);
                        ylist.push_back(ry);
                        colorlist.push_back(col);
                    }
                }
            }
        }
        if(maxi == 0.0)
            maxi = 1.0;
        maxi *= 0.6;
        // Continued
        /*
        pyscript << "plt.scatter([";
        for(double d : xlist)
            pyscript << d << ",";
        pyscript << "],[";
        for(double d : ylist)
            pyscript << d << ",";
        pyscript << "], c=[";
        for(double c : colorlist)
            pyscript << c << ",";
        pyscript << "], cmap=\"gist_heat_r\", vmin=0, vmax=" << maxi << ")\n";*/
        plt::scatter_colored(xlist, ylist, colorlist, 1, {{"cmap","gist_heat_r"}, {"vmin", "0"}, {"vmax", std::to_string(maxi)}});
    }
    else {
        fig = plt::figure_size(1000,1000);
        plt::set_aspect_equal();
        vector<vector<double>> imgrid(ymax-ymin+1, vector<double>(xmax-xmin+1, 0.0));
        vector<double> listvals;
        for(int y = ymin; y <= ymax; y++) {
            for(int x = xmin; x <= xmax; x++) {
                double sum = 0.0;
                for(int iside = 0; iside < 3; iside++) {
                    cd val = grid[x+center[0]][y+center[1]][iside];
                    double col = std::real(val*std::conj(val));
                    sum += col;
                }
                listvals.push_back(sum);
                imgrid[y+center[1]][x+center[0]] = sum;
            }
        }
        std::sort(listvals.rbegin(), listvals.rend());
        double maxi = (listvals[0]+listvals[1])/2;
        if(maxi == 0.0)
            maxi = 1.0;
        maxi *= .6;

        double minx = (xmin-.5);
        double maxx = (xmax+.5);
        double miny = dy*(ymin-.5);
        double maxy = dy*(ymax+.5);
        plt::imshow(imgrid, {minx, maxx, miny, maxy}, {{"origin", "lower"}, {"cmap", "gist_heat_r"}, {"vmin", "0.0"}, {"vmax", std::to_string(maxi)}});
        plt::plot({0.0, 0.0}, {0.0, maxy}, {{"color","red"}});
        plt::plot({0.0, maxx}, {0.0, maxx*std::tan(M_PI/6)}, {{"color","red"}});
    }
    // Continued
    /*pyscript << "plt.savefig(\"" << prefix << "_" << iGrid << ".png\")\n";
    pyscript.flush();
    pyscript.close();*/
    std::ostringstream filename;
    filename << prefix << "_" << iGrid << ".png";
    plt::save(prefix + "/" + filename.str());
    plt::clf();
    plt::close(fig);
    Py_DECREF(fig);

    // Plot around dislocation line
    vector<double> xlist2, ylist2;
    for(int y = 0; y <= ymax; y++) {
        vector<int> klist;
        if(y%2)
            klist = {2, 0, 1};
        else
            klist = {0, 1, 2};
        for(int k : klist) {
            double rx, ry;
            std::tie(rx, ry) = real_coords(k, 0, y);
            xlist2.push_back(ry);
            cd val = grid[center[0]][center[1]+y][k];
            double col = std::real(val*std::conj(val));
            ylist2.push_back(col);
        }
    }
    plt::plot(xlist2, ylist2);
    std::ostringstream filename_disloc;
    filename_disloc << prefix << "_dislocation_" << iGrid << ".png";
    plt::save(prefix + "/" + filename_disloc.str());
    plt::clf();
    plt::close();
}

vector<double> dislocAmplitudes;

void step_walk(int step = -1) {
    std::cerr << "Begin step " << step << std::endl;
    grid = applyCoins(grid, U);
    for(int j = 0; j < 3; j++) {
        grid = applyCoins(grid, U*C0*U.adjoint());
        grid = shift(grid);
    }
    grid = applyCoins(grid, U.adjoint());
    double sumDisloc = 0;
    for(int y = 0; y <= ymax; y++)
        for(int k = 0; k < 3; k++)
            sumDisloc += sqmodulus(grid[center[0]][y+center[1]][k]);
    for(int y = 0; y <= ymax && 3*y+1 <= xmax; y++)
        for(int k = 0; k < 3; k++)
            sumDisloc += sqmodulus(grid[center[0]+3*y+1][center[1]+y][k]);
    dislocAmplitudes.push_back(sumDisloc);
    std::cerr << "Total amplitude: " << sumAmplitudes() << "\n";
    std::cerr << "End step " << step << std::endl;
}

void plotDislocAmplitude() {
    std::cerr << "Plotting dislocation amplitude" << std::endl;
    plt::plot(dislocAmplitudes);
    plt::save(prefix + "/" + prefix + "_disloc.png");
    plt::clf();
    plt::close();
}

void print_params() {
    std::ofstream ostream(prefix + "/settings.txt");
    ostream << "num_steps = " << num_steps << "\n";
    ostream << "initial state: " << initialStateName[initialState] << "\n";
    if(plotCone)
        ostream << "Cone coordinates\n";
    else
        ostream << "Unfolded cone\n";
    ostream << "xmin = " << xmin << "\n";
    ostream << "xmax = " << xmax << "\n";
    ostream << "ymin = " << ymin << "\n";
    ostream << "ymax = " << ymax << "\n";
    ostream << "U=\n" << U << "\n";
    ostream << "C0=\n" << C0 << std::endl;
    ostream.close();
}

void listInitialStates() {
    for(size_t i = 0; i < initialStateName.size(); i++)
        std::cerr << "- " << i << ": " << initialStateName[i] << "\n";
}

int main(int argc, char **argv)
{
    if(argc <= 2) {
        std::cerr << "Usage: " << std::string(argv[0]) << " <initial state> <plot cone>\n";
        std::cerr << "Initial states:\n";
        listInitialStates();
        std::cerr << "<plot cone> should be 1 if the figure should be plotted in cone coordinates, 0 otherwise.\n";
        return 1;
    }
    initialState = std::atoi(argv[1]);
    if(initialState < 0 || initialState >= (int)initialStateName.size()) {
        std::cerr << "Invalid initial state " << initialState << ". List of possible states:\n";
        listInitialStates();
        return 2;
    }
    plotCone = (bool) std::atoi(argv[2]);
    std::ostringstream str;
    str <<
        "simul_cone_newdisloc_" << initialStateName[initialState];
    if(plotCone)
        str << "_conecoord_";
    else
        str << "_altcoord_";
    str <<
        std::setw(4) << std::setfill('0') << ltm->tm_year+1900 << "-" <<
        std::setw(2) << ltm->tm_mon+1 << "-" <<
        std::setw(2) << ltm->tm_mday << "_" <<
        std::setw(2) << ltm->tm_hour << "-" <<
        std::setw(2) << ltm->tm_min << "-" <<
        std::setw(2) << ltm->tm_sec;
    prefix = str.str();
    std::filesystem::create_directory(prefix);
    init_HQ();
    print_params();
    // Initial state
    // A square
    if(initialState == 0) {
        for(int x = xmin/5; x <= xmax/5; x++)
            for(int y = ymin/5; y <= ymax/5; y++)
                for(int k = 0; k < 3; k++)
                    if(x <= 0 || y <= (x-1)/3)
                        grid[center[0]+x][center[1]+y][k] = 1;
        normalizeGrid();
    }
    // Shifted
    else if(initialState == 1) {
        for(int k = 0; k < 3; k++)
            grid[center[0]][center[1]+ymax/2-1][k]=1/sqrt(3);
    }
    // Shifted (in another way)
    else if(initialState == 2) {
        for(int k = 0; k < 3; k++)
            grid[center[0]+xmin/4+1][center[1]+ymin/4+1][k]=1/sqrt(3);
    }
    // Centered
    else if(initialState == 3) {
        vector<vector<int> > centercoords = {{-1,-1},{-1,0},{0,-1},{0,0},{1,-1},{1,0}};
        for(vector<int> coord : centercoords)
            for(int k = 0; k < 3; k++)
                grid[center[0]+coord[0]][center[1]+coord[1]][k]=1/sqrt(3*centercoords.size());
    }
    // Shifted, both sides of the line
    else if(initialState == 4) {
        for(int k=0; k<3; k++) {
            grid[center[0]+10][center[1]+3][k] = 1/sqrt(6);
            grid[center[0]][center[1]+6][k] = 1/sqrt(6);
        }
    }
    // Everywhere, uniformly
    else if(initialState == 5) {
        for(int x = xmin; x <= xmax; x++)
            for(int y = ymin; y <= ymax; y++)
                if(x<=0 || y <= (x-1)/3)
                    for(int k = 0; k < 3; k++)
                        grid[center[0]+x][center[1]+y][k] = 1;
        normalizeGrid();
    }
    plot(0);
    for(int i = 0; i < num_steps; i++) {
        step_walk(i);
        if((i+1)%10 == 0)
            plot(i+1);
    }
    plotDislocAmplitude();

}


#include <boost/config.hpp>
#include <boost/version.hpp>

#if BOOST_VERSION >= 105600 && (! defined(BOOST_GCC) || BOOST_GCC >= 40500)
#include <iostream>
#include <fstream>
#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>
#include <CGAL/Polygon_2.h>
#include <CGAL/Polygon_with_holes_2.h>
#include <CGAL/Constrained_Delaunay_triangulation_2.h>
#include <CGAL/Constrained_triangulation_plus_2.h>
#include <CGAL/Polyline_simplification_2/simplify.h>
#include <CGAL/Polyline_simplification_2/Squared_distance_cost.h>
#include <CGAL/IO/WKT.h>

namespace PS = CGAL::Polyline_simplification_2;

typedef CGAL::Exact_predicates_inexact_constructions_kernel K;
typedef CGAL::Polygon_2<K>                                  Polygon_2;
typedef CGAL::Polygon_with_holes_2<K>                       Polygon_with_holes_2;

typedef PS::Vertex_base_2<K> Vb;
typedef CGAL::Constrained_triangulation_face_base_2<K> Fb;
typedef CGAL::Triangulation_data_structure_2<Vb, Fb> TDS;
typedef CGAL::Constrained_Delaunay_triangulation_2<K, TDS, CGAL::Exact_predicates_tag> CDT;
typedef CGAL::Constrained_triangulation_plus_2<CDT>     CT;
typedef CT::Point                           Point;
typedef CT::Constraint_iterator             Constraint_iterator;
typedef CT::Vertices_in_constraint_iterator Vertices_in_constraint_iterator;
typedef CT::Points_in_constraint_iterator   Points_in_constraint_iterator;
typedef PS::Stop_below_count_ratio_threshold Stop;
typedef PS::Squared_distance_cost Cost;
int main(int argc, char* argv[])
{
  std::ifstream ifs( (argc==1)?"data/polygon.wkt":argv[1]);
  CT ct;
  Polygon_with_holes_2 P;
  while(CGAL::read_polygon_WKT(ifs, P)){
    const Polygon_2& poly = P.outer_boundary();
    ct.insert_constraint(poly);
    for(Polygon_with_holes_2::Hole_const_iterator it = P.holes_begin(); it != P.holes_end(); ++it){
      const Polygon_2& hole = *it;
      ct.insert_constraint(hole);
    }
  }
  PS::simplify(ct, Cost(), Stop(0.5));

  for(Constraint_iterator cit = ct.constraints_begin();
      cit != ct.constraints_end();
      ++cit) {
    std::cout << "simplified polyline" << std::endl;
    for(Points_in_constraint_iterator vit =
          ct.points_in_constraint_begin(*cit);
        vit != ct.points_in_constraint_end(*cit);
        ++vit)
      std::cout << *vit << std::endl;
  }
  return 0;
}

#else

int main()
{
  return 0;
}
#endif


//
// test/ibstream.hpp
//
// Copyright (c) Chris Glover, 2017-2018
//
// 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)
//

#define BOOST_TEST_MODULE TestBitstreams
#include <boost/test/unit_test.hpp>

#include "common.hpp"
#include <boost/array.hpp>
#include <boost/radix/detail/bits.hpp>
#include <boost/radix/static_ibitstream_lsb.hpp>
#include <boost/radix/static_ibitstream_msb.hpp>
#include <boost/radix/static_obitstream_lsb.hpp>
#include <boost/radix/static_obitstream_msb.hpp>
#include <vector>

template <std::size_t Bits>
static void check_static_istream_lsb(std::size_t num_segments) {
  std::size_t const packed_segment_size =
      boost::radix::bits::to_packed_segment_size<Bits>::value;
  std::size_t const unpacked_segment_size =
      boost::radix::bits::to_unpacked_segment_size<Bits>::value;

  std::size_t max_value = ~(std::size_t(~0) << Bits);

  // Checking every bit combination in every position would be possible, though
  // it would take a long time, so instead we check to make sure we can extract
  // every bit combination from every posision, with all zeros in one pass and
  // all ones in a second pass.
  std::vector<bits_type> packed(packed_segment_size);
  std::vector<bits_type> unpacked(unpacked_segment_size);

  for(std::size_t i = 0; i < max_value; ++i) {
    std::size_t offset = 0;
    bits_type value    = bits_type(i);
    for(std::size_t j = 0; j < unpacked_segment_size; ++j) {
      std::fill(packed.begin(), packed.end(), 0);
      set_bits_lsb(packed, value, offset, Bits);
      boost::radix::static_ibitstream_lsb<Bits>()(packed.begin(), unpacked);
      BOOST_TEST(unpacked[j] == value);

      std::fill(packed.begin(), packed.end(), bits_type(~0));
      set_bits_lsb(packed, value, offset, Bits);
      boost::radix::static_ibitstream_lsb<Bits>()(packed.begin(), unpacked);
      BOOST_TEST(unpacked[j] == value);

      offset += Bits;
    }
  }
}

template <std::size_t Bits>
static void check_static_istream_msb(std::size_t num_segments) {
  std::size_t const packed_segment_size =
      boost::radix::bits::to_packed_segment_size<Bits>::value;
  std::size_t const unpacked_segment_size =
      boost::radix::bits::to_unpacked_segment_size<Bits>::value;

  std::size_t max_value = ~(std::size_t(~0) << Bits);

  // Checking every bit combination in every position would be possible, though
  // it would take a long time, so instead we check to make sure we can extract
  // every bit combination from every posision, with all zeros in one pass and
  // all ones in a second pass.
  std::vector<bits_type> packed(packed_segment_size);
  std::vector<bits_type> unpacked(unpacked_segment_size);

  for(std::size_t i = 0; i < max_value; ++i) {
    std::size_t offset = 0;
    bits_type value    = bits_type(i);
    for(std::size_t j = 0; j < unpacked_segment_size; ++j) {
      std::fill(packed.begin(), packed.end(), 0);
      set_bits_msb(packed, value, offset, Bits);
      boost::radix::static_ibitstream_msb<Bits>()(packed.begin(), unpacked);
      BOOST_TEST(unpacked[j] == value);

      std::fill(packed.begin(), packed.end(), bits_type(~0));
      set_bits_msb(packed, value, offset, Bits);
      boost::radix::static_ibitstream_msb<Bits>()(packed.begin(), unpacked);
      BOOST_TEST(unpacked[j] == value);

      offset += Bits;
    }
  }
}

template <std::size_t Bits>
static void check_static_ostream_lsb(std::size_t num_segments) {
  std::size_t const packed_segment_size =
      boost::radix::bits::to_packed_segment_size<Bits>::value;
  std::size_t const unpacked_segment_size =
      boost::radix::bits::to_unpacked_segment_size<Bits>::value;

  std::size_t max_value = ~(std::size_t(~0) << Bits);

  // Checking every bit combination in every position would be possible, though
  // it would take a long time, so instead we check to make sure we can extract
  // every bit combination from every posision, with all zeros in one pass and
  // all ones in a second pass.
  std::vector<bits_type> packed(packed_segment_size);
  std::vector<bits_type> unpacked(unpacked_segment_size);

  for(std::size_t i = 0; i < max_value; ++i) {
    std::size_t offset = 0;
    bits_type value    = bits_type(i);
    for(std::size_t j = 0; j < unpacked_segment_size; ++j) {
      std::fill(unpacked.begin(), unpacked.end(), 0);
      unpacked[j] = value;
      boost::radix::static_obitstream_lsb<Bits>()(unpacked, packed.data());
      BOOST_TEST(get_bits_lsb(packed, offset, Bits) == value);

      std::fill(unpacked.begin(), unpacked.end(), bits_type(max_value));
      unpacked[j] = value;
      boost::radix::static_obitstream_lsb<Bits>()(unpacked, packed.data());
      BOOST_TEST(get_bits_lsb(packed, offset, Bits) == value);

      offset += Bits;
    }
  }
}

template <std::size_t Bits>
static void check_static_ostream_msb(std::size_t num_segments) {
  std::size_t const packed_segment_size =
      boost::radix::bits::to_packed_segment_size<Bits>::value;
  std::size_t const unpacked_segment_size =
      boost::radix::bits::to_unpacked_segment_size<Bits>::value;

  std::size_t max_value = ~(std::size_t(~0) << Bits);

  // Checking every bit combination in every position would be possible, though
  // it would take a long time, so instead we check to make sure we can extract
  // every bit combination from every posision, with all zeros in one pass and
  // all ones in a second pass.
  std::vector<bits_type> packed(packed_segment_size);
  std::vector<bits_type> unpacked(unpacked_segment_size);

  for(std::size_t i = 0; i < max_value; ++i) {
    std::size_t offset = 0;
    bits_type value    = bits_type(i);
    for(std::size_t j = 0; j < unpacked_segment_size; ++j) {
      std::fill(unpacked.begin(), unpacked.end(), 0);
      unpacked[j] = value;
      boost::radix::static_obitstream_msb<Bits>()(unpacked, packed.data());
      BOOST_TEST(get_bits_msb(packed, offset, Bits) == value);

      std::fill(unpacked.begin(), unpacked.end(), max_value);
      unpacked[j] = value;
      boost::radix::static_obitstream_msb<Bits>()(unpacked, packed.data());
      BOOST_TEST(get_bits_msb(packed, offset, Bits) == value);

      offset += Bits;
    }
  }
}

template <std::size_t Bits>
static void check_static_iostream_lsb(std::size_t num_segments) {
  std::size_t const packed_segment_size =
      boost::radix::bits::to_packed_segment_size<Bits>::value;
  std::size_t const unpacked_segment_size =
      boost::radix::bits::to_unpacked_segment_size<Bits>::value;

  std::size_t max_value = ~(std::size_t(~0) << Bits);

  // Checking every bit combination in every position would be possible, though
  // it would take a long time, so instead we check to make sure we can extract
  // every bit combination from every posision, with all zeros in one pass and
  // all ones in a second pass.
  std::vector<bits_type> packed(packed_segment_size);
  std::vector<bits_type> unpacked_source(unpacked_segment_size);
  std::vector<bits_type> unpacked_result(unpacked_segment_size);

  for(std::size_t i = 0; i < max_value; ++i) {
    std::size_t offset = 0;
    bits_type value    = bits_type(i);
    for(std::size_t j = 0; j < unpacked_segment_size; ++j) {
      std::fill(unpacked_source.begin(), unpacked_source.end(), 0);
      unpacked_source[j] = value;
      boost::radix::static_obitstream_lsb<Bits>()(unpacked_source, packed.data());
      boost::radix::static_ibitstream_lsb<Bits>()(
          packed.begin(), unpacked_result);
      BOOST_TEST(unpacked_source == unpacked_result);

      std::fill(unpacked_source.begin(), unpacked_source.end(), max_value);
      unpacked_source[j] = value;
      boost::radix::static_obitstream_lsb<Bits>()(unpacked_source, packed.data());
      boost::radix::static_ibitstream_lsb<Bits>()(
          packed.begin(), unpacked_result);
      BOOST_TEST(unpacked_source == unpacked_result);

      offset += Bits;
    }
  }
}

template <std::size_t Bits>
static void check_static_iostream_msb(std::size_t num_segments) {
  std::size_t const packed_segment_size =
      boost::radix::bits::to_packed_segment_size<Bits>::value;
  std::size_t const unpacked_segment_size =
      boost::radix::bits::to_unpacked_segment_size<Bits>::value;

  std::size_t max_value = ~(std::size_t(~0) << Bits);

  // Checking every bit combination in every position would be possible, though
  // it would take a long time, so instead we check to make sure we can extract
  // every bit combination from every posision, with all zeros in one pass and
  // all ones in a second pass.
  std::vector<bits_type> packed(packed_segment_size);
  std::vector<bits_type> unpacked_source(unpacked_segment_size);
  std::vector<bits_type> unpacked_result(unpacked_segment_size);

  for(std::size_t i = 0; i < max_value; ++i) {
    std::size_t offset = 0;
    bits_type value    = bits_type(i);
    for(std::size_t j = 0; j < unpacked_segment_size; ++j) {
      std::fill(unpacked_source.begin(), unpacked_source.end(), 0);
      unpacked_source[j] = value;
      boost::radix::static_obitstream_msb<Bits>()(unpacked_source, packed.data());
      boost::radix::static_ibitstream_msb<Bits>()(
          packed.begin(), unpacked_result);
      BOOST_TEST(unpacked_source == unpacked_result);

      std::fill(unpacked_source.begin(), unpacked_source.end(), max_value);
      unpacked_source[j] = value;
      boost::radix::static_obitstream_msb<Bits>()(unpacked_source, packed.data());
      boost::radix::static_ibitstream_msb<Bits>()(
          packed.begin(), unpacked_result);
      BOOST_TEST(unpacked_source == unpacked_result);

      offset += Bits;
    }
  }
}

BOOST_AUTO_TEST_CASE(static_ibitstream_lsb) {
  // Testing 1 segment lets us catch buffer overflows.
  // Testing 10 gives us more data and tests the segment boundaries.
  boost::array<std::size_t, 2> segments = {1, 10};
  for(int i = 0; i < segments.size(); ++i) {
    check_static_istream_lsb<1>(segments[i]);
    check_static_istream_lsb<2>(segments[i]);
    check_static_istream_lsb<3>(segments[i]);
    check_static_istream_lsb<4>(segments[i]);
    check_static_istream_lsb<5>(segments[i]);
    check_static_istream_lsb<6>(segments[i]);
    check_static_istream_lsb<7>(segments[i]);
  }
}

BOOST_AUTO_TEST_CASE(static_ibitstream_msb) {
  // Testing 1 segment lets us catch buffer overflows.
  // Testing 10 gives us more data and tests the segment boundaries.
  boost::array<std::size_t, 2> segments = {1, 10};
  for(int i = 0; i < segments.size(); ++i) {
    check_static_istream_msb<1>(segments[i]);
    check_static_istream_msb<2>(segments[i]);
    check_static_istream_msb<3>(segments[i]);
    check_static_istream_msb<4>(segments[i]);
    check_static_istream_msb<5>(segments[i]);
    check_static_istream_msb<6>(segments[i]);
    check_static_istream_msb<7>(segments[i]);
  }
}

BOOST_AUTO_TEST_CASE(static_obitstream_lsb) {
  // Testing 1 segment lets us catch buffer overflows.
  // Testing 10 gives us more data and tests the segment boundaries.
  boost::array<std::size_t, 2> segments = {1, 10};
  for(int i = 0; i < segments.size(); ++i) {
    check_static_ostream_lsb<1>(segments[i]);
    check_static_ostream_lsb<2>(segments[i]);
    check_static_ostream_lsb<3>(segments[i]);
    check_static_ostream_lsb<4>(segments[i]);
    check_static_ostream_lsb<5>(segments[i]);
    check_static_ostream_lsb<6>(segments[i]);
    check_static_ostream_lsb<7>(segments[i]);
  }
}

BOOST_AUTO_TEST_CASE(static_obitstream_msb) {
  // Testing 1 segment lets us catch buffer overflows.
  // Testing 10 gives us more data and tests the segment boundaries.
  boost::array<std::size_t, 2> segments = {1, 10};
  for(int i = 0; i < segments.size(); ++i) {
    check_static_ostream_msb<1>(segments[i]);
    check_static_ostream_msb<2>(segments[i]);
    check_static_ostream_msb<3>(segments[i]);
    check_static_ostream_msb<4>(segments[i]);
    check_static_ostream_msb<5>(segments[i]);
    check_static_ostream_msb<6>(segments[i]);
    check_static_ostream_msb<7>(segments[i]);
  }
}

BOOST_AUTO_TEST_CASE(static_iobitstream_lsb) {
  // Testing 1 segment lets us catch buffer overflows.
  // Testing 10 gives us more data and tests the segment boundaries.
  boost::array<std::size_t, 2> segments = {1, 10};
  for(int i = 0; i < segments.size(); ++i) {
    check_static_iostream_lsb<1>(segments[i]);
    check_static_iostream_lsb<2>(segments[i]);
    check_static_iostream_lsb<3>(segments[i]);
    check_static_iostream_lsb<4>(segments[i]);
    check_static_iostream_lsb<5>(segments[i]);
    check_static_iostream_lsb<6>(segments[i]);
    check_static_iostream_lsb<7>(segments[i]);
  }
}

BOOST_AUTO_TEST_CASE(static_iobitstream_msb) {
  // Testing 1 segment lets us catch buffer overflows.
  // Testing 10 gives us more data and tests the segment boundaries.
  boost::array<std::size_t, 2> segments = {1, 10};
  for(int i = 0; i < segments.size(); ++i) {
    check_static_iostream_msb<1>(segments[i]);
    check_static_iostream_msb<2>(segments[i]);
    check_static_iostream_msb<3>(segments[i]);
    check_static_iostream_msb<4>(segments[i]);
    check_static_iostream_msb<5>(segments[i]);
    check_static_iostream_msb<6>(segments[i]);
    check_static_iostream_msb<7>(segments[i]);
  }
}


//==================================================================================================
/**
  EVE - Expressive Vector Engine
  Copyright : EVE Contributors & Maintainers
  SPDX-License-Identifier: MIT
**/
//==================================================================================================
#include "test.hpp"
#include <eve/function/cyl_bessel_j1.hpp>
#include <eve/function/diff/cyl_bessel_j1.hpp>
#include <eve/function/prev.hpp>
#include <eve/constant/inf.hpp>
#include <eve/constant/minf.hpp>
#include <eve/constant/nan.hpp>
#include <eve/platform.hpp>
#include <cmath>
#include <boost/math/special_functions/bessel.hpp>
#include <boost/math/special_functions/bessel_prime.hpp>

EVE_TEST_TYPES( "Check return types of cyl_bessel_j1"
            , eve::test::simd::ieee_reals
            )
<typename T>(eve::as<T>)
{
  using v_t = eve::element_type_t<T>;
  TTS_EXPR_IS(eve::cyl_bessel_j1(T(0)), T);
  TTS_EXPR_IS(eve::cyl_bessel_j1(v_t(0)), v_t);
};

 EVE_TEST( "Check behavior of cyl_bessel_j1 on wide"
        , eve::test::simd::ieee_reals
        , eve::test::generate(eve::test::randoms(0.0, 5.5),
                              eve::test::randoms(5.5, 9.5),
                              eve::test::randoms(9.5, 60.0))
         )
   <typename T>(T const& a0, T const& a1, T const& a2)
{
  using v_t = eve::element_type_t<T>;

  auto eve__cyl_bessel_j1 =  [](auto x) { return eve::cyl_bessel_j1(x); };
  auto std__cyl_bessel_j1 =  [](auto x)->v_t { return boost::math::cyl_bessel_j(v_t(1), x); };
  if constexpr( eve::platform::supports_invalids )
  {
    TTS_ULP_EQUAL(eve__cyl_bessel_j1(eve::minf(eve::as<v_t>())), eve::zero(eve::as<v_t>()), 0);
    TTS_ULP_EQUAL(eve__cyl_bessel_j1(eve::inf(eve::as<v_t>())), v_t(0), 0);
    TTS_ULP_EQUAL(eve__cyl_bessel_j1(eve::nan(eve::as<v_t>())), eve::nan(eve::as<v_t>()), 0);
    TTS_ULP_EQUAL(eve__cyl_bessel_j1(eve::minf(eve::as< T>())), eve::zero(eve::as< T>()), 0);
    TTS_ULP_EQUAL(eve__cyl_bessel_j1(eve::inf(eve::as< T>())),  T(0), 0);
    TTS_ULP_EQUAL(eve__cyl_bessel_j1(eve::nan(eve::as< T>())), eve::nan(eve::as< T>()), 0);
  }
  TTS_ULP_EQUAL(eve__cyl_bessel_j1(v_t(1000)) , std__cyl_bessel_j1( v_t(1000)) , 2.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_j1(v_t(10)), std__cyl_bessel_j1(v_t(10))  , 2.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_j1(v_t(5)),  std__cyl_bessel_j1(v_t(5))   , 2.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_j1(v_t(2)),  std__cyl_bessel_j1(v_t(2))   , 2.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_j1(v_t(1.5)),std__cyl_bessel_j1(v_t(1.5)) , 2.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_j1(v_t(0.5)),std__cyl_bessel_j1(v_t(0.5)) , 2.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_j1(v_t(1)),  std__cyl_bessel_j1(v_t(1))   , 2.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_j1(v_t(0)),  std__cyl_bessel_j1(v_t(0))   , 2.0);

  TTS_ULP_EQUAL(eve__cyl_bessel_j1( T(1000)) ,  T(std__cyl_bessel_j1( v_t(1000)) ), 2.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_j1( T(10)) ,  T(std__cyl_bessel_j1( v_t(10)) ), 2.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_j1( T(5))  ,  T(std__cyl_bessel_j1( v_t(5))  ), 2.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_j1( T(2))  ,  T(std__cyl_bessel_j1( v_t(2))  ), 2.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_j1( T(1.5)),  T(std__cyl_bessel_j1( v_t(1.5))), 2.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_j1( T(0.5)),  T(std__cyl_bessel_j1( v_t(0.5))), 2.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_j1( T(1))  ,  T(std__cyl_bessel_j1( v_t(1))  ), 2.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_j1( T(0))  ,  T(std__cyl_bessel_j1( v_t(0))  ), 2.0);

  TTS_ULP_EQUAL(eve__cyl_bessel_j1(a0), map(std__cyl_bessel_j1, a0), 25.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_j1(a1), map(std__cyl_bessel_j1, a1), 300.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_j1(a2), map(std__cyl_bessel_j1, a2), 300.0);

  TTS_ULP_EQUAL(eve__cyl_bessel_j1(v_t(-1000)) , std__cyl_bessel_j1( v_t(-1000)) , 2.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_j1(v_t(-10)), std__cyl_bessel_j1(v_t(-10))  , 2.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_j1(v_t(-5)),  std__cyl_bessel_j1(v_t(-5))   , 2.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_j1(v_t(-2)),  std__cyl_bessel_j1(v_t(-2))   , 2.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_j1(v_t(-1.5)),std__cyl_bessel_j1(v_t(-1.5)) , 2.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_j1(v_t(-0.5)),std__cyl_bessel_j1(v_t(-0.5)) , 2.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_j1(v_t(-1)),  std__cyl_bessel_j1(v_t(-1))   , 2.0);

  TTS_ULP_EQUAL(eve__cyl_bessel_j1( T(-1000)) ,  T(std__cyl_bessel_j1( v_t(-1000)) ), 2.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_j1( T(-10)) ,  T(std__cyl_bessel_j1( v_t(-10)) ), 2.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_j1( T(-5))  ,  T(std__cyl_bessel_j1( v_t(-5))  ), 2.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_j1( T(-2))  ,  T(std__cyl_bessel_j1( v_t(-2))  ), 2.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_j1( T(-1.5)),  T(std__cyl_bessel_j1( v_t(-1.5))), 2.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_j1( T(-0.5)),  T(std__cyl_bessel_j1( v_t(-0.5))), 2.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_j1( T(-1))  ,  T(std__cyl_bessel_j1( v_t(-1))  ), 2.0);

  TTS_ULP_EQUAL(eve__cyl_bessel_j1(-a0), map(std__cyl_bessel_j1, -a0), 25.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_j1(-a1), map(std__cyl_bessel_j1, -a1), 300.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_j1(-a2), map(std__cyl_bessel_j1, -a2), 300.0);

};

EVE_TEST( "Check behavior of diff(cyl_bessel_j1) on wide"
        , eve::test::simd::ieee_reals
        , eve::test::generate(eve::test::randoms(-60.0, 60.0))
        )
  <typename T>(T a0 )
{
  using v_t =  eve::element_type_t<T>;
  auto eve__diff_bessel_j1 =  [](auto x) { return eve::diff(eve::cyl_bessel_j1)(x); };
  auto std__diff_bessel_j1 =  [](auto x)->v_t { return boost::math::cyl_bessel_j_prime(v_t(1), x); };
  TTS_RELATIVE_EQUAL(eve__diff_bessel_j1(a0),   map(std__diff_bessel_j1, a0), 1.0e-3);
};