#include "test_utils.hpp"
#include "lib/general_matching.hpp"
#include "lib/graph_formats.hpp"
#include "matching/micali_vazirani.hpp"
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/max_cardinality_matching.hpp>

using bgraph = boost::adjacency_list<boost::vecS, boost::vecS, boost::undirectedS>;
using matemap_t = std::vector<boost::graph_traits<bgraph>::vertex_descriptor>;

bgraph to_boost(int V, const edges_t& g) {
    bgraph bg(V);
    for (auto [u, v] : g)
        add_edge(u, v, bg);
    return bg;
}

int boost_matching_size(const bgraph& bg) {
    matemap_t mate(num_vertices(bg));
    boost::greedy_matching<bgraph, size_t*>::find_matching(bg, &mate[0]);
    boost::edmonds_maximum_cardinality_matching(bg, &mate[0]);
    int cnt = 0;
    for (auto& mapped : mate)
        cnt += mapped != bgraph::null_vertex();
    return cnt / 2;
}

void stress_test_general_matching() {
    LOOP_FOR_DURATION_OR_RUNS_TRACKED (20s, now, 50'000, runs) {
        print_time(now, 20s, "stress general matching ({} runs)", runs);

        int V = rand_unif<int>(30, 100);
        int M = min(V / 2, rand_unif<int>(0.1 * V, 0.5 * V));
        double p = rand_unif<double>(3, 15) / V;

        auto g = random_general_matching(V, M, p);
        general_matching_hide_topology(V, g);

        bgraph bg = to_boost(V, g);
        int boo = boost_matching_size(bg);

        micali_vazirani vg(V, g);
        int ans0 = vg.max_matching();

        assert(M == boo && M == ans0 && M >= 1);
    }
}

void speed_test_general_matching() {
    vector<int> Vs = {6000, 15000, 30000, 50000, 100000, 200000, 300000, 500000, 800000};
    vector<double> pVs = {2.0, 5.0, 8.0, 12.0, 20.0};
    vector<double> as = {-.35, -.15, -.05, -.01, -.001, 0, +.001, +.01, +.05, +.15, +.35};

    vector<tuple<int, double, double>> inputs;
    for (int V : Vs) {
        for (double pV : pVs) {
            for (double a : as) {
                double p = pV / V, E = V * pV;
                if (p <= 1.0 && E <= 5'000'000) {
                    inputs.push_back({V, pV, a});
                }
            }
        }
    }

    const auto runtime = 360'000ms / inputs.size();
    map<tuple<pair<double, double>, int, stringable>, stringable> table;

    for (auto [V, pV, alpha] : inputs) {
        START_ACC(mv);
        double p = min(1.0, pV / V);
        int64_t Es = 0, Exp = pV * V;

        LOOP_FOR_DURATION_OR_RUNS_TRACKED (runtime, now, 1000, runs) {
            print_time(now, runtime,
                       "speed general matching V={} E={} alpha={:.3f} ({} runs)", V, Exp,
                       alpha, runs);

            edges_t g = random_geometric_undirected(V, 2 * p, alpha);
            random_relabel_graph_inplace(V, g);

            ADD_TIME_BLOCK(mv) {
                shuffle(begin(g), end(g), mt);
                micali_vazirani vg(V, g);
                vg.max_matching();
            }

            Es += g.size();
        }

        table[{{pV, alpha}, V, "mv"}] = FORMAT_EACH(mv, runs);
        table[{{pV, alpha}, V, "E"}] = format("{:.1f}", 1.0 * Es / runs);
    }

    print_time_table(table, "General matching");
}

int main() {
    RUN_BLOCK(stress_test_general_matching());
    RUN_BLOCK(speed_test_general_matching());
    return 0;
}


/*
 * main_optimize.cpp
 * Date: 2013-01-28
 * Author: Karsten Ahnert (karsten.ahnert@gmx.de)
 */

#define FUSION_MAX_VECTOR_SIZE 20

#include <gpcxx/tree/basic_tree.hpp>
#include <gpcxx/generate/uniform_symbol.hpp>
#include <gpcxx/generate/node_generator.hpp>
#include <gpcxx/generate/ramp.hpp>
#include <gpcxx/operator/mutation.hpp>
#include <gpcxx/operator/simple_mutation_strategy.hpp>
#include <gpcxx/operator/random_selector.hpp>
#include <gpcxx/operator/tournament_selector.hpp>
#include <gpcxx/operator/crossover.hpp>
#include <gpcxx/operator/one_point_crossover_strategy.hpp>
#include <gpcxx/operator/reproduce.hpp>
#include <gpcxx/eval/static_eval.hpp>
#include <gpcxx/eval/regression_fitness.hpp>
#include <gpcxx/evolve/static_pipeline.hpp>
#include <gpcxx/io/best_individuals.hpp>
#include <gpcxx/stat/population_statistics.hpp>
#include <gpcxx/app/timer.hpp>
#include <gpcxx/app/normalize.hpp>
#include <gpcxx/app/generate_evenly_spaced_test_data.hpp>

#include <boost/fusion/include/make_vector.hpp>

#include <iostream>
#include <fstream>
#include <random>
#include <vector>
#include <functional>

const std::string tab = "\t";

namespace fusion = boost::fusion;

typedef double value_type;
typedef gpcxx::regression_training_data< value_type , 3 > trainings_data_type;
typedef std::mt19937 rng_type ;
typedef char symbol_type;
typedef std::array< value_type , 3 > eval_context_type;
typedef std::vector< value_type > fitness_type;

namespace pl = std::placeholders;

int main( int argc , char *argv[] )
{
    rng_type rng;

    trainings_data_type c = gpcxx::generate_evenly_spaced_test_data< 3 >( -5.0 , 5.0 + 0.1 , 0.4 , []( double x1 , double x2 , double x3 ) {
                        return  1.0 / ( 1.0 + pow( x1 , -4.0 ) ) + 1.0 / ( 1.0 + pow( x2 , -4.0 ) ) + 1.0 / ( 1.0 + pow( x3 , -4.0 ) ); } );
    gpcxx::normalize( c.y );

    std::ofstream fout1( "testdata.dat" );
    for( size_t i=0 ; i<c.x[0].size() ; ++i )
        fout1 << c.y[i] << " " << c.x[0][i] << " " << c.x[1][i] << " " << c.x[2][i] << "\n";
    fout1.close();

    auto eval = gpcxx::make_static_eval< value_type , symbol_type , eval_context_type >(
        fusion::make_vector(
            fusion::make_vector( 'x' , []( eval_context_type const& t ) { return t[0]; } )
          , fusion::make_vector( 'y' , []( eval_context_type const& t ) { return t[1]; } )
          , fusion::make_vector( 'z' , []( eval_context_type const& t ) { return t[2]; } )
          ) ,
        fusion::make_vector(
            fusion::make_vector( 's' , []( double v ) -> double { return std::sin( v ); } )
          , fusion::make_vector( 'c' , []( double v ) -> double { return std::cos( v ); } )
          , fusion::make_vector( 'e' , []( double v ) -> double { return std::exp( v ); } )
          , fusion::make_vector( 'l' , []( double v ) -> double { return ( std::abs( v ) < 1.0e-20 ) ? log( 1.0e-20 ) : std::log( std::abs( v ) ); } )
          ) ,
        fusion::make_vector(
            fusion::make_vector( '+' , std::plus< double >() )
          , fusion::make_vector( '-' , std::minus< double >() )
          , fusion::make_vector( '*' , std::multiplies< double >() )
          , fusion::make_vector( '/' , std::divides< double >() )
          ) );
    typedef decltype( eval ) eval_type;
    typedef eval_type::node_attribute_type node_attribute_type;

    typedef gpcxx::basic_tree< node_attribute_type > tree_type;
    typedef std::vector< tree_type > population_type;
    typedef gpcxx::static_pipeline< population_type , fitness_type , rng_type > evolver_type;

    size_t population_size = 1000;
    size_t generation_size = 20;
    double number_elite = 1;
    double mutation_rate = 0.0;
    double crossover_rate = 0.6;
    double reproduction_rate = 0.3;
    size_t min_tree_height = 8 , max_tree_height = 8;
    size_t tournament_size = 15;

    // generators< rng_type > gen( rng );
    auto terminal_gen = eval.get_terminal_symbol_distribution();
    auto unary_gen = eval.get_unary_symbol_distribution();
    auto binary_gen = eval.get_binary_symbol_distribution();
    gpcxx::node_generator< node_attribute_type , rng_type , 3 > node_generator {
        { 2.0 * double( terminal_gen.num_symbols() ) , 0 , terminal_gen } ,
        { double( unary_gen.num_symbols() ) , 1 , unary_gen } ,
        { double( binary_gen.num_symbols() ) , 2 , binary_gen } };

    auto tree_generator = gpcxx::make_ramp( rng , node_generator , min_tree_height , max_tree_height , 0.5 );

    evolver_type evolver( number_elite , mutation_rate , crossover_rate , reproduction_rate , rng );
    std::vector< double > fitness( population_size , 0.0 );
    std::vector< tree_type > population( population_size );

    auto fitness_f = gpcxx::regression_fitness< eval_type >( eval );
    evolver.mutation_function() = gpcxx::make_mutation(
        gpcxx::make_simple_mutation_strategy( rng , node_generator ) ,
        gpcxx::make_tournament_selector( rng , tournament_size ) );
    evolver.crossover_function() = gpcxx::make_crossover(
        gpcxx::make_one_point_crossover_strategy( rng , max_tree_height ) ,
        gpcxx::make_tournament_selector( rng , tournament_size ) );
    evolver.reproduction_function() = gpcxx::make_reproduce( gpcxx::make_tournament_selector( rng , tournament_size ) );

    gpcxx::timer timer;

    // initialize population with random trees and evaluate fitness
    timer.restart();
    for( size_t i=0 ; i<population.size() ; ++i )
    {
        tree_generator( population[i] );
        fitness[i] = fitness_f( population[i] , c );
    }
    std::cout << gpcxx::indent( 0 ) << "Generation time " << timer.seconds() << std::endl;
    std::cout << gpcxx::indent( 1 ) << "Best individuals" << std::endl << gpcxx::best_individuals( population , fitness , 1 , 10 ) << std::endl;
    std::cout << gpcxx::indent( 1 ) << "Statistics : " << gpcxx::calc_population_statistics( population ) << std::endl;
    std::cout << gpcxx::indent( 1 ) << std::endl << std::endl;

    timer.restart();
    for( size_t generation=1 ; generation<=generation_size ; ++generation )
    {
        gpcxx::timer iteration_timer;
        iteration_timer.restart();
        evolver.next_generation( population , fitness );
        double evolve_time = iteration_timer.seconds();
        iteration_timer.restart();
        std::transform( population.begin() , population.end() , fitness.begin() , [&]( tree_type const &t ) { return fitness_f( t , c ); } );
        double eval_time = iteration_timer.seconds();

        std::cout << gpcxx::indent( 0 ) << "Generation " << generation << std::endl;
        std::cout << gpcxx::indent( 1 ) << "Evolve time " << evolve_time << std::endl;
        std::cout << gpcxx::indent( 1 ) << "Eval time " << eval_time << std::endl;
        std::cout << gpcxx::indent( 1 ) << "Best individuals" << std::endl << gpcxx::best_individuals( population , fitness , 2 , 10 ) << std::endl;
        std::cout << gpcxx::indent( 1 ) << "Statistics : " << gpcxx::calc_population_statistics( population ) << std::endl << std::endl;
    }
    std::cout << "Overall time : " << timer.seconds() << std::endl;

    return 0;
}


///////////////////////////////////////////////////////////////////////////////
// BSD 3-Clause License
//
// Copyright (C) 2018-2020, LAAS-CNRS, University of Edinburgh
// Copyright note valid unless otherwise stated in individual files.
// All rights reserved.
///////////////////////////////////////////////////////////////////////////////

#ifndef CROCODDYL_MULTIBODY_COST_BASE_HPP_
#define CROCODDYL_MULTIBODY_COST_BASE_HPP_

#include <boost/shared_ptr.hpp>
#include <boost/make_shared.hpp>
#include "crocoddyl/multibody/states/multibody.hpp"
#include "crocoddyl/core/data-collector-base.hpp"
#include "crocoddyl/core/activation-base.hpp"
#include "crocoddyl/core/utils/to-string.hpp"

namespace crocoddyl {

struct CostDataAbstract;  // forward declaration

class CostModelAbstract {
 public:
  CostModelAbstract(boost::shared_ptr<StateMultibody> state, boost::shared_ptr<ActivationModelAbstract> activation,
                    const std::size_t& nu, const bool& with_residuals = true);
  CostModelAbstract(boost::shared_ptr<StateMultibody> state, boost::shared_ptr<ActivationModelAbstract> activation,
                    const bool& with_residuals = true);
  CostModelAbstract(boost::shared_ptr<StateMultibody> state, const std::size_t& nr, const std::size_t& nu,
                    const bool& with_residuals = true);
  CostModelAbstract(boost::shared_ptr<StateMultibody> state, const std::size_t& nr, const bool& with_residuals = true);
  ~CostModelAbstract();

  virtual void calc(const boost::shared_ptr<CostDataAbstract>& data, const Eigen::Ref<const Eigen::VectorXd>& x,
                    const Eigen::Ref<const Eigen::VectorXd>& u) = 0;
  virtual void calcDiff(const boost::shared_ptr<CostDataAbstract>& data, const Eigen::Ref<const Eigen::VectorXd>& x,
                        const Eigen::Ref<const Eigen::VectorXd>& u, const bool& recalc = true) = 0;
  virtual boost::shared_ptr<CostDataAbstract> createData(DataCollectorAbstract* const data);

  void calc(const boost::shared_ptr<CostDataAbstract>& data, const Eigen::Ref<const Eigen::VectorXd>& x);
  void calcDiff(const boost::shared_ptr<CostDataAbstract>& data, const Eigen::Ref<const Eigen::VectorXd>& x);

  const boost::shared_ptr<StateMultibody>& get_state() const;
  const boost::shared_ptr<ActivationModelAbstract>& get_activation() const;
  const std::size_t& get_nu() const;

 protected:
  boost::shared_ptr<StateMultibody> state_;
  boost::shared_ptr<ActivationModelAbstract> activation_;
  std::size_t nu_;
  bool with_residuals_;
  Eigen::VectorXd unone_;

#ifdef PYTHON_BINDINGS

 public:
  void calc_wrap(const boost::shared_ptr<CostDataAbstract>& data, const Eigen::VectorXd& x,
                 const Eigen::VectorXd& u = Eigen::VectorXd()) {
    if (u.size() == 0) {
      calc(data, x);
    } else {
      calc(data, x, u);
    }
  }

  void calcDiff_wrap(const boost::shared_ptr<CostDataAbstract>& data, const Eigen::VectorXd& x,
                     const Eigen::VectorXd& u, const bool& recalc) {
    calcDiff(data, x, u, recalc);
  }
  void calcDiff_wrap(const boost::shared_ptr<CostDataAbstract>& data, const Eigen::VectorXd& x,
                     const Eigen::VectorXd& u) {
    calcDiff(data, x, u, true);
  }
  void calcDiff_wrap(const boost::shared_ptr<CostDataAbstract>& data, const Eigen::VectorXd& x) {
    calcDiff(data, x, unone_, true);
  }
  void calcDiff_wrap(const boost::shared_ptr<CostDataAbstract>& data, const Eigen::VectorXd& x, const bool& recalc) {
    calcDiff(data, x, unone_, recalc);
  }

#endif
};

struct CostDataAbstract {
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  template <typename Model>
  CostDataAbstract(Model* const model, DataCollectorAbstract* const data)
      : shared(data),
        activation(model->get_activation()->createData()),
        cost(0.),
        Lx(model->get_state()->get_ndx()),
        Lu(model->get_nu()),
        Lxx(model->get_state()->get_ndx(), model->get_state()->get_ndx()),
        Lxu(model->get_state()->get_ndx(), model->get_nu()),
        Luu(model->get_nu(), model->get_nu()),
        r(model->get_activation()->get_nr()),
        Rx(model->get_activation()->get_nr(), model->get_state()->get_ndx()),
        Ru(model->get_activation()->get_nr(), model->get_nu()) {
    Lx.fill(0);
    Lu.fill(0);
    Lxx.fill(0);
    Lxu.fill(0);
    Luu.fill(0);
    r.fill(0);
    Rx.fill(0);
    Ru.fill(0);
  }
  virtual ~CostDataAbstract() {}

  DataCollectorAbstract* shared;
  boost::shared_ptr<ActivationDataAbstract> activation;
  double cost;
  Eigen::VectorXd Lx;
  Eigen::VectorXd Lu;
  Eigen::MatrixXd Lxx;
  Eigen::MatrixXd Lxu;
  Eigen::MatrixXd Luu;
  Eigen::VectorXd r;
  Eigen::MatrixXd Rx;
  Eigen::MatrixXd Ru;
};

}  // namespace crocoddyl

#endif  // CROCODDYL_MULTIBODY_COST_BASE_HPP_


#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MAIN minmax tests

#include <boost/test/unit_test.hpp>

#include <algorithm>
#include <initializer_list>
#include <vector>

#include <point.h>

using point = point_t<int>;

BOOST_AUTO_TEST_SUITE( minmax_test_suite )

BOOST_AUTO_TEST_CASE( binary_min_default_compare )
{
    const point p1{0, 0};
    const point p2{10, 20};
    BOOST_CHECK_EQUAL(std::min(p1, p2), p1);
    BOOST_CHECK_EQUAL(std::min(p2, p1), p1);
    BOOST_CHECK_EQUAL(std::min(p1, p2), (point{0, 0}));
}

BOOST_AUTO_TEST_CASE( binary_min_custom_compare )
{
    const point p1{0, 0};
    const point p2{0, 20};
    BOOST_CHECK_EQUAL(std::min(p1, p2, raster_by_x()), p2);
    BOOST_CHECK_EQUAL(std::min(p2, p1, raster_by_x()), p2);
    BOOST_CHECK_EQUAL(std::min(p1, p2, raster_by_x()), (point{0, 20}));
}

BOOST_AUTO_TEST_CASE( binary_min_conversion_compare )
{
    const point p1{-10, -10};
    const point p2{5, 5};
    /*! p1 is smaller in the default, raster order, but has larger area after
     * the implicit conversion to rectangle. */
    BOOST_CHECK_EQUAL(std::min(p1, p2, compare_by_area<int>()), p2);
}

BOOST_AUTO_TEST_CASE( binary_max_default_compare )
{
    const point p1{0, 0};
    const point p2{10, 20};
    BOOST_CHECK_EQUAL(std::max(p1, p2), p2);
    BOOST_CHECK_EQUAL(std::max(p2, p1), p2);
    BOOST_CHECK_EQUAL(std::max(p1, p2), (point{10, 20})); /*! most vexing parse, but there you are */
}

BOOST_AUTO_TEST_CASE( binary_max_custom_compare )
{
    const point p1{0, 0};
    const point p2{0, 20};
    BOOST_CHECK_EQUAL(std::max(p1, p2, raster_by_x()), p1);
    BOOST_CHECK_EQUAL(std::max(p2, p1, raster_by_x()), p1);
    BOOST_CHECK_EQUAL(std::max(p1, p2, raster_by_x()), (point{0, 0}));
}

BOOST_AUTO_TEST_CASE( binary_max_conversion_compare )
{
    const point p1{-10, -10};
    const point p2{5, 5};
    BOOST_CHECK_EQUAL(std::max(p1, p2, compare_by_area<int>()), p1);
}

BOOST_AUTO_TEST_CASE( n_ary_min_default_compare )
{
    std::initializer_list<point> points{{0, 0}, {50, 50}, {20, 10}, {100, 100}, {15, 50}};
    const point target{0, 0};

    BOOST_CHECK_EQUAL(std::min(points), target);
}

BOOST_AUTO_TEST_CASE( n_ary_min_custom_compare )
{
    std::initializer_list<point> points{{0, 0}, {50, 50}, {20, 10}, {100, 100}, {15, 50}};
    const point target{0, 0};

    BOOST_CHECK_EQUAL(std::min(points, raster_by_x()), target);
}

BOOST_AUTO_TEST_CASE( n_ary_min_conversion_compare )
{
    std::initializer_list<point> points{{0, 0}, {50, 50}, {20, 10}, {100, 100}, {15, 50}};
    const point target{0, 0};

    BOOST_CHECK_EQUAL(std::min(points, compare_by_area<int>()), target);
}

BOOST_AUTO_TEST_CASE( n_ary_max_default_compare )
{
    std::initializer_list<point> points{{0, 0}, {50, 50}, {20, 10}, {100, 100}, {15, 50}};
    const point target{100, 100};

    BOOST_CHECK_EQUAL(std::max(points), target);
}

BOOST_AUTO_TEST_CASE( n_ary_max_custom_compare )
{
    std::initializer_list<point> points{{0, 0}, {50, 50}, {20, 10}, {100, 100}, {15, 50}};
    const point target{100, 100};

    BOOST_CHECK_EQUAL(std::max(points, raster_by_x()), target);
}

BOOST_AUTO_TEST_CASE( n_ary_max_conversion_compare )
{
    std::initializer_list<point> points{{0, 0}, {50, 50}, {20, 10}, {100, 100}, {15, 50}};
    const point target{100, 100};

    BOOST_CHECK_EQUAL(std::max(points, compare_by_area<int>()), target);
}

BOOST_AUTO_TEST_CASE( min_element_default_compare )
{
    points_variety_fixture f;
    const point target{0, 0};

    BOOST_CHECK_EQUAL(*std::min_element(std::begin(f.points), std::end(f.points)), target);
}

BOOST_AUTO_TEST_CASE( min_element_custom_compare )
{
    points_variety_fixture f;
    const point target{0, 5};

    BOOST_CHECK_EQUAL(*std::min_element(std::begin(f.points), std::end(f.points), raster_by_x()), target);
}

BOOST_AUTO_TEST_CASE( min_element_conversion_compare )
{
    points_variety_fixture f;
    const point target{0, 0};

    BOOST_CHECK_EQUAL(*std::min_element(std::begin(f.points), std::end(f.points), compare_by_area<int>()), target);
}

BOOST_AUTO_TEST_CASE( max_element_default_compare )
{
    points_variety_fixture f;
    const point target{100, 100};

    BOOST_CHECK_EQUAL(*std::max_element(std::begin(f.points), std::end(f.points)), target);
}

BOOST_AUTO_TEST_CASE( max_element_custom_compare )
{
    points_variety_fixture f;
    const point target{100, 100};

    BOOST_CHECK_EQUAL(*std::max_element(std::begin(f.points), std::end(f.points), raster_by_x()), target);
}

BOOST_AUTO_TEST_CASE( max_element_conversion_compare )
{
    points_variety_fixture f;
    const point target{100, 100};

    BOOST_CHECK_EQUAL(*std::max_element(std::begin(f.points), std::end(f.points), compare_by_area<int>()), target);
}

BOOST_AUTO_TEST_CASE( binary_minmax_default_compare )
{
    const point p1{0, 0};
    const point p2{0, 20};

    auto minmax = std::minmax(p1, p2);

    BOOST_CHECK_EQUAL(minmax.first, p1);
    BOOST_CHECK_EQUAL(minmax.second, p2);
}

BOOST_AUTO_TEST_CASE( binary_minmax_custom_compare )
{
    const point p1{0, 0};
    const point p2{0, 20};

    auto minmax = std::minmax(p1, p2, raster_by_x());

    BOOST_CHECK_EQUAL(minmax.first, p2);
    BOOST_CHECK_EQUAL(minmax.second, p1);
}

BOOST_AUTO_TEST_CASE( binary_minmax_conversion_compare )
{
    const point p1{0, 0};
    const point p2{0, 20};

    auto minmax = std::minmax(p1, p2, compare_by_area<int>());

    BOOST_CHECK_EQUAL(minmax.first, p1);
    BOOST_CHECK_EQUAL(minmax.second, p2);
}

BOOST_AUTO_TEST_CASE( n_ary_minmax_default_compare )
{
    std::initializer_list<point> points{{0, 0}, {50, 50}, {20, 10}, {100, 100}, {15, 50}, {100, 90}, {0, 5}};
    const point min{0, 0};
    const point max{100, 100};

    auto minmax = std::minmax(points);

    BOOST_CHECK_EQUAL(minmax.first, min);
    BOOST_CHECK_EQUAL(minmax.second, max);
}

BOOST_AUTO_TEST_CASE( n_ary_minmax_custom_compare )
{
    std::initializer_list<point> points{{0, 0}, {50, 50}, {20, 10}, {100, 100}, {15, 50}, {100, 90}, {0, 5}};
    const point min{0, 5};
    const point max{100, 90};

    auto minmax = std::minmax(points, raster_by_x());

    BOOST_CHECK_EQUAL(minmax.first, min);
    BOOST_CHECK_EQUAL(minmax.second, max);
}

BOOST_AUTO_TEST_CASE( n_ary_minmax_conversion_compare )
{
    std::initializer_list<point> points{{0, 0}, {50, 50}, {20, 10}, {100, 100}, {15, 50}, {100, 90}, {0, 5}};
    const point min{0, 0};
    const point max{100, 100};

    auto minmax = std::minmax(points, compare_by_area<int>());

    BOOST_CHECK_EQUAL(minmax.first, min);
    BOOST_CHECK_EQUAL(minmax.second, max);
}

BOOST_AUTO_TEST_CASE( minmax_element_default_compare )
{
    points_variety_fixture f;
    const point min_target{0, 0};
    const point max_target{100, 100};

    auto minmax = std::minmax_element(std::begin(f.points), std::end(f.points));

    BOOST_CHECK_EQUAL(*minmax.first, min_target);
    BOOST_CHECK_EQUAL(*minmax.second, max_target);
}

BOOST_AUTO_TEST_CASE( minmax_element_custom_compare )
{
    points_variety_fixture f;
    const point min_target{0, 5};
    const point max_target{100, 100};

    auto minmax = std::minmax_element(std::begin(f.points), std::end(f.points), raster_by_x());

    BOOST_CHECK_EQUAL(*minmax.first, min_target);
    BOOST_CHECK_EQUAL(*minmax.second, max_target);
}

BOOST_AUTO_TEST_CASE( minmax_element_conversion_compare )
{
    points_variety_fixture f;
    const point min_target{0, 0};
    const point max_target{100, 100};

    auto minmax = std::minmax_element(std::begin(f.points), std::end(f.points), compare_by_area<int>());

    BOOST_CHECK_EQUAL(*minmax.first, min_target);
    BOOST_CHECK_EQUAL(*minmax.second, max_target);
}

BOOST_AUTO_TEST_SUITE_END( /* minmax_test_suite */ )


#include <boost/random.hpp>
#include <iostream>

int main()
{
	boost::mt19937 mtRand;

	for (int i = 0; i < 100; ++i)
	{
		std::cout << mtRand() << std::endl;
	}

	return 0;
}


#include <iostream>
#include <vector>
#include <cmath>

#include <boost/timer/timer.hpp>

#include <poplar/DeviceManager.hpp>
#include <poplar/IPUModel.hpp>

#include <poplar/Engine.hpp>
#include <poplar/Program.hpp>
#include <poplin/codelets.hpp>
#include <popops/ElementWise.hpp>
#include <popops/codelets.hpp>
#include <popops/ExprOp.hpp>
#include <popops/Pad.hpp>
#include <poputil/TileMapping.hpp>

#include "KalmanFilter.h"

using namespace poplar;
using namespace poplar::program;
using namespace popops;

float d = 1.0;
float sigma = 10E-2;
int N = 5;
float z = 0.1;
float x0 = 0.01;
float theta0 = 10E-3;

int main(int argc, char const *argv[]) {

  Device dev = KalmanFilter::connectToIPU();

  // IPUModel ipuModel;
  // ipuModel.compileIPUCode = true;

  // Device dev = ipuModel.createDevice();

  Graph graph(dev.getTarget());

  popops::addCodelets(graph);
  poplin::addCodelets(graph);
  graph.addCodelets("matrixInverseVertex.cpp");
  graph.addCodelets("matrixProductVertex.cpp");
  graph.addCodelets("scaledAddVertex.cpp");
  graph.addCodelets("packHitsVertex.cpp");

  int n_inputs = 1;
  int batch_size = 1;

  std::vector<Tensor> inputs(n_inputs);
  std::vector<Tensor> inputs_batch(n_inputs);

  for (uint i = 0; i < inputs.size(); i++) {

    std::string iStr = std::to_string(i);

    inputs[i] = graph.addVariable(FLOAT, {5, 2}, "x_in" + iStr);
    inputs_batch[i] = graph.addVariable(FLOAT, {uint(batch_size), 5 * 2}, "x_in_batch" + iStr); // Check dims!
    graph.setTileMapping(inputs[i], i);
    graph.setTileMapping(inputs_batch[i], i);
  }

  Sequence preProg;

  std::vector<DataStream> inStreams(n_inputs);
  std::vector<Tensor> covs(n_inputs);
  std::vector<Tensor> qs(n_inputs);
  std::vector<Tensor> hs(n_inputs);
  std::vector<Tensor> gs(n_inputs);
  std::vector<Tensor> fs(n_inputs);
  std::vector<Tensor> d(n_inputs);
  std::vector<Tensor> dInit(n_inputs);
  std::vector<Tensor> dSkip(n_inputs);
  std::vector<Tensor> scatterInto(n_inputs);
  std::vector<Tensor> loop(n_inputs);
  std::vector<Tensor> zero(n_inputs);
  std::vector<Tensor> one(n_inputs);
  std::vector<Tensor> loop_batch(n_inputs);
  std::vector<Tensor> hitThisLoop(n_inputs);

  std::vector<Tensor> p_proj_all(n_inputs);
  std::vector<Tensor> C_proj_all(n_inputs);

  std::vector<Tensor> p_filt_all(n_inputs);
  std::vector<Tensor> C_filt_all(n_inputs);

  std::vector<Tensor> p_smooth(n_inputs);
  std::vector<Tensor> C_smooth(n_inputs);

  for (uint i = 0; i < covs.size(); i++) {

    std::string iStr = std::to_string(i);

    inStreams[i] = graph.addHostToDeviceFIFO("inStream" + iStr, FLOAT, 5 * 2 * batch_size);
    preProg.add(Copy(inStreams[i], inputs_batch[i]));

  }

  Sequence prog;

  std::vector<Tensor> covFlat(covs.size());

  for (uint i = 0; i < covs.size(); i++) {

    std::string iStr = std::to_string(i);

    loop[i] = graph.addVariable(INT, {1}, "loop");
    graph.setTileMapping(loop[i], i);

    scatterInto[i] = graph.addConstant<int>(INT, {1}, {0});
    graph.setTileMapping(scatterInto[i], i);

    zero[i] = graph.addConstant<int>(INT, {1}, {0});
    graph.setTileMapping(zero[i], i);

    one[i] = graph.addConstant<int>(INT, {1}, {1});
    graph.setTileMapping(one[i], i);

    prog.add(Copy(inputs_batch[i].slice(0, 1, 0).reshape({5, 2}), inputs[i]));

    covFlat[i] = graph.addConstant<float>(FLOAT, {16, 1}, {sigma * sigma, 0., 0., 0.,
                                                          0., M_PI, 0., 0.,
                                                          0., 0., sigma * sigma, 0.,
                                                          0., 0., 0., M_PI
                                                          });

    Tensor qFlat = graph.addConstant<float>(FLOAT, {16, 1}, {0.});

    Tensor hFlat = graph.addConstant<float>(FLOAT, {16, 1}, {1., 0., 0., 0.,
                                                             0., 0., 0., 0.,
                                                             0., 0., 1., 0.,
                                                             0., 0., 0., 0.});
    Tensor fFlat = graph.addConstant<float>(FLOAT, {16, 1}, {1., 1., 0., 0.,
                                                             0., 1., 0., 0.,
                                                             0., 0., 1., 1.,
                                                             0., 0., 0., 1.});
    Tensor gFlat = graph.addConstant<float>(FLOAT, {16, 1}, {float(1.0)/(sigma * sigma), 0., 0., 0.,
                                                             0, 0., 0., 0.,
                                                             0, 0., float(1.0)/(sigma * sigma), 0.,
                                                             0, 0., 0., 0.,
                                                             });

    d[i] = graph.addVariable(FLOAT, {1, 1}, "d" + iStr);
    dInit[i] = graph.addConstant<float>(FLOAT, {1, 1}, {1.});
    dSkip[i] = graph.addConstant<float>(FLOAT, {1, 1}, {2.});

    prog.add(Copy(dInit[i], d[i]));

    covs[i] = graph.addVariable(FLOAT, {4, 4}, "cov" + iStr);
    prog.add(Copy(covFlat[i].reshape({4, 4}), covs[i]));

    p_proj_all[i] = graph.addVariable(FLOAT, {5, 4, 1}, "p_proj_all" + iStr);
    C_proj_all[i] = graph.addVariable(FLOAT, {5, 4, 4}, "C_proj_all" + iStr);

    p_filt_all[i] = graph.addVariable(FLOAT, {5, 4, 1}, "p_filt_all" + iStr);
    C_filt_all[i] = graph.addVariable(FLOAT, {5, 4, 4}, "C_filt_all" + iStr);

    p_smooth[i] = graph.addVariable(FLOAT, {4, 1}, "p_smooth" + iStr);
    C_smooth[i] = graph.addVariable(FLOAT, {4, 4}, "C_smooth" + iStr);

    graph.setTileMapping(p_proj_all[i], i);
    graph.setTileMapping(C_proj_all[i], i);
    graph.setTileMapping(p_filt_all[i], i);
    graph.setTileMapping(C_filt_all[i], i);

    graph.setTileMapping(p_smooth[i], i);
    graph.setTileMapping(C_smooth[i], i);

    qs[i] = qFlat.reshape({4, 4});

    hs[i] = hFlat.reshape({4, 4});
    gs[i] = gFlat.reshape({4, 4});
    fs[i] = fFlat.reshape({4, 4});

    graph.setTileMapping(covFlat[i], i);
    graph.setTileMapping(qFlat, i);
    graph.setTileMapping(d[i], i);
    graph.setTileMapping(dInit[i], i);
    graph.setTileMapping(dSkip[i], i);

    graph.setTileMapping(covs[i], i);
    graph.setTileMapping(qs[i], i);

    graph.setTileMapping(gFlat, i);
    graph.setTileMapping(hFlat, i);
    graph.setTileMapping(fFlat, i);
    graph.setTileMapping(hs[i], i);
    graph.setTileMapping(gs[i], i);
    graph.setTileMapping(fs[i], i);
  }

  // Init p with hits
  auto [packIterationTensorsInit, ps] = KalmanFilter::packIterationTensors(graph, loop, scatterInto, inputs);

  prog.add(packIterationTensorsInit);

  // Prepare hits for each loop
  auto [packIterationTensorsProg, hits] = KalmanFilter::packIterationTensors(graph, loop, scatterInto, inputs);

  auto [projProg, p_proj, C_proj] = KalmanFilter::project(graph, ps, covs, fs, qs);

  auto [filterSeq, outP, C] = KalmanFilter::filter(graph, hs, gs, hits, p_proj, C_proj);

  std::vector<Tensor> p_proj_chi2(ps.size());
  std::vector<Tensor> p_filt_chi2(ps.size());
  std::vector<Tensor> C_proj_chi2(ps.size());

  std::vector<Tensor> chiSqThreshold(ps.size());

  // For chi2 calc, test
  for (uint i = 0; i < ps.size(); i++) {

    std::string iStr = std::to_string(i);

    p_proj_chi2[i] = graph.addVariable(FLOAT, {4, 1}, "p_proj_chi2" + iStr);
    graph.setTileMapping(p_proj_chi2[i], i);

    p_filt_chi2[i] = graph.addVariable(FLOAT, {4, 1}, "p_filt_chi2" + iStr);
    graph.setTileMapping(p_filt_chi2[i], i);

    C_proj_chi2[i] = graph.addVariable(FLOAT, {4, 4}, "C_proj_chi2" + iStr);
    graph.setTileMapping(C_proj_chi2[i], i);

    chiSqThreshold[i] = graph.addConstant<float>(FLOAT, {1}, {0.4});
    graph.setTileMapping(chiSqThreshold[i], i);

  }

  // ChiSq computation

  auto [resSeq, res] = KalmanFilter::calcResidual(graph, hits, p_filt_chi2, hs);

  auto [chiSqSeq, chiSq] = KalmanFilter::calcChiSq(graph, res, gs, C_proj_chi2, p_proj_chi2, p_filt_chi2);

  auto [chiSqTestSeq, chiSqTestPred] = KalmanFilter::chiSqTest(graph, chiSq, chiSqThreshold);

  // Update loop index
  Sequence updateIterator;

  for (uint i = 0; i < inputs.size(); i++) {

    auto [computeIterate, itr] = KalmanFilter::iterate(graph, loop[i], i);
    updateIterator.add(Execute(computeIterate));
    updateIterator.add(Copy(itr, loop[i]));

  }

  Sequence planeLoop;

  planeLoop.add(packIterationTensorsProg);

  // For EKF
  // planeLoop.add(stateProg);
  // planeLoop.add(jacProg);

  planeLoop.add(projProg);
  planeLoop.add(filterSeq);

  // Save proj and filt states for smoothing step

  for (uint i = 0; i < ps.size(); i++) {

    auto [append_p_proj_Seq, p_proj_new] = KalmanFilter::appendTo(graph, p_proj[i], loop[i], p_proj_all[i], i);
    auto [append_C_proj_Seq, C_proj_new] = KalmanFilter::appendTo(graph, C_proj[i], loop[i], C_proj_all[i], i);
    auto [append_p_filt_Seq, p_filt_new] = KalmanFilter::appendTo(graph, outP[i], loop[i], p_filt_all[i], i);
    auto [append_C_filt_Seq, C_filt_new] = KalmanFilter::appendTo(graph, C[i], loop[i], C_filt_all[i], i);

    planeLoop.add(Execute(append_p_proj_Seq));
    planeLoop.add(Execute(append_C_proj_Seq));
    planeLoop.add(Execute(append_p_filt_Seq));
    planeLoop.add(Execute(append_C_filt_Seq));

    planeLoop.add(Copy(p_proj_new, p_proj_all[i]));
    planeLoop.add(Copy(C_proj_new, C_proj_all[i]));
    planeLoop.add(Copy(p_filt_new, p_filt_all[i]));
    planeLoop.add(Copy(C_filt_new, C_filt_all[i]));

  }

  // For chi2 calc, test
  // for (uint i = 0; i < ps.size(); i++) {
  //
  //   planeLoop.add(Copy(p_proj[i], p_proj_chi2[i]));
  //   planeLoop.add(Copy(outP[i], p_filt_chi2[i]));
  //   planeLoop.add(Copy(C_proj[i], C_proj_chi2[i]));
  //
  // }

  // planeLoop.add(resSeq);
  // planeLoop.add(chiSqSeq);
  // planeLoop.add(chiSqTestSeq);

  planeLoop.add(updateIterator);

  // Set up p for next projection, using p_filt
  // Set up covs for next projection, using C(_filt)
  for (uint i = 0; i < inputs.size(); i++) {
    planeLoop.add(Copy(outP[i], ps[i]));
    planeLoop.add(Copy(C[i], covs[i]));
  }

  // for (uint i = 0; i < inputs.size(); i++) {
  //
  //   auto [skipSwitchSeq, switchP, switchC, outD] = skipSwitch(graph, ps[i], covs[i],
  //                                                     outP[i], C[i],
  //                                                     chiSq[i],
  //                                                     i);
  //   planeLoop.add(Execute(skipSwitchSeq));
  //   planeLoop.add(Copy(switchP, ps[i]));
  //   planeLoop.add(Copy(switchC, covs[i]));
  //   planeLoop.add(Copy(outD, d[i]));
  // }

  prog.add(poplar::program::Repeat(5, planeLoop));

  for (uint i = 0; i < inputs.size(); i++) {
    // Reset iterator for smoothing step
    prog.add(Copy(zero[i], loop[i]));

    // Copy last filtered state to initial smoothing state
    prog.add(Copy(outP[i], p_smooth[i]));
    prog.add(Copy(C[i], C_smooth[i]));
  }

  std::vector<Tensor> p_proj_smooth(inputs.size());
  std::vector<Tensor> C_proj_smooth(inputs.size());
  std::vector<Tensor> p_filt_smooth(inputs.size());
  std::vector<Tensor> C_filt_smooth(inputs.size());

  Sequence smoothLoop;

  for (uint i = 0; i < inputs.size(); i++) {

    // Offset iterator for filtered states (when starting from idx == 0)
    auto [loopFiltSeq, loopFilt] = KalmanFilter::iterate(graph, loop[i], i);
    smoothLoop.add(Execute(loopFiltSeq));

    auto [sm_p_proj_seq, p_proj] = KalmanFilter::smoothingState(graph, p_proj_all[i], loop[i], i);
    auto [sm_C_proj_seq, C_proj] = KalmanFilter::smoothingState(graph, C_proj_all[i], loop[i], i);
    auto [sm_p_filt_seq, p_filt] = KalmanFilter::smoothingState(graph, p_filt_all[i], loopFilt, i);
    auto [sm_C_filt_seq, C_filt] = KalmanFilter::smoothingState(graph, C_filt_all[i], loopFilt, i);

    smoothLoop.add(Execute(sm_p_proj_seq));
    smoothLoop.add(Execute(sm_C_proj_seq));
    smoothLoop.add(Execute(sm_p_filt_seq));
    smoothLoop.add(Execute(sm_C_filt_seq));

    p_proj_smooth[i] = p_proj;
    C_proj_smooth[i] = C_proj;
    p_filt_smooth[i] = p_filt;
    C_filt_smooth[i] = C_filt;

  }

  auto [smoothSeq, p_smooth_new, C_smooth_new] = KalmanFilter::smooth(graph, p_smooth, C_smooth, p_filt_smooth, C_filt_smooth, p_proj_smooth, C_proj_smooth, fs);

  smoothLoop.add(smoothSeq);

  smoothLoop.add(PrintTensor("p_smooth_new", p_smooth_new[0]));

  for (uint i = 0; i < inputs.size(); i++) {

    // Propagate last smoothed state
    smoothLoop.add(Copy(p_smooth_new[i], p_smooth[i]));
    smoothLoop.add(Copy(C_smooth_new[i], C_smooth[i]));

  }

  smoothLoop.add(updateIterator);

  prog.add(poplar::program::Repeat(4, smoothLoop));

  // After loop over planes, for the next in the batch:

  for (uint i = 0; i < inputs.size(); i++) {
    // Reset plane iterator for next batch
    prog.add(Copy(zero[i], loop[i]));
  }

  Sequence progRepeat;
  progRepeat.add(poplar::program::Repeat(batch_size, prog));

  Sequence progMain;
  progMain.add(preProg);
  progMain.add(progRepeat);

  // Engine engine(graph, prog);
  Engine engine(graph, progMain);
  engine.load(dev);

  // Test input
  std::vector<float> v1 = {
    -0.02062073, -0.12062073,
    -0.02062073, -0.22062073,
    -0.12062073, -0.42062073,
    -0.12062073, -0.52062073,
    -0.22062073, -0.62062073,
  };

  std::vector<std::vector<float>> vs;
  for (uint i = 0; i < inputs.size(); i++) {
    // Instead of 1 * 5 inputs, N * 5 inputs
    std::vector<float> v10;
    for (uint j = 0; j < batch_size; j++) {
      v10.insert(std::end(v10), std::begin(v1), std::end(v1));
    }
    vs.push_back(v10);
  }

  for (uint i = 0; i < inputs.size(); i++) {
    engine.connectStream(inStreams[i], &vs[i][0], &vs[i][5 * 2 * batch_size]);
  }

  {
  boost::timer::auto_cpu_timer t;
  engine.run(0);
  }

  return 0;
}


//####### Test module for Quantum Synchrotron emission engine (normalized units) #######

//Define Module name
 #define BOOST_TEST_MODULE "Quantum Synchrotron emission engine (normalized units)"

//Will automatically define a main for this test
 #define BOOST_TEST_DYN_LINK

 #include <utility>

 //Include Boost unit tests library & library for floating point comparison
 #include <boost/test/unit_test.hpp>
 #include <boost/test/floating_point_comparison.hpp>

//Normalized units are used for this test
#define PXRMP_USE_NORMALIZED_UNITS
#include "quantum_sync_engine.hpp"

//Uses random numbers
#include "rng_wrapper.hpp"

using namespace picsar::multi_physics;

//________________________________

//Helper function
template<typename REAL>
quantum_synchrotron_engine<REAL, stl_rng_wrapper<REAL>> get_qs_stl_set_lambda(uint64_t seed, REAL lambda,
quantum_synchrotron_engine_ctrl<REAL> qs_ctrl = quantum_synchrotron_engine_ctrl<REAL>())
{
    stl_rng_wrapper<REAL> wrap{seed};
    auto qs_engine =  quantum_synchrotron_engine<REAL, stl_rng_wrapper<REAL>>{std::move(wrap), 1.0, qs_ctrl};
    qs_engine.set_lambda(static_cast<REAL>(lambda));
    return qs_engine;
}

// ------------- Tests --------------

//Tolerance for double precision calculations
const double double_tolerance = 1.0e-3;

//Tolerance for single precision calculations
const float float_tolerance = 1.0e-2;

//Templated tolerance
template <typename T>
T tolerance()
{
    if(std::is_same<T,float>::value)
        return float_tolerance;
    else
        return double_tolerance;
}

//Test get/set lambda for quantum_synchrotron_engine generic
template <typename T>
void quantum_sync_engine_gs()
{
    auto qs_engine = get_qs_stl_set_lambda<T>
        (390109317, static_cast<T>(800.0*si_nanometer));
    BOOST_CHECK_EQUAL( static_cast<T>(800.0*si_nanometer), qs_engine.get_lambda());
}

//Test get/set lambda for quantum_synchrotron_engine (double precision)
BOOST_AUTO_TEST_CASE( quantum_sync_engine_gs_double_1 )
{
    quantum_sync_engine_gs<double>();
}

//Test get/set lambda for quantum_synchrotron_engine (single precision)
BOOST_AUTO_TEST_CASE( quantum_sync_engine_gs_single_1 )
{
    quantum_sync_engine_gs<float>();
}

// ------------- optical depth --------------
//Test get/set lambda forquantum_synchrotron_engine generic
template <typename T>
void quantum_sync_engine_opt()
{
    auto qs_engine = get_qs_stl_set_lambda<T>
        (390109317, static_cast<T>(800.0*si_nanometer));
    BOOST_TEST ( qs_engine.get_optical_depth() >= static_cast<T>(0.0) );
}

//Test get new optical depth (double precision)
BOOST_AUTO_TEST_CASE( quantum_sync_engine_opt_double_1 )
{
    quantum_sync_engine_opt<double>();
}

//Test get new optical depth (single precision)
BOOST_AUTO_TEST_CASE( quantum_sync_engine_opt_single_1 )
{
    quantum_sync_engine_opt<float>();
}

// ------------- photon emission rate --------------

//Test photon emission (generic)
template <typename T>
void quantum_sync_engine_phtoem()
{
    T lambda = static_cast<T>(800. * si_nanometer);

    auto qs_engine = get_qs_stl_set_lambda<T>(390109317,lambda);

    T px =  static_cast<T>(149.825);
    T py =  static_cast<T>(933.115);
    T pz =  static_cast<T>(-538.195);
    T ex =  static_cast<T>(931.686);
    T ey =  static_cast<T>(-861.074);
    T ez =  static_cast<T>(944.652);
    T bx =  static_cast<T>(531.406);
    T by =  static_cast<T>(670.933);
    T bz =  static_cast<T>(660.057);

    //T exp = static_cast<T>(xxx);
    T energy = sqrt( static_cast<T>(1.0) + norm2(vec3<T>{px,py,pz}));
    T res = qs_engine.compute_dN_dt
        (energy, chi_lepton(px,py,pz,ex,ey,ez,bx,by,bz,lambda) );

    BOOST_CHECK_EQUAL(res/energy,res/energy); //TO DO
    //BOOST_CHECK_SMALL(res, tolerance<T>());
}

//Test photon emission (double precision)
BOOST_AUTO_TEST_CASE( quantum_sync_engine_phtoem_double_1 )
{
    quantum_sync_engine_phtoem<double>();
}

//Test photon emission (single precision)
BOOST_AUTO_TEST_CASE( quantum_sync_engine_phtoem_single_1 )
{
    quantum_sync_engine_phtoem<float>();
}


#include <boost/test/unit_test.hpp>

#include "defines.hpp"

#include <scorum/protocol/asset.hpp>
#include <scorum/rewards_math/formulas.hpp>

using namespace scorum::rewards_math;
using namespace scorum::protocol;

using scorum::protocol::curve_id;

namespace database_fixture {
struct rewards_math_abs_reward_shares_fixture
{
    const uint16_t used_voting_power = SCORUM_PERCENT(50);
    const share_type effective_balance_shares = ASSET_SP(100e+3).amount;
};
}

using namespace database_fixture;

BOOST_FIXTURE_TEST_SUITE(rewards_math_abs_reward_shares_tests, rewards_math_abs_reward_shares_fixture)

BOOST_AUTO_TEST_CASE(calculate_abs_reward_shares_zerro_balance)
{
    BOOST_CHECK_EQUAL(calculate_abs_reward_shares(used_voting_power, share_type(0)), share_type(0));
}

BOOST_AUTO_TEST_CASE(calculate_abs_reward_shares_positive)
{
    BOOST_CHECK_LT(calculate_abs_reward_shares(used_voting_power, effective_balance_shares), effective_balance_shares);
}

BOOST_AUTO_TEST_SUITE_END()


/*
 * Copyright (c) 2019 Opticks Team. All Rights Reserved.
 *
 * This file is part of Opticks
 * (see https://bitbucket.org/simoncblyth/opticks).
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *   http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#include <cmath>
#include <cassert>
#include <cmath>
#include <cstring>

#include <boost/math/constants/constants.hpp>

#include "OpticksCSG.h"

#include "NPrism.hpp"
#include "NBBox.hpp"
#include "NPlane.hpp"
#include "NPart.hpp"

nprism::nprism(float apex_angle_degrees, float height_mm, float depth_mm, float fallback_mm)
{
    param.f.x = apex_angle_degrees  ;
    param.f.y = height_mm  ;
    param.f.z = depth_mm  ;
    param.f.w = fallback_mm  ;
}

nprism::nprism(const nquad& param_)
{
    param = param_ ;
}

float nprism::height()
{
    return param.f.y > 0.f ? param.f.y : param.f.w ;
}
float nprism::depth()
{
    return param.f.z > 0.f ? param.f.z : param.f.w ;
}
float nprism::hwidth()
{
    float pi = boost::math::constants::pi<float>() ;
    return height()*tan((pi/180.f)*param.f.x/2.0f) ;
}

void nprism::dump(const char* msg)
{
    param.dump(msg);
}

nbbox nprism::bbox()
{
    float h  = height();
    float hw = hwidth();
    float d  = depth();

    nbbox bb ;
    bb.min = {-hw,0.f,-d/2.f } ;
    bb.max = { hw,  h, d/2.f } ;

    return bb ;
}

npart nprism::part()
{
    // hmm more dupe of hemi-pmt.cu/make_prism
    // but if could somehow make vector types appear
    // the same could use same code with CUDA ?

    nbbox bb = bbox();

    npart p ;
    p.zero();
    p.setParam(param) ;
    p.setTypeCode(CSG_PRISM);
    p.setBBox(bb);

    return p ;
}


//
// Created by tom on 01/12/2020.
//

#ifndef SHAPE_RETRIEVAL__BAG_OF_FEATURES_H_
#define SHAPE_RETRIEVAL__BAG_OF_FEATURES_H_

#define GABOR_KERNEL_SIZE 11
#define TILE_SIZE 4
#define KERNEL_WIDTH 0.2
#define NB_ORIENTATION 4

#include <Eigen/Core>
#include <opencv2/opencv.hpp>
#include <opencv2/imgproc.hpp>
#include <cmath>
#include <omp.h>
#include <iostream>

using namespace std;
using namespace Eigen;
using namespace cv;

class BagOfFeatures
{
 private :

	Mat line_rendering;

 public :
	MatrixXd features;

	BagOfFeatures();
	BagOfFeatures(Mat& _line_rendering);
	~BagOfFeatures();

	void gabor_computing();

	Mat ApplyFiter(double theta, double sigma_x, double sigma_y, double omega);
};

#endif //SHAPE_RETRIEVAL__BAG_OF_FEATURES_H_


#pragma once

// Armadillo
#include <armadillo>

// Mantella
#include "mantella_bits/optimisationProblem.hpp"

namespace mant {
  namespace bbob {
    class BlackBoxOptimisationBenchmark : public OptimisationProblem {
     public:
      explicit BlackBoxOptimisationBenchmark(
          const arma::uword numberOfDimensions);

     protected:
      arma::vec getRandomParameterTranslation();

      arma::vec getParameterConditioning(
          const double conditionNumber);

      arma::vec getConditionedParameter(
          const arma::vec& parameter);

      arma::vec getAsymmetricParameter(
          const double asymmetry,
          const arma::vec& parameter);

      arma::vec getOscillatedParameter(
          const arma::vec& parameter);

      double getRandomObjectiveValueTranslation();

      double getOscillatedValue(
          const double oscilliation);
    };
  }
}


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

  Program:   Visualization Toolkit
  Module:    vtkAnnotateOBBs.cxx

  Copyright (c) Ken Martin, Will Schroeder, Bill Lorensen
  All rights reserved.
  See Copyright.txt or http://www.kitware.com/Copyright.htm for details.

     This software is distributed WITHOUT ANY WARRANTY; without even
     the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
     PURPOSE.  See the above copyright notice for more information.

=========================================================================*/
#include "vtkAnnotateOBBs.h"

#include "vtkPolyData.h"
#include "vtkPointData.h"
#include "vtkInformation.h"
#include "vtkInformationVector.h"
#include "vtkNew.h"
#include "vtkObjectFactory.h"
#include "vtkStreamingDemandDrivenPipeline.h"
#include "vtkSmartPointer.h"
#include "vtkAppendPolyData.h"
#include "vtkThresholdPoints.h"
#include "vtkOutlineSource.h"
#include "vtkOBBTree.h"

#include <Eigen/Geometry>

//----------------------------------------------------------------------------
namespace {

  static void ComputeOBB(vtkPolyData* polyData, double cornerData[24])
  {
    Eigen::Vector3d origin, x, y, z, sizes;
    vtkSmartPointer<vtkOBBTree> obb = vtkSmartPointer<vtkOBBTree>::New();
    obb->ComputeOBB(polyData->GetPoints(), origin.data(), x.data(), y.data(), z.data(), sizes.data());

    // make right handed
    Eigen::Vector3d rightHandedZ = x.cross(y);
    if (z.dot(rightHandedZ) < 0)
      {
      origin += z;
      z *= -1;
      }

    Eigen::Vector3d corners[8];
    corners[0] = origin;
    corners[1] = origin+x;
    corners[2] = origin+y;
    corners[3] = origin+x+y;
    corners[4] = z+origin;
    corners[5] = z+origin+x;
    corners[6] = z+origin+y;
    corners[7] = z+origin+x+y;
    for (int i = 0; i < 8; ++i)
      {
      cornerData[i*3+0] = corners[i][0];
      cornerData[i*3+1] = corners[i][1];
      cornerData[i*3+2] = corners[i][2];
      }
  }

}

//----------------------------------------------------------------------------
vtkStandardNewMacro(vtkAnnotateOBBs);

//----------------------------------------------------------------------------
vtkAnnotateOBBs::vtkAnnotateOBBs()
{
  this->SetNumberOfInputPorts(1);
  this->SetNumberOfOutputPorts(1);
  this->AnnotateLabelZero = true;

  this->SetInputArrayToProcess(
    0,0,0, vtkDataObject::FIELD_ASSOCIATION_POINTS,
    vtkDataSetAttributes::SCALARS);
}

//----------------------------------------------------------------------------
vtkAnnotateOBBs::~vtkAnnotateOBBs()
{
}

//----------------------------------------------------------------------------
int vtkAnnotateOBBs::RequestData(
  vtkInformation* vtkNotUsed(request),
  vtkInformationVector **inputVector,
  vtkInformationVector *outputVector)
{
  vtkInformation *inInfo = inputVector[0]->GetInformationObject(0);
  vtkPolyData *input = vtkPolyData::SafeDownCast(inInfo->Get(vtkDataObject::DATA_OBJECT()));

  vtkInformation *outInfo = outputVector->GetInformationObject(0);
  vtkPolyData *output = vtkPolyData::SafeDownCast(outInfo->Get(vtkDataObject::DATA_OBJECT()));

  vtkDataArray* labels = this->GetInputArrayToProcess(0, inputVector);
  if (!labels)
    {
    vtkErrorMacro("Could not get input scalars.");
    return 0;
    }

  vtkSmartPointer<vtkPolyData> inputCopy = vtkSmartPointer<vtkPolyData>::New();
  inputCopy->ShallowCopy(input);

  vtkIdType minLabel = labels->GetRange()[0];
  vtkIdType maxLabel = labels->GetRange()[1];

  if (minLabel == 0 && !this->AnnotateLabelZero)
    {
    minLabel = 1;
    }

  vtkNew<vtkAppendPolyData> appendFilter;

  for (vtkIdType i = minLabel; i <= maxLabel; ++i)
    {
    vtkNew<vtkThresholdPoints> threshold;
    threshold->ThresholdBetween(i, i);
    threshold->SetInputData(inputCopy);
    threshold->SetInputArrayToProcess(0, 0, 0, vtkDataObject::FIELD_ASSOCIATION_POINTS, labels->GetName());
    threshold->Update();
    vtkPolyData* labelPoints = threshold->GetOutput();

    double cornerData[24];
    ComputeOBB(labelPoints, cornerData);

    vtkSmartPointer<vtkOutlineSource> outline = vtkSmartPointer<vtkOutlineSource>::New();
    outline->SetBoxTypeToOriented();
    outline->SetCorners(cornerData);
    appendFilter->AddInputConnection(outline->GetOutputPort());
    }

  if (appendFilter->GetNumberOfInputConnections(0))
    {
    appendFilter->Update();
    }

  output->ShallowCopy(appendFilter->GetOutput());
  return 1;
}

//----------------------------------------------------------------------------
void vtkAnnotateOBBs::PrintSelf(ostream& os, vtkIndent indent)
{
  this->Superclass::PrintSelf(os,indent);
}


//  (C) Copyright Gennadiy Rozental 2011-2014.
//  Distributed under the Boost Software License, Version 1.0.
//  (See accompanying file LICENSE_1_0.txt or copy at
//  http://www.boost.org/LICENSE_1_0.txt)

//  See http://www.boost.org/libs/test for the library home page.
//
//!@file
//!@brief Random generator
// ***************************************************************************

#ifndef BOOST_TEST_DATA_MONOMORPHIC_GENERATORS_RANDOM_HPP_101512GER
#define BOOST_TEST_DATA_MONOMORPHIC_GENERATORS_RANDOM_HPP_101512GER

// Boost.Test
#include <boost/test/data/config.hpp>

#if !defined(BOOST_TEST_NO_RANDOM_DATASET_AVAILABLE) || defined(BOOST_TEST_DOXYGEN_DOC__)

#include <boost/test/data/monomorphic/generate.hpp>
#include <boost/test/data/monomorphic/generators/keywords.hpp>

// STL
#include <random>

#include <boost/test/detail/suppress_warnings.hpp>

//____________________________________________________________________________//

namespace boost {
namespace unit_test {
namespace data {

namespace {
nfp::keyword<struct seed_t>         seed;
nfp::keyword<struct distrbution_t>  distribution;
nfp::keyword<struct engine_t>       engine;
} // local namespace

namespace monomorphic {

namespace ds_detail {
template<typename SampleType>
struct default_distribution {
    typedef typename mpl::if_<std::is_integral<SampleType>,
                              std::uniform_int_distribution<SampleType>,
                              std::uniform_real_distribution<SampleType> >::type type;
};

} // namespace ds_detail

// ************************************************************************** //
// **************                   random_t                   ************** //
// ************************************************************************** //

/*!@brief Generator for the random sequences
 *
 * This class implements the generator concept (see @ref boost::unit_test::data::monomorphic::generated_by) for implementing
 * a random number generator.
 */
template<typename SampleType        = double,
         typename DistributionType  = typename ds_detail::default_distribution<SampleType>::type,
         typename EngineType        = std::default_random_engine>
class random_t {
public:
    typedef SampleType          data_type;
    typedef DistributionType    distr_type;
    typedef EngineType          engine_type;

    random_t()
    : m_distribution()
    , m_engine( std::random_device()() )
    {}
    explicit random_t( distr_type&& d )
    : m_distribution( std::forward<distr_type>(d) )
    , m_engine( std::random_device()() ){}
    random_t( engine_type&& e, distr_type&& d )
    : m_distribution( std::forward<distr_type>(d) )
    , m_engine( std::forward<engine_type>(e) ){}

    // Generator interface
    data::size_t        capacity() const    { return BOOST_TEST_DS_INFINITE_SIZE; }
    SampleType          next()
    {
        return m_distribution( m_engine );
    }
    void                reset()             {}

    //! Sets the seed of the pseudo-random number engine.
    template<typename SeedType>
    void seed( SeedType&& seed )            { m_engine.seed( std::forward<SeedType>( seed ) ); }

private:
    // Data members
    DistributionType    m_distribution;
    EngineType          m_engine;
};

//____________________________________________________________________________//

} // namespace monomorphic

//! @brief Returns an infinite sequence of random numbers.
//!
//! The following overloads are available:
//! @code
//! auto d = random();
//! auto d = random(begin, end);
//! auto d = random(params);
//! @endcode
//!
//!
//! - The first overload uses the default distribution, which is uniform and which elements
//!   are @c double type (the values are in [0, 1) ).
//! - The second overload generates numbers in the given interval. The distribution is uniform (in [begin, end)
//!   for real numbers, and in [begin, end] for integers). The type of the distribution is deduced from the type
//!   of the @c begin and @c end parameters.
//! - The third overload generates numbers using the named parameter inside @c params , which are:
//!   - @c distribution: the distribution used. In this overload, since the type of the samples cannot be deduced,
//!     the samples are of type @c double and the distribution is uniform real in [0, 1).
//!   - @c seed: the seed for generating the values
//!   - @c engine: the random number generator engine
//!
//! The function returns an object that implements the dataset API.
//! @note This function is available only for C++11 capable compilers.
inline monomorphic::generated_by< monomorphic::random_t<> > random()
{
    return monomorphic::generated_by<monomorphic::random_t<>>( monomorphic::random_t<>() );
}

//____________________________________________________________________________//

/// @overload boost::unit_test::data::random()
template<typename SampleType>
inline monomorphic::generated_by< monomorphic::random_t<SampleType> >
random( SampleType begin, SampleType end )
{
    typedef monomorphic::random_t<SampleType> Gen;
    typedef typename Gen::distr_type distr_type;
    return monomorphic::generated_by<Gen>( Gen( distr_type(begin,end) ) );
}

//____________________________________________________________________________//

namespace ds_detail {
template<typename Params>
struct random_gen_type {
    typedef typename nfp::param_type<Params,decltype(distribution),std::uniform_real_distribution<>>::type distr_type;
    typedef typename nfp::param_type<Params,decltype(engine),std::default_random_engine>::type engine_type;
    typedef typename distr_type::result_type sample_type;

    typedef monomorphic::random_t<sample_type,distr_type,engine_type> type;
};

}

/// @overload boost::unit_test::data::random()
template<typename Params>
inline monomorphic::generated_by<typename ds_detail::random_gen_type<Params>::type>
random( Params const& params )
{
    typedef typename ds_detail::random_gen_type<Params>::type Gen;
    typedef typename Gen::distr_type distr_type;
    typedef typename Gen::engine_type engine_type;

    std::random_device rd;
    engine_type E;
//    engine_type E( rd );
    if( params.has(engine) )
        E = params[engine];

    distr_type D;
    if( params.has(distribution) )
        D = params[distribution];

    Gen G( std::move(E), std::move(D) );

    if( params.has(seed) )
        G.seed( params[seed] );

    return monomorphic::generated_by<Gen>( std::move(G) );
}

} // namespace data
} // namespace unit_test
} // namespace boost

#include <boost/test/detail/enable_warnings.hpp>

#endif // BOOST_TEST_NO_RANDOM_DATASET_AVAILABLE

#endif // BOOST_TEST_DATA_MONOMORPHIC_GENERATORS_RANDOM_HPP_101512GER


#include "matrix_product_dense.h"
#include <optional>
#include <Eigen/Core>
#include <Eigen/LU>

template<typename T> using MatrixType  = Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic, Eigen::ColMajor>;
template<typename T> using VectorType  = Eigen::Matrix<T, Eigen::Dynamic, 1, Eigen::ColMajor>;
template<typename T> using VectorTypeT = Eigen::Matrix<T, 1, Eigen::Dynamic, Eigen::RowMajor>;

namespace dense_lu {
    std::optional<Eigen::PartialPivLU<MatrixType<double>>>               lu_real       = std::nullopt;
    std::optional<Eigen::PartialPivLU<MatrixType<std::complex<double>>>> lu_cplx       = std::nullopt;

    void reset(){
        lu_real.reset();
        lu_cplx.reset();
    }
    template<typename Scalar>
    void init(){
        if constexpr (std::is_same_v<Scalar,double>)
            dense_lu::lu_real = Eigen::PartialPivLU<MatrixType<Scalar>>();
        if constexpr (std::is_same_v<Scalar,std::complex<double>>)
            dense_lu::lu_cplx = Eigen::PartialPivLU<MatrixType<Scalar>>();
    }
}

template<typename Scalar>
DenseMatrixProduct<Scalar>::~DenseMatrixProduct(){
    dense_lu::reset();
}

// Pointer to data constructor, copies the matrix into an internal Eigen matrix.
template<typename Scalar>
DenseMatrixProduct<Scalar>::DenseMatrixProduct(const Scalar *const A_, const int L_,const bool copy_data, const eigutils::eigSetting::Form form_, const eigutils::eigSetting::Side side_)
    : A_ptr(A_), L(L_), form(form_), side(side_) {
    if (copy_data){
        A_stl.resize(L*L);
        std::copy(A_ptr,A_ptr + L*L, A_stl.begin());
        A_ptr = A_stl.data();
    }
    dense_lu::init<Scalar>();
    init_profiling();
}

template<typename Scalar>
void DenseMatrixProduct<Scalar>::print() const {
    Eigen::Map<const MatrixType<Scalar>> A_matrix (A_ptr, L, L);
    std::cout << "A_matrix: \n" << A_matrix << std::endl;
}

// Function definitions

template<typename Scalar>
void DenseMatrixProduct<Scalar>::FactorOP()

/*  Partial pivot LU decomposition
 *  Factors P(A-sigma*I) = LU
 */
{
    if(readyFactorOp) { return; }
    std::cout << "Starting LU \n";
    Eigen::Map<const MatrixType<Scalar>> A_matrix (A_ptr, L, L);

    t_factorOp.tic();
    assert(readyShift and "Shift value sigma has not been set.");

    if constexpr(std::is_same_v<Scalar, double>) {
        dense_lu::lu_real = Eigen::PartialPivLU<MatrixType<Scalar>>();
        dense_lu::lu_real.value().compute(A_matrix - sigmaR * Eigen::MatrixXd::Identity(L, L));
    }
    if constexpr(std::is_same_v<Scalar, std::complex<double>>) {
        Scalar sigma = std::complex<double>(sigmaR, sigmaI);
        dense_lu::lu_cplx = Eigen::PartialPivLU<MatrixType<Scalar>>();
        dense_lu::lu_cplx.value().compute(A_matrix - sigma * Eigen::MatrixXd::Identity(L, L));
    }

    readyFactorOp = true;
    t_factorOp.toc();
    std::cout << "Finished LU \n";
    std::cout << "Time LU Op [ms]: " << std::fixed << std::setprecision(3) << t_factorOp.get_last_time_interval() * 1000 << '\n';
}

template<typename Scalar>
void DenseMatrixProduct<Scalar>::MultOPv(Scalar *x_in_ptr, Scalar *x_out_ptr) {
    using namespace eigutils::eigSetting;
    assert(readyFactorOp and "FactorOp() has not been run yet.");
    switch(side) {
        case Side::R: {
            Eigen::Map<VectorType<Scalar>> x_in(x_in_ptr, L);
            Eigen::Map<VectorType<Scalar>> x_out(x_out_ptr, L);
            if constexpr(std::is_same_v<Scalar, double>)
                x_out.noalias() = dense_lu::lu_real.value().solve(x_in);
            if constexpr(std::is_same_v<Scalar, std::complex<double>>)
                x_out.noalias() = dense_lu::lu_cplx.value().solve(x_in);
            break;
        }
        case Side::L: {
            Eigen::Map<VectorTypeT<Scalar>> x_in(x_in_ptr, L);
            Eigen::Map<VectorTypeT<Scalar>> x_out(x_out_ptr, L);
            if constexpr(std::is_same_v<Scalar, double>)
                x_out.noalias() = x_in * dense_lu::lu_real.value().inverse();
            if constexpr(std::is_same_v<Scalar, std::complex<double>>)
                x_out.noalias() = x_in * dense_lu::lu_cplx.value().inverse();
            break;
        }
    }
    counter++;
}

template<typename Scalar>
void DenseMatrixProduct<Scalar>::MultAx(Scalar *x_in, Scalar *x_out) {
    using namespace eigutils::eigSetting;
    Eigen::Map<const MatrixType<Scalar>> A_matrix (A_ptr, L, L);
    switch(form) {
        case Form::NONSYMMETRIC:
            switch(side) {
                case Side::R: {
                    Eigen::Map<VectorType<Scalar>> x_vec_in(x_in, L);
                    Eigen::Map<VectorType<Scalar>> x_vec_out(x_out, L);
                    x_vec_out.noalias() = A_matrix * x_vec_in;
                    break;
                }
                case Side::L: {
                    Eigen::Map<VectorTypeT<Scalar>> x_vec_in(x_in, L);
                    Eigen::Map<VectorTypeT<Scalar>> x_vec_out(x_out, L);
                    x_vec_out.noalias() = x_vec_in * A_matrix;
                    break;
                }
            }
            break;
        case Form::SYMMETRIC: {
            Eigen::Map<VectorType<Scalar>> x_vec_in(x_in, L);
            Eigen::Map<VectorType<Scalar>> x_vec_out(x_out, L);
            x_vec_out.noalias() = A_matrix.template selfadjointView<Eigen::Upper>() * x_vec_in;
            break;
        }
    }
    counter++;
}

// Explicit instantiations
template class DenseMatrixProduct<double>;
template class DenseMatrixProduct<std::complex<double>>;


/**
 * @file
 * @brief Implementation of linear Lagrangian finite elements for the Dirichlet
 *        problem for the Laplacian
 * @author Ralf Hiptmair
 * @date   October 2018
 * @copyright MIT License
 */

#include <cmath>

#include <boost/filesystem.hpp>
#include <boost/program_options.hpp>

#include <lf/assemble/assemble.h>
#include <lf/geometry/geometry.h>
#include <lf/io/io.h>
#include <lf/mesh/hybrid2d/hybrid2d.h>
#include <lf/refinement/refinement.h>
#include "lf/fe/fe.h"
#include "lf/mesh/test_utils/test_meshes.h"
#include "lf/mesh/utils/utils.h"

static unsigned int dbg_ctrl = 0;
const unsigned int dbg_dofh = 1;
const unsigned int dbg_mesh = 2;
const unsigned int dbg_mat = 4;
const unsigned int dbg_vec = 8;
const unsigned int dbg_bdf = 16;
const unsigned int dbg_elim = 32;
const unsigned int dbg_basic = 64;
const unsigned int dbg_trp = 128;

/**
 * @brief Build a vector of boundary flags for degrees of freedom
 *
 * @param dofh Local-to-global mapper managing indexing for shape
 * functions
 * @return boolean vector of boundary flags, whose length agrees with the
 *         number of global shape functions managed by `dofh`.
 *
 * Every global shape functions belongs to a unique mesh entity. If that
 * entity is contained in the boundary of the mesh, then the flag array
 * entry with the index of the global shape function is set to `true`,
 * otherwise to `false`.
 */
std::vector<bool> flagBoundaryDOFs(const lf::assemble::DofHandler &dofh) {
  const lf::base::size_type N{dofh.NoDofs()};
  // Flag all entities on the boundary
  auto bd_flags{lf::mesh::utils::flagEntitiesOnBoundary(dofh.Mesh())};
  // Run through all global shape functions and check whether
  // they are associated with an entity on the boundary. Store
  // this information in a boolean vector
  std::vector<bool> tmp_bd_flags(N, false);
  for (lf::assemble::gdof_idx_t dofnum = 0; dofnum < N; ++dofnum) {
    const lf::mesh::Entity &dof_entity{dofh.Entity(dofnum)};
    tmp_bd_flags[dofnum] = bd_flags(dof_entity);
    SWITCHEDSTATEMENT(
        dbg_ctrl, dbg_bdf,
        std::cout << "FBD: dof " << dofnum << "@ " << dof_entity << " ["
                  << dofh.Mesh()->Index(dof_entity) << "] ";
        if (tmp_bd_flags[dofnum]) { std::cout << "ON BOUNDARY"; } std::cout
        << std::endl);
  }
  return tmp_bd_flags;
}

/** @brief Eliminate degrees of freedom located on the boundary
 *
 * No longer in use. Replaced with lf::assemble::fix_flagged_solution_components
 */
void eliminateBoundaryDofs(const std::vector<bool> &tmp_bd_flags,
                           lf::assemble::COOMatrix<double> *A) {
  const lf::base::size_type N = tmp_bd_flags.size();
  LF_ASSERT_MSG((A->cols() == N) && (A->rows() == N),
                "Matrix dimension mismath");

  // Remove rows associated with dofs on the boundary
  auto new_last = std::remove_if(
      A->triplets().begin(), A->triplets().end(),
      [&tmp_bd_flags](lf::assemble::COOMatrix<double>::Triplet &triplet) {
        SWITCHEDSTATEMENT(dbg_ctrl, dbg_elim, if (tmp_bd_flags[triplet.row()]) {
          std::cout << "EBD: removing " << triplet.row() << ',' << triplet.col()
                    << "[" << triplet.value() << "]" << std::endl;
        });
        return tmp_bd_flags[triplet.row()];
      });
  A->triplets().erase(new_last, A->triplets().end());
  // Add unit diagonal entries to rows belonging to dofs on the boundary
  for (lf::assemble::gdof_idx_t dofnum = 0; dofnum < N; ++dofnum) {
    if (tmp_bd_flags[dofnum]) {
      A->AddToEntry(dofnum, dofnum, 1.0);
    }
  }
}

/** @brief Insert sampled values from Dirichlet data into right hand side
 * vector
 *
 */
void insertDirichletDataRHS(const std::vector<bool> &tmp_bd_flags,
                            Eigen::VectorXd *rhs,
                            const Eigen::VectorXd &dirichlet_values) {
  const lf::base::size_type N = tmp_bd_flags.size();
  LF_VERIFY_MSG((rhs->size() == N), "rhs vector size mismatch");
  LF_VERIFY_MSG((dirichlet_values.size() == N), "data vector size mismatch");

  for (lf::assemble::gdof_idx_t dofnum = 0; dofnum < N; ++dofnum) {
    if (tmp_bd_flags[dofnum]) {
      // Shape function associated with the boundary
      (*rhs)[dofnum] = dirichlet_values[dofnum];
    }
  }
}

/** @brief Solves Dirichlet problem for the Laplacian
 *
 * @tparam SOLFUNC functor providing boundary values/exact solution
 * @tparam RHSFUNC functor for right hand side source function
 * @param mesh reference to the mesh on which the FE solution is to be
 * computed
 * @param u functor object for solution, also used to sample Dirichlet
 * data
 * @param f object supplying right-hand-side source function
 *
 * @return L2 norm of the nodal error, which is just the L2 norm of the
 *         discretization error approximated by means of the the 2D
 * trapezoidal rule.
 */
template <typename SOLFUNC, typename RHSFUNC>
double L2ErrorLinearFEDirichletLaplacian(
    const std::shared_ptr<const lf::mesh::Mesh> &mesh_p, SOLFUNC &&u,
    RHSFUNC &&f) {
  LF_ASSERT_MSG(mesh_p != nullptr, "Invalid mesh pointer");
  LF_ASSERT_MSG((mesh_p->DimMesh() == 2) && (mesh_p->DimWorld() == 2),
                "For 2D planar meshes only!");
  // Debugging output
  SWITCHEDSTATEMENT(
      dbg_ctrl, dbg_basic,
      std::cout << "Dirichlet Laplacian: Linear FE L2 error on mesh with "
                << mesh_p->Size(0) << " cells, " << mesh_p->Size(1)
                << " edges, " << mesh_p->Size(2) << " nodes" << std::endl;)
  SWITCHEDSTATEMENT(
      dbg_ctrl, dbg_mesh,
      const int tmp_mesh_ctrl = lf::mesh::hybrid2d::Mesh::output_ctrl_;
      lf::mesh::hybrid2d::Mesh::output_ctrl_ = 100;
      lf::mesh::utils::PrintInfo(*mesh_p, std::cout);
      lf::mesh::hybrid2d::Mesh::output_ctrl_ = tmp_mesh_ctrl);
  // Initialize objects for local computations
  lf::fe::LinearFELaplaceElementMatrix loc_mat_laplace{};
  lf::fe::LinearFELocalLoadVector<double, decltype(f)> loc_vec_sample(f);
  // Initialization of index mapping for linear finite elements
  lf::assemble::UniformFEDofHandler loc_glob_map(
      mesh_p, {{lf::base::RefEl::kPoint(), 1}});
  SWITCHEDSTATEMENT(dbg_ctrl, dbg_dofh,
                    std::cout << loc_glob_map << std::endl;);
  // Dimension of finite element space
  const lf::assemble::size_type N_dofs(loc_glob_map.NoDofs());
  // Matrix in triplet format holding Galerkin matrix
  lf::assemble::COOMatrix<double> mat(N_dofs, N_dofs);
  // Building the Galerkin matrix (trial space = test space)
  // This Galerkin matrix is oblivious of Dirichlet boundary conditions
  mat = lf::assemble::AssembleMatrixLocally<lf::assemble::COOMatrix<double>>(
      0, loc_glob_map, loc_mat_laplace);
  // Debugging output
  SWITCHEDSTATEMENT(dbg_ctrl, dbg_trp, mat.PrintInfo(std::cout));
  SWITCHEDSTATEMENT(dbg_ctrl, dbg_mat,
                    std::cout << "Full " << mat.rows() << 'x' << mat.cols()
                              << " stiffness matrix, " << mat.triplets().size()
                              << " tripets:\n"
                              << mat.makeDense() << std::endl);

  // Filling the right-hand-side vector
  auto rhsvec = lf::assemble::AssembleVectorLocally<Eigen::VectorXd>(
      0, loc_glob_map, loc_vec_sample);
  // Sample Dirichlet date from the exact solution
  Eigen::VectorXd dirichlet_data(loc_glob_map.NoDofs());
  const Eigen::Matrix<double, 0, 1> ref_coord{
      Eigen::Matrix<double, 0, 1>::Zero(0, 1)};
  for (const lf::mesh::Entity &node : mesh_p->Entities(mesh_p->DimMesh())) {
    LF_ASSERT_MSG(node.RefEl() == lf::base::RefEl::kPoint(),
                  "Wrong topological type for a node");
    const Eigen::Vector2d point = node.Geometry()->Global(ref_coord);
    const lf::assemble::size_type num_int_dof =
        loc_glob_map.NoInteriorDofs(node);
    LF_ASSERT_MSG(num_int_dof == 1, "Node with " << num_int_dof << " dof");
    const lf::base::RandomAccessRange<const lf::assemble::gdof_idx_t> gsf_idx(
        loc_glob_map.InteriorGlobalDofIndices(node));
    const lf::assemble::gdof_idx_t node_dof_idx = gsf_idx[0];
    dirichlet_data[node_dof_idx] = u(point);
  }

  // modify linear system in order to take into account boundary data
  std::vector<bool> tmp_bd_flags{flagBoundaryDOFs(loc_glob_map)};
  // >> Old version
  // eliminateBoundaryDofs(tmp_bd_flags, mat);
  // insertDirichletDataRHS(tmp_bd_flags, rhsvec, dirichlet_data);
  // Identify dof indices associated with the boundary
  // >>
  // >> Equivalent new versions
  lf::assemble::fix_flagged_solution_comp_alt<double>(
      [&tmp_bd_flags,
       &dirichlet_data](lf::assemble::gdof_idx_t i) -> std::pair<bool, double> {
        LF_ASSERT_MSG((i < tmp_bd_flags.size()) && (i < dirichlet_data.size()),
                      "Illegal index " << i);
        return std::make_pair(tmp_bd_flags[i], dirichlet_data[i]);
      },
      mat, rhsvec);
  // lf::assemble::fix_flagged_solution_components<double>(
  //   tmp_bd_flags, dirichlet_data, mat, rhsvec);
  // >>
  // Debugging output
  SWITCHEDSTATEMENT(dbg_ctrl, dbg_mat,
                    std::cout << "Reduced " << mat.rows() << 'x' << mat.cols()
                              << " stiffness matrix, " << mat.triplets().size()
                              << " triplets:\n"
                              << mat.makeDense() << std::endl);

  // Initialize sparse matrix
  Eigen::SparseMatrix<double> stiffness_matrix(mat.makeSparse());
  // Solve linear system
  Eigen::SparseLU<Eigen::SparseMatrix<double>> solver;
  solver.compute(stiffness_matrix);
  Eigen::VectorXd sol_vec = solver.solve(rhsvec);
  if (solver.info() != Eigen::Success) {
    std::cout << "solver failed!" << std::endl;
  }

  // Compute the norm of nodal error cell by cell
  double nodal_err = 0.0;
  for (const lf::mesh::Entity &cell : mesh_p->Entities(0)) {
    const lf::base::RandomAccessRange<const lf::assemble::gdof_idx_t>
        cell_dof_idx(loc_glob_map.GlobalDofIndices(cell));
    LF_ASSERT_MSG(loc_glob_map.NoLocalDofs(cell) == cell.RefEl().NumNodes(),
                  "Inconsistent node number");
    const lf::base::size_type num_nodes = cell.RefEl().NumNodes();
    double sum = 0.0;
    for (int k = 0; k < num_nodes; ++k) {
      sum += std::pow(
          sol_vec[cell_dof_idx[k]] - dirichlet_data[cell_dof_idx[k]], 2);
    }
    nodal_err += lf::geometry::Volume(*cell.Geometry()) * (sum / num_nodes);
  }
  return std::sqrt(nodal_err);
}

/** @brief Solves Dirichlet problem for the Laplacian on a sequence of
 *         regularly refined meshes
 * @param coarse_mesh_p pointer to coarsest mesh
 * @param reflevels number of refinements
 * @param u exact solution of the boundary value problem
 * @param f right hand side source function
 * @return L2 norms of discretization errors on each refinement level
 *
 */
template <typename SOLFUNCTOR, typename RHSFUNCTOR>
std::vector<double> SolveDirLaplSeqMesh(
    std::shared_ptr<lf::mesh::Mesh> coarse_mesh_p, unsigned int reflevels,
    SOLFUNCTOR &&u, RHSFUNCTOR &&f) {
  // Prepare for creating a hierarchy of meshes
  std::shared_ptr<lf::mesh::hybrid2d::MeshFactory> mesh_factory_ptr =
      std::make_shared<lf::mesh::hybrid2d::MeshFactory>(2);
  lf::refinement::MeshHierarchy multi_mesh(std::move(coarse_mesh_p),
                                           mesh_factory_ptr);

  // Perform several steps of regular refinement of the given mesh
  for (int refstep = 0; refstep < reflevels; ++refstep) {
    multi_mesh.RefineRegular(/*lf::refinement::RefPat::rp_barycentric*/);
  }
  // Solve Dirichlet boundary value problem on every level
  lf::assemble::size_type L = multi_mesh.NumLevels();
  std::vector<double> errors(L);
  for (int level = 0; level < L; level++) {
    errors.push_back(
        L2ErrorLinearFEDirichletLaplacian(multi_mesh.getMesh(level), u, f));
  }
  return errors;
}

int main(int argc, const char **argv) {
  // Pointer to the current mesh
  std::shared_ptr<lf::mesh::Mesh> mesh_p;

  // Processing command line arguments
  bool verbose = false;
  namespace po = boost::program_options;
  po::options_description desc("Allowed options");
  // clang-format off
  desc.add_options()
  ("help,h", "-h -v -f <filename> -s <selection>")
  ("filename,f", "File to load coarse mesh from ")
  ("selector,s", po::value<int>()->default_value(0), "Selection of test mesh")
  ("reflevels,r", po::value<int>()->default_value(2), "Number of refinement levels")
  ("bvpsel,b", po::value<int>()->default_value(0),
   "Selector for Dirichlet data and rhs function")
  ("verbose,v", po::bool_switch(&verbose),"Enable verbose mode");
  // clang-format on
  po::variables_map vm;
  po::store(po::parse_command_line(argc, argv, desc), vm);
  if (vm.count("help") > 0) {
    std::cout << desc << std::endl;
    std::cout << "Internal variables that can be set by name=value args"
              << std::endl;
    lf::base::ListCtrlVars(std::cout);
  } else {
    lf::base::ReadCtrVarsCmdArgs(argc, argv);
    std::cout << "*** Solving Dirichlet problems for the Laplacian ***"
              << std::endl;
    // Retrieve number of degrees of freedom for each entity type from
    // command line arguments
    if (vm.count("filename") > 0) {
      // A filename was specified
      std::string filename{vm["filename"].as<std::string>()};
      if (filename.length() > 0) {
        std::cout << "Reading mesh from file " << filename << std::endl;
        boost::filesystem::path here = __FILE__;
        auto mesh_file_path = here.parent_path() / filename.c_str();
        auto mesh_factory =
            std::make_unique<lf::mesh::hybrid2d::MeshFactory>(2);
        lf::io::GmshReader reader(std::move(mesh_factory),
                                  mesh_file_path.string());
        mesh_p = reader.mesh();
      }
    } else {
      std::cout << "No mesh file supplied, using GenerateHybrid2DTestMesh()"
                << std::endl;
      if (vm.count("selector") > 0) {
        const int selector = vm["selector"].as<int>();
        std::cout << "Using test mesh no " << selector << std::endl;
        if ((selector >= 0) && (selector <= 4)) {
          mesh_p = lf::mesh::test_utils::GenerateHybrid2DTestMesh(selector);
        }
      }
    }
    // Set number of refinement levels
    unsigned int reflevels = 2;
    if (vm.count("reflevels") > 0) {
      reflevels = vm["reflevels"].as<int>();
    }
    unsigned int bvpsel = 0;
    if (vm.count("bvpsel") > 0) {
      bvpsel = vm["bvpsel"].as<int>();
    }
    if (mesh_p == nullptr) {
      // Default mesh
      std::cout << "Using default mesh; test mesh 0" << std::endl;
      mesh_p = lf::mesh::test_utils::GenerateHybrid2DTestMesh(0);
    }
    // At this point a pointer to the mesh is stored in mesh_p
    // Output summary information about the coarsest mesh
    std::cout << "Coarse mesh: " << mesh_p->Size(0) << " cells, "
              << mesh_p->Size(1) << " edges, " << mesh_p->Size(2) << " vertices"
              << std::endl;
    std::cout << reflevels << " refinement levels requested" << std::endl;

    // Problem data provided by function pointers
    std::function<double(const Eigen::Vector2d &)> u, f;

    // Initialize the problem data
    std::cout << "Problem setting " << bvpsel << " selected" << std::endl;
    switch (bvpsel) {
      case 0: {
        // A linear solution, no error, if contained in FE space
        f = [](const Eigen::Vector2d &) { return 0.0; };
        u = [](const Eigen::Vector2d &x) { return (x[0] + 2.0 * x[1]); };
        break;
      }
      case 1: {
        // Quadratic polynomial solution
        f = [](const Eigen::Vector2d &) { return -4.0; };
        u = [](const Eigen::Vector2d &x) {
          return (std::pow(x[0], 2) + std::pow(x[1], 2));
        };
        break;
      }
      default: {
        LF_VERIFY_MSG(false, "Illegal problem number");
        break;
      }
    }

    // Set debugging switches
    lf::fe::LinearFELaplaceElementMatrix::dbg_ctrl = 0;
    // LinearFELaplaceElementMatrix::dbg_geo |
    // LinearFELaplaceElementMatrix::dbg_locmat;
    lf::fe::LinearFELocalLoadVector<double, decltype(f)>::dbg_ctrl = 0;
    lf::assemble::DofHandler::output_ctrl_ = 6;
    dbg_ctrl = dbg_basic;  // | dbg_mat | dbg_mesh | dbg_dofh | dbg_trp;
    // lf::assemble::ass_mat_dbg_ctrl = 255;

    // Compute finite element solution and error
    auto L2errs = SolveDirLaplSeqMesh(mesh_p, reflevels, u, f);
    int level = 0;
    for (auto &err : L2errs) {
      std::cout << "L2 rrror on level " << level << " = " << err << std::endl;
      level++;
    }
  }
  return 0;
}


// Dlib resnet implementation, from dlib/examples
#include <dlib/dnn.h>
#include <iostream>
#include <dlib/data_io.h>

using namespace std;
using namespace dlib;

// we build blocks using alias templates

template <
    int N,
    template <typename> class BN,
    int stride,
    typename SUBNET
    >
using block  = BN<con<N,3,3,1,1,relu<BN<con<N,3,3,stride,stride,SUBNET>>>>>;

// skip connections
template <
    template <int,template<typename>class,int,typename> class block,
    int N,
    template<typename>class BN,
    typename SUBNET
    >
using residual = add_prev1<block<N,BN,1,tag1<SUBNET>>>;

// downsampling resnet bloack
template <
    template <int,template<typename>class,int,typename> class block,
    int N,
    template<typename>class BN,
    typename SUBNET
    >
using residual_down = add_prev2<avg_pool<2,2,2,2,skip1<tag2<block<N,BN,2,tag1<SUBNET>>>>>>;

// affine layers instead of batch norm for testing
template <typename SUBNET> using res       = relu<residual<block,8,bn_con,SUBNET>>;
template <typename SUBNET> using ares      = relu<residual<block,8,affine,SUBNET>>;
template <typename SUBNET> using res_down  = relu<residual_down<block,8,bn_con,SUBNET>>;
template <typename SUBNET> using ares_down = relu<residual_down<block,8,affine,SUBNET>>;

// with the help of aliases the main network is defined
const unsigned long number_of_classes = 10;
using net_type = loss_multiclass_log<fc<number_of_classes,
                            avg_pool_everything<
                            res<res<res<res_down<
                            repeat<9,res, // repeat this layer 9 times
                            res_down<
                            res<
                            input<matrix<unsigned char>>
                            >>>>>>>>>>;

// prelu block
template <typename SUBNET>
using pres  = prelu<add_prev1<bn_con<con<8,3,3,1,1,prelu<bn_con<con<8,3,3,1,1,tag1<SUBNET>>>>>>>>;

// ----------------------------------------------------------------------------------------

int main(int argc, char** argv) try
{
	// Parse path to mnist
    if (argc != 2)
    {
        cout << "This example needs the MNIST dataset to run!" << endl;
        cout << "You can get MNIST from http://yann.lecun.com/exdb/mnist/" << endl;
        cout << "Download the 4 files that comprise the dataset, decompress them, and" << endl;
        cout << "put them in a folder.  Then give that folder as input to this program." << endl;
        return 1;
    }

    // containers for ubyte mnist
    std::vector<matrix<unsigned char>> training_images;
    std::vector<unsigned long> training_labels;
    std::vector<matrix<unsigned char>> testing_images;
    std::vector<unsigned long> testing_labels;
    // load the data using the provided function from dlib
    load_mnist_dataset(argv[1], training_images, training_labels, testing_images, testing_labels);

    // slower cuDNN methods for big data
    set_dnn_prefer_smallest_algorithms();

    // creating an initial network
    net_type net;
    // change the number of fc neurons in the last layer
    net_type net2(num_fc_outputs(15));

    // this shows a way to replace relu with prelu
    using net_type2 = loss_multiclass_log<fc<number_of_classes,
                                avg_pool_everything<
                                pres<res<res<res_down< // 2 prelu layers here
                                tag4<repeat<9,pres,    // 9 groups, each containing 2 prelu layers
                                res_down<
                                res<
                                input<matrix<unsigned char>>
                                >>>>>>>>>>>;
    net_type2 pnet(prelu_(0.2),
                   prelu_(0.25),
                   repeat_group(prelu_(0.3),prelu_(0.4))
                   );

    // model inspection
    cout << "The pnet has " << pnet.num_layers << " layers in it." << endl;
    // this is like model.summary()
    cout << pnet << endl;

    // get a layers output by index
    layer<3>(pnet).get_output();
    // or get params for vis purposes
    cout << "prelu param: "<< layer<7>(pnet).layer_details().get_initial_param_value() << endl;

    // get layers by their type
    layer<tag1>(pnet);
    layer<tag4,1>(pnet);
    layer<tag4,2>(pnet);

    // trainer with adam instead of sgd
    dnn_trainer<net_type,adam> trainer(net,adam(0.0005, 0.9, 0.999));
    // below code to parallelize training on gpu devices
    //dnn_trainer<net_type,adam> trainer(net,adam(0.0005, 0.9, 0.999), {0,1});

    trainer.be_verbose();
    trainer.set_max_num_epochs(1);
    // scheduler and earlystopping
    trainer.set_iterations_without_progress_threshold(2000);
    trainer.set_learning_rate_shrink_factor(0.1);
    // The learning rate will start at 1e-3.
    trainer.set_learning_rate(1e-3);
    trainer.set_synchronization_file("mnist_resnet_sync", std::chrono::seconds(100));

    // here we use steps to train on a hypothetical dataset that does not fit in ram
    std::vector<matrix<unsigned char>> mini_batch_samples;
    std::vector<unsigned long> mini_batch_labels;
    dlib::rand rnd(time(0));

    while(trainer.get_learning_rate() >= 1e-6)
    {
        mini_batch_samples.clear();
        mini_batch_labels.clear();

        // creating the batch with 128 size
        while(mini_batch_samples.size() < 128)
        {
            auto idx = rnd.get_random_32bit_number()%training_images.size();
            mini_batch_samples.push_back(training_images[idx]);
            mini_batch_labels.push_back(training_labels[idx]);
        }

        // step once with the selected batch
        trainer.train_one_step(mini_batch_samples, mini_batch_labels);

        // also can use trainer.test_one_step for validation data
    }

    // get net, when parallelizing training, waits for all subprocesses to end and gets the model
    trainer.get_net();

    net.clean();
    serialize("mnist_res_network.dat") << net;

    // replace batch norm layers
    using test_net_type = loss_multiclass_log<fc<number_of_classes,
                                avg_pool_everything<
                                ares<ares<ares<ares_down<
                                repeat<9,ares,
                                ares_down<
                                ares<
                                input<matrix<unsigned char>>
                                >>>>>>>>>>;
    // create a testing net and load the model again
    test_net_type tnet = net;
    deserialize("mnist_res_network.dat") >> tnet;

    // predict using the model and see how many of the images were classified correctly

    std::vector<unsigned long> predicted_labels = tnet(training_images);
    int num_right = 0;
    int num_wrong = 0;
    for (size_t i = 0; i < training_images.size(); ++i)
    {
        if (predicted_labels[i] == training_labels[i])
            ++num_right;
        else
            ++num_wrong;

    }
    cout << "training num_right: " << num_right << endl;
    cout << "training num_wrong: " << num_wrong << endl;
    cout << "training accuracy:  " << num_right/(double)(num_right+num_wrong) << endl;

    predicted_labels = tnet(testing_images);
    num_right = 0;
    num_wrong = 0;
    for (size_t i = 0; i < testing_images.size(); ++i)
    {
        if (predicted_labels[i] == testing_labels[i])
            ++num_right;
        else
            ++num_wrong;

    }
    cout << "testing num_right: " << num_right << endl;
    cout << "testing num_wrong: " << num_wrong << endl;
    cout << "testing accuracy:  " << num_right/(double)(num_right+num_wrong) << endl;

}
catch(std::exception& e)
{
    cout << e.what() << endl;
}


//=======================================================================
// Copyright 1997, 1998, 1999, 2000 University of Notre Dame.
// Authors: Andrew Lumsdaine, Lie-Quan Lee, Jeremy G. Siek
//
// This file is part of the Boost Graph Library
//
// You should have received a copy of the License Agreement for the
// Boost Graph Library along with the software; see the file LICENSE.
// If not, contact Office of Research, University of Notre Dame, Notre
// Dame, IN 46556.
//
// Permission to modify the code and to distribute modified code is
// granted, provided the text of this NOTICE is retained, a notice that
// the code was modified is included with the above COPYRIGHT NOTICE and
// with the COPYRIGHT NOTICE in the LICENSE file, and that the LICENSE
// file is distributed with the modified code.
//
// LICENSOR MAKES NO REPRESENTATIONS OR WARRANTIES, EXPRESS OR IMPLIED.
// By way of example, but not limitation, Licensor MAKES NO
// REPRESENTATIONS OR WARRANTIES OF MERCHANTABILITY OR FITNESS FOR ANY
// PARTICULAR PURPOSE OR THAT THE USE OF THE LICENSED SOFTWARE COMPONENTS
// OR DOCUMENTATION WILL NOT INFRINGE ANY PATENTS, COPYRIGHTS, TRADEMARKS
// OR OTHER RIGHTS.
//=======================================================================
#include <boost/config.hpp>
#include <iostream>
#include <iterator>
#include <vector>
#include <algorithm>
#include <utility>
#include <boost/graph/edge_list.hpp>
#include <boost/graph/incremental_components.hpp>
#include <boost/pending/disjoint_sets.hpp>
#include <boost/utility.hpp>
#include <boost/graph/graph_utility.hpp>

/*

  This example demonstrates the usage of the
  connected_components_on_edgelist algorithm. This differs from the
  connect_components algorithm in that the graph object
  only needs to provide access to the "list" of edges (via the
  edges() function).

  The example graphs come from "Introduction to
  Algorithms", Cormen, Leiserson, and Rivest p. 87 (though we number
  the vertices from zero instead of one).

  Sample output:

  An undirected graph (edge list):
  (0,1) (1,4) (4,0) (2,5)
  Total number of components: 3
  Vertex 0 is in the component who's representative is 1
  Vertex 1 is in the component who's representative is 1
  Vertex 2 is in the component who's representative is 5
  Vertex 3 is in the component who's representative is 3
  Vertex 4 is in the component who's representative is 1
  Vertex 5 is in the component who's representative is 5

  component 0 contains: 4 1 0
  component 1 contains: 3
  component 2 contains: 5 2

 */

using namespace std;
using boost::tie;

int main(int , char* [])
{
  using namespace boost;
  typedef int Index; // ID of a Vertex
  typedef pair<Index,Index> Edge;
  const int N = 6;
  const int E = 4;
  Edge edgelist[] = { Edge(0, 1), Edge(1, 4), Edge(4, 0), Edge(2, 5) };

  edge_list<Edge*,Edge,ptrdiff_t,std::random_access_iterator_tag> g(edgelist, edgelist + E);
  cout << "An undirected graph (edge list):" << endl;
  print_edges(g, identity_property_map());
  cout << endl;

  disjoint_sets_with_storage<> ds;
  incremental_components(g, ds);

  component_index<int> components(&ds.parents()[0],
                                  &ds.parents()[0] + ds.parents().size());

  cout << "Total number of components: " << components.size() << endl;
  for (int k = 0; k != N; ++k)
    cout << "Vertex " << k << " is in the component who's representative is "
         << ds.find_set(k) << endl;
  cout << endl;

  for (component_index<int>::size_type i = 0; i < components.size(); ++i) {
    cout << "component " << i << " contains: ";
    component_index<int>::value_type::iterator
      j = components[i].begin(),
      jend = components[i].end();
    for ( ; j != jend; ++j)
      cout << *j << " ";
    cout << endl;
  }

  return 0;
}


#include <Eigen/Core>
#include <Eigen/Geometry>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/common/transforms.h>

#include <iostream>
#include <fstream>
#include <string>

int main(int argc, char* argv[])
{
    if(argc != 3)
    {
        std::cout << "Need 2 argument for Area and Room!" << std::endl;

        return 0;
    }

    std::string Area_num = argv[1];
    std::string Area_name = "Area_" + Area_num;
    std::string Room_name = argv[2];

    pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZRGB>);
    pcl::PCDWriter Pclwriter;

	std::ifstream read_file;

    std::cout << "Opening : ../Stanford3dDataset_v1.2_Aligned_Version/"
                    + Area_name + "/" + Room_name + "/" + Room_name + ".txt" << std::endl;

	read_file.open("../Stanford3dDataset_v1.2_Aligned_Version/"
                    + Area_name + "/" + Room_name + "/" + Room_name + ".txt", ios::binary);

    std::string line;
    float pos;
    float color;
    pcl::PointXYZRGB point;
    float midpoint[3] = {0};
    std::vector<pcl::PointXYZRGB> PointVec;

	while(std::getline(read_file, line))
	{
        std::stringstream lineinput(line);
        lineinput >> pos;
        point.x = pos;
        lineinput >> pos;
        point.y = pos;
        lineinput >> pos;
        point.z = pos;
        lineinput >> color;
        point.r = int(color);
        lineinput >> color;
        point.g = int(color);
        lineinput >> color;
        point.b = int(color);

        PointVec.push_back(point);

        midpoint[0] -= point.x;
        midpoint[1] -= point.y;
        midpoint[2] -= point.z;
	}

    cloud->width = PointVec.size();
    cloud->height = 1;
    cloud->resize(cloud->height * cloud->width);

    for(uint i = 0;i < PointVec.size();i++)
    {
        cloud->points[i] = PointVec[i];
    }

    std::cout << "File read successfully" << std::endl;

    Eigen::Affine3f transform = Eigen::Affine3f::Identity();
    Eigen::Vector3f translation;
    translation << midpoint[0]/PointVec.size(), midpoint[1]/PointVec.size(), midpoint[2]/PointVec.size();
    transform.translate(translation);
    pcl::transformPointCloud(*cloud, *cloud, transform);

    pcl::visualization::CloudViewer viewer("Dataset Visualizer");
    viewer.showCloud(cloud);

    while(!viewer.wasStopped ())
    {

    }

    // Pclwriter.write("../savings/" + Area_num + "_" + Room_num + ".pcd",*(cloud));

    return 0;
}


// Distributed under the MIT License.
// See LICENSE.txt for details.

#pragma once

#include <array>
#include <boost/optional.hpp>
#include <cstddef>
#include <limits>

#include "DataStructures/Tensor/TypeAliases.hpp"
#include "Utilities/TypeTraits.hpp"

namespace PUP {
class er;
}  // namespace PUP

namespace CoordinateMaps {

/*!
 * \ingroup CoordinateMapsGroup
 *
 * \brief Redistributes gridpoints on the sphere.
 * \image html EquatorialCompression.png "A sphere with an `aspect_ratio` of 3."
 *
 * \details A mapping from the sphere to itself which depends on a single
 * parameter, the `aspect_ratio` \f$\alpha\f$, which is the ratio of the
 * horizontal length to the vertical height for a given point. This parameter
 * name was chosen because points with \f$\tan \theta = 1\f$ get mapped to
 * points with \f$\tan \theta' = \alpha\f$. In general, gridpoints located
 * at an angle \f$\theta\f$ from the pole are mapped to a new angle
 * \f$\theta'\f$ satisfying \f$\tan \theta' = \alpha \tan \theta\f$.
 *
 * For an `aspect_ratio` greater than one, the gridpoints are mapped towards
 * the equator, leading to an equatorially compressed grid. For an
 * `aspect_ratio` less than one, the gridpoints are mapped towards the poles.
 * Note that the aspect ratio must be positive.
 *
 * We define the auxiliary variables \f$ r := \sqrt{x^2 + y^2 +z^2}\f$
 * and \f$ \rho := \sqrt{x^2 + y^2 + \alpha^{-2} z^2}\f$.
 *
 * The map corresponding to this transformation in cartesian coordinates
 * is then given by:
 *
 * \f[\vec{x}'(x,y,z) =
 * \frac{r}{\rho}\begin{bmatrix}
 * x\\
 * y\\
 * \alpha^{-1} z\\
 * \end{bmatrix}\f]
 *
 */
class EquatorialCompression {
 public:
  static constexpr size_t dim = 3;
  explicit EquatorialCompression(double aspect_ratio) noexcept;
  EquatorialCompression() = default;
  ~EquatorialCompression() = default;
  EquatorialCompression(EquatorialCompression&&) = default;
  EquatorialCompression(const EquatorialCompression&) = default;
  EquatorialCompression& operator=(const EquatorialCompression&) = default;
  EquatorialCompression& operator=(EquatorialCompression&&) = default;

  template <typename T>
  std::array<tt::remove_cvref_wrap_t<T>, 3> operator()(
      const std::array<T, 3>& source_coords) const noexcept;

  boost::optional<std::array<double, 3>> inverse(
      const std::array<double, 3>& target_coords) const noexcept;

  template <typename T>
  tnsr::Ij<tt::remove_cvref_wrap_t<T>, 3, Frame::NoFrame> jacobian(
      const std::array<T, 3>& source_coords) const noexcept;

  template <typename T>
  tnsr::Ij<tt::remove_cvref_wrap_t<T>, 3, Frame::NoFrame> inv_jacobian(
      const std::array<T, 3>& source_coords) const noexcept;

  // clang-tidy: google runtime references
  void pup(PUP::er& p) noexcept;  // NOLINT

 private:
  template <typename T>
  std::array<tt::remove_cvref_wrap_t<T>, 3> angular_distortion(
      const std::array<T, 3>& coords, double inverse_alpha) const noexcept;
  template <typename T>
  tnsr::Ij<tt::remove_cvref_wrap_t<T>, 3, Frame::NoFrame>
  angular_distortion_jacobian(const std::array<T, 3>& coords,
                              double inverse_alpha) const noexcept;
  friend bool operator==(const EquatorialCompression& lhs,
                         const EquatorialCompression& rhs) noexcept;

  double aspect_ratio_{std::numeric_limits<double>::signaling_NaN()};
  double inverse_aspect_ratio_{std::numeric_limits<double>::signaling_NaN()};
};
bool operator!=(const EquatorialCompression& lhs,
                const EquatorialCompression& rhs) noexcept;
}  // namespace CoordinateMaps


/* ScaFES
 * Copyright (c) 2011-2015, ZIH, TU Dresden, Federal Republic of Germany.
 * For details, see the files COPYING and LICENSE in the base directory
 * of the package.
 */

#ifndef MODULATEDGAUSSIANSOURCE_HPP_
#define    MODULATEDGAUSSIANSOURCE_HPP_

#include <boost/property_tree/ptree.hpp>

template <typename T>
class ModulatedGaussianSource {
public:
    ModulatedGaussianSource(const ModulatedGaussianSource&)=delete;

    ~ModulatedGaussianSource() { };

    ModulatedGaussianSource(const T& dt, const boost::property_tree::ptree& p)
    : dt_(dt)
    , f_mod_(p.get<T>("source.f_mod"))
    , t0_(p.get<T>("source.t0"))
    , spread_(p.get<T>("source.spread")) { };

    template <typename TT> TT operator()(const TT & n) const
    {
        TT v=(t0_-n)/spread_;
        v*=v;
        v*=-.5;
        v=exp(v);
        TT mod=cos(2.*M_PI*f_mod_*n*dt_);
        TT ret=v*mod;
        return ret;
    }

private:
    const T dt_;
    const T f_mod_;
    const T t0_;
    const T spread_;
};

#endif    /* MODULATEDGAUSSIANSOURCE_HPP_ */