/*!
*	@file	CurvatureFlow.cpp
*	@brief	Implementation of a curvature flow algorithm
*/

#include "CurvatureFlow.h"

#include <boost/numeric/ublas/matrix_sparse.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/vector_sparse.hpp>

#include <boost/numeric/bindings/traits/ublas_sparse.hpp>
#include <boost/numeric/bindings/traits/ublas_vector.hpp>

#include <boost/numeric/bindings/umfpack/umfpack.hpp>

namespace psalm
{

/*!
*	Sets default values
*/

CurvatureFlow::CurvatureFlow()
{
	num_steps	= 0;
	dt		= 0.5;
}

/*!
*	Applies the curvature flow algorithm to the vertices of a given mesh.
*	The size of timesteps needs to be set before. The input mesh is
*	irreversibly changed by this operation.
*
*	@param	input_mesh Mesh on which the algorithm works.
*	@return	false if an error occurred, else true
*/

bool CurvatureFlow::apply_to(mesh& input_mesh)
{
	namespace ublas = boost::numeric::ublas;
	namespace umf = boost::numeric::bindings::umfpack;

	size_t n = input_mesh.num_vertices();
	if(n == 0)
		return(true); // silently ignore empty meshes

	// Stores x,y,z components of the vertices in the mesh
	ublas::vector<double> X(n);
	ublas::vector<double> Y(n);
	ublas::vector<double> Z(n);

	// Fill vector with position data
	for(size_t i = 0; i < n; i++)
	{
		const v3ctor& pos = input_mesh.get_vertex(i)->get_position();

		X[i] = pos[0];
		Y[i] = pos[1];
		Z[i] = pos[2];
	}

	for(size_t i = 0; i < num_steps; i++)
	{
		// Prepare for "solving" the linear system (for now, this is something
		// akin to the explicit Euler method)

		ublas::compressed_matrix<	double,
						ublas::column_major,
						0,
						ublas::unbounded_array<int>,
						ublas::unbounded_array<double> > M(n, n);	// transformed matrix for the solving
												// process, i.e. id - dt*K, where K is
												// the matrix of the curvature operator

		M = ublas::identity_matrix<double>(n, n) - dt*calc_curvature_operator(input_mesh);

		// Solve x,y,z components independently. This may be slower,
		// but sufficient for small meshes.

		umf::symbolic_type<double> Symbolic;
		umf::numeric_type<double> Numeric;

		umf::symbolic(M, Symbolic);
		umf::numeric(M, Symbolic, Numeric);

		ublas::vector<double> X_new(input_mesh.num_vertices());
		ublas::vector<double> Y_new(input_mesh.num_vertices());
		ublas::vector<double> Z_new(input_mesh.num_vertices());

		umf::solve(M, X_new, X, Numeric);
		umf::solve(M, Y_new, Y, Numeric);
		umf::solve(M, Z_new, Z, Numeric);

		X = X_new;
		Y = Y_new;
		Z = Z_new;

		for(size_t i = 0; i < n; i++)
			input_mesh.get_vertex(i)->set_position(X[i], Y[i], Z[i]);
	}

	return(true);
}

/*!
*	Given an input mesh, calculates the curvature operator matrix for this
*	mesh. The matrix will be a _sparse_ matrix, hence the need for handling
*	it via boost.
*
*	@param	input_mesh Mesh to be processed
*	@return	Sparse matrix describing the curvature operator
*/

boost::numeric::ublas::compressed_matrix<double> CurvatureFlow::calc_curvature_operator(mesh& input_mesh)
{
	using namespace boost::numeric::ublas;
	compressed_matrix<double> K(input_mesh.num_vertices(), input_mesh.num_vertices()); // K as in "Kurvature"...

	// We iterate over all vertices and calculate the contributions of each
	// vertex to the corresponding entry of the matrix

	for(size_t i = 0; i < input_mesh.num_vertices(); i++)
	{
		vertex* v = input_mesh.get_vertex(i);
		std::vector<const vertex*> neighbours = v->get_neighbours();

		// FIXME: Used to update the correct matrix entries
		// below. This assumes that the IDs have been allocated
		// sequentially.
		size_t cur_id = v->get_id();

		// Find "opposing angles" for all neighbours; these are
		// the $\alpha_{ij}$ and $\beta_{ij}$ values used for
		// calculating the discrete curvature

		for(size_t j = 0; j < neighbours.size(); j++)
		{
			std::pair<double, double> angles = v->find_opposite_angles(neighbours[j]);
			if(angles.first >= 0.0 && angles.second >= 0.0)
			{
				// calculate contribution to matrix entries

				double contribution = 1.0/tan(angles.first) + 1.0/tan(angles.second);

				K(cur_id, cur_id)			+= contribution;
				K(cur_id, neighbours[j]->get_id())	-= contribution;
			}
		}
	}

	// Scale the ith row of the matrix by the Voronoi area around the ith
	// vertex; this works because the _first_ iterator of all matrix types
	// is dense, whereas the second iterator is sparse in this case

	size_t i = 0;
	for(compressed_matrix<double>::iterator1 it1 = K.begin1(); it1 != K.end1(); it1++)
	{
		double area = input_mesh.get_vertex(i)->calc_ring_area();
		if(area < 2*std::numeric_limits<double>::epsilon())
		{
			// skip on error or upon encountering a Voronoi area
			// that is too small
			i++;
			continue;
		}

		for(compressed_matrix<double>::iterator2 it2 = it1.begin(); it2 != it1.end(); it2++)
			(*it2) /= 4.0*area;

		i++;
	}

	return(K);
}

/*!
*	Sets current value for the delta parameter (used per timestep).
*
*	@param delta New value for delta parameter
*/

void CurvatureFlow::set_delta(double delta)
{
	this->dt = delta;
}

/*!
*	@returns Current value of delta parameter (used per timestep).
*/

double CurvatureFlow::get_delta()
{
	return(dt);
}

/*!
*	Sets current value for the number of steps the algorithm is supposed to
*	perform.
*
*	@param num_steps New value for the number of steps
*/

void CurvatureFlow::set_steps(size_t num_steps)
{
	this->num_steps = num_steps;
}

/*!
*	@returns Current number of steps the algorithm performs when being
*	applied to a mesh.
*/

size_t CurvatureFlow::get_steps()
{
	return(num_steps);
}

} // end of namespace "psalm"


#include <PCP/Common/Option.h>
#include <PCP/Common/Log.h>
#include <PCP/Common/Progress.h>
#include <PCP/Common/Colors.h>

#include <PCP/Geometry/Geometry.h>
#include <PCP/Geometry/Loader.h>

#include <PCP/SpacePartitioning/KdTree.h>

#include <Eigen/Eigenvalues>

#include <Ponca/Fitting>

using namespace pcp;

using WeightKernel = Ponca::SmoothWeightKernel<Scalar>;
using WeightFunc   = Ponca::DistWeightFunc<ConstPoint, WeightKernel>;
using SphereFit    = Ponca::Basket<ConstPoint, WeightFunc, Ponca::OrientedSphereFit,
                                                           Ponca::OrientedSphereScaleSpaceDer,
                                                           Ponca::GLSParam,
                                                           Ponca::GLSDer,
                                                           Ponca::GLSGeomVar>;

void colorize_and_save(Geometry& g, const std::vector<Scalar>& feature, const std::string& name, const std::string& path);

int main(int argc, char *argv[])
{
    Option opt(argc, argv);
    const String in_input    = opt.get_string("input",  "i").set_required();
    const String in_output   = opt.get_string("output", "o").set_default(".");
    const Scalar in_scale    = opt.get_float( "scale"      ).set_default(0.01);

    bool ok = opt.ok();
    if(!ok) return 1;
    info() << opt;

    Geometry g;
    ok = Loader::Load(in_input, g);
    if(!ok) return 1;
    PCP_ASSERT(g.has_normals());
    const int point_count = g.size();

    g.build_kdtree();

    const auto aabb = g.aabb();
    const auto aabb_diag = aabb.diagonal().norm();
    const auto radius = in_scale * aabb_diag;

    auto prog = Progress(point_count);

    std::vector<Scalar> feature_uc(point_count);
    std::vector<Scalar> feature_ul(point_count);
    std::vector<Scalar> feature_uq(point_count);
    std::vector<Scalar> feature_gv(point_count);

    #pragma omp parallel for
    for(int i=0; i<point_count; ++i)
    {
        SphereFit fit;
        fit.setWeightFunc(WeightFunc(radius));
        fit.init(g[i]);
        for(int j : g.kdtree().range_neighbors(i, radius))
        {
            fit.addNeighbor(g.at(j));
        }

        const auto status = fit.finalize();

        if(status == Ponca::FIT_RESULT::STABLE)
        {
            feature_uc[i] = fit.m_uc / radius;
            feature_ul[i] = 1 - fit.m_ul.norm();
            feature_uq[i] = fit.m_uq * radius;
            feature_gv[i] = std::sqrt(fit.geomVar());
        }
        else
        {
            warning() << "Unstable fit at point " << i;
        }

        ++prog;
    }

    g.request_colors();

    colorize_and_save(g, feature_uc, "uc", in_output);
    colorize_and_save(g, feature_ul, "ul", in_output);
    colorize_and_save(g, feature_uq, "uq", in_output);
    colorize_and_save(g, feature_gv, "gv", in_output);

    return 0;
}

////////////////////////////////////////////////////////////////////////////////

void colorize_and_save(Geometry& g, const std::vector<Scalar>& feature, const std::string& name, const std::string& path)
{
    const int point_count = g.size();

    const auto colormap = BiColormap::Jet();

    limited_priority_queue<Scalar, std::greater<Scalar>> q(0.10 * point_count);

    Scalar min = +std::numeric_limits<Scalar>::max();
    Scalar max = -std::numeric_limits<Scalar>::max();
    Scalar mean = 0;
    for(int i=0; i<point_count; ++i)
    {
        q.push(std::abs(feature[i]));
        min = std::min(min, feature[i]);
        max = std::max(max, feature[i]);
        mean += feature[i];
    }
    mean /= point_count;

    const Scalar limit = q.bottom();

    info() << name << " in (" << min << "," << max << ") mean=" << mean << " limit=" << limit;

    #pragma omp parallel for
    for(int i=0; i<point_count; ++i)
    {
        g.color(i) = colormap(feature[i], limit);
    }

    Loader::Save(path+"/"+name+".ply", g, false);
}


//  Boost GCD & LCM common_factor.hpp test program  --------------------------//

//  (C) Copyright Daryle Walker 2001.  Permission to copy, use, modify, sell
//  and distribute this software is granted provided this copyright
//  notice appears in all copies.  This software is provided "as is" without
//  express or implied warranty, and with no claim as to its suitability for
//  any purpose.

//  See http://www.boost.org for most recent version including documentation.

//  Revision History
//  07 Nov 2001  Initial version (Daryle Walker)

#define  BOOST_INCLUDE_MAIN

#include <boost/config.hpp>              // for BOOST_MSVC
#include <boost/cstdlib.hpp>             // for boost::exit_success
#include <boost/math/common_factor.hpp>  // for boost::math::gcd, etc.
#include <boost/test/test_tools.hpp>     // for main, BOOST_TEST

#include <iostream>  // for std::cout (std::endl indirectly)

// Control to determine what kind of built-in integers are used
#ifndef CONTROL_INT_TYPE
#define CONTROL_INT_TYPE  int
#endif

// Main testing function
int
test_main
(
    int         ,   // "argc" is unused
    char *      []  // "argv" is unused
)
{
    using std::cout;
    using std::endl;

#ifndef BOOST_MSVC
    using boost::math::gcd;
    using boost::math::static_gcd;
    using boost::math::lcm;
    using boost::math::static_lcm;
#else
    using namespace boost::math;
#endif

    typedef CONTROL_INT_TYPE  int_type;
    typedef unsigned CONTROL_INT_TYPE uint_type;

    // GCD tests
    cout << "Doing tests on gcd." << endl;

    BOOST_TEST( gcd<int_type>(  1,  -1) ==  1 );
    BOOST_TEST( gcd<int_type>( -1,   1) ==  1 );
    BOOST_TEST( gcd<int_type>(  1,   1) ==  1 );
    BOOST_TEST( gcd<int_type>( -1,  -1) ==  1 );
    BOOST_TEST( gcd<int_type>(  0,   0) ==  0 );
    BOOST_TEST( gcd<int_type>(  7,   0) ==  7 );
    BOOST_TEST( gcd<int_type>(  0,   9) ==  9 );
    BOOST_TEST( gcd<int_type>( -7,   0) ==  7 );
    BOOST_TEST( gcd<int_type>(  0,  -9) ==  9 );
    BOOST_TEST( gcd<int_type>( 42,  30) ==  6 );
    BOOST_TEST( gcd<int_type>(  6,  -9) ==  3 );
    BOOST_TEST( gcd<int_type>(-10, -10) == 10 );
    BOOST_TEST( gcd<int_type>(-25, -10) ==  5 );
    BOOST_TEST( gcd<int_type>(  3,   7) ==  1 );
    BOOST_TEST( gcd<int_type>(  8,   9) ==  1 );
    BOOST_TEST( gcd<int_type>(  7,  49) ==  7 );

    // GCD tests
    cout << "Doing tests on unsigned-gcd." << endl;

    BOOST_TEST( gcd<uint_type>(  1u,   1u) ==  1u );
    BOOST_TEST( gcd<uint_type>(  0u,   0u) ==  0u );
    BOOST_TEST( gcd<uint_type>(  7u,   0u) ==  7u );
    BOOST_TEST( gcd<uint_type>(  0u,   9u) ==  9u );
    BOOST_TEST( gcd<uint_type>( 42u,  30u) ==  6u );
    BOOST_TEST( gcd<uint_type>(  3u,   7u) ==  1u );
    BOOST_TEST( gcd<uint_type>(  8u,   9u) ==  1u );
    BOOST_TEST( gcd<uint_type>(  7u,  49u) ==  7u );

    cout << "Doing tests on static_gcd." << endl;

    BOOST_TEST( (static_gcd< 1,  1>::value) == 1 );
    BOOST_TEST( (static_gcd< 0,  0>::value) == 0 );
    BOOST_TEST( (static_gcd< 7,  0>::value) == 7 );
    BOOST_TEST( (static_gcd< 0,  9>::value) == 9 );
    BOOST_TEST( (static_gcd<42, 30>::value) == 6 );
    BOOST_TEST( (static_gcd< 3,  7>::value) == 1 );
    BOOST_TEST( (static_gcd< 8,  9>::value) == 1 );
    BOOST_TEST( (static_gcd< 7, 49>::value) == 7 );

    // LCM tests
    cout << "Doing tests on lcm." << endl;

    BOOST_TEST( lcm<int_type>(  1,  -1) ==  1 );
    BOOST_TEST( lcm<int_type>( -1,   1) ==  1 );
    BOOST_TEST( lcm<int_type>(  1,   1) ==  1 );
    BOOST_TEST( lcm<int_type>( -1,  -1) ==  1 );
    BOOST_TEST( lcm<int_type>(  0,   0) ==  0 );
    BOOST_TEST( lcm<int_type>(  6,   0) ==  0 );
    BOOST_TEST( lcm<int_type>(  0,   7) ==  0 );
    BOOST_TEST( lcm<int_type>( -5,   0) ==  0 );
    BOOST_TEST( lcm<int_type>(  0,  -4) ==  0 );
    BOOST_TEST( lcm<int_type>( 18,  30) == 90 );
    BOOST_TEST( lcm<int_type>( -6,   9) == 18 );
    BOOST_TEST( lcm<int_type>(-10, -10) == 10 );
    BOOST_TEST( lcm<int_type>( 25, -10) == 50 );
    BOOST_TEST( lcm<int_type>(  3,   7) == 21 );
    BOOST_TEST( lcm<int_type>(  8,   9) == 72 );
    BOOST_TEST( lcm<int_type>(  7,  49) == 49 );

    cout << "Doing tests on unsigned-lcm." << endl;

    BOOST_TEST( lcm<uint_type>(  1u,   1u) ==  1u );
    BOOST_TEST( lcm<uint_type>(  0u,   0u) ==  0u );
    BOOST_TEST( lcm<uint_type>(  6u,   0u) ==  0u );
    BOOST_TEST( lcm<uint_type>(  0u,   7u) ==  0u );
    BOOST_TEST( lcm<uint_type>( 18u,  30u) == 90u );
    BOOST_TEST( lcm<uint_type>(  3u,   7u) == 21u );
    BOOST_TEST( lcm<uint_type>(  8u,   9u) == 72u );
    BOOST_TEST( lcm<uint_type>(  7u,  49u) == 49u );

    cout << "Doing tests on static_lcm." << endl;

    BOOST_TEST( (static_lcm< 1,  1>::value) ==  1 );
    BOOST_TEST( (static_lcm< 0,  0>::value) ==  0 );
    BOOST_TEST( (static_lcm< 6,  0>::value) ==  0 );
    BOOST_TEST( (static_lcm< 0,  7>::value) ==  0 );
    BOOST_TEST( (static_lcm<18, 30>::value) == 90 );
    BOOST_TEST( (static_lcm< 3,  7>::value) == 21 );
    BOOST_TEST( (static_lcm< 8,  9>::value) == 72 );
    BOOST_TEST( (static_lcm< 7, 49>::value) == 49 );

    return boost::exit_success;
}


///\author Ethan Knox
///\date 10/6/2020.

#ifndef PURDUE_PHYS_580_POINCARE
#define PURDUE_PHYS_580_POINCARE
#define _USE_MATH_DEFINES
#endif //PURDUE_PHYS_580_POINCARE

#include <cmath>
#include <iostream>
#include <fstream>
#include <string>
#include <vector>
#include <limits>
#include <iomanip>
#include <functional>

#include <boost/program_options.hpp>
#include <boost/numeric/odeint.hpp>

using namespace std;
using namespace std::placeholders;
namespace opt = boost::program_options;

typedef std::vector<double> state_t;
typedef boost::numeric::odeint::runge_kutta4<state_t> rk4;

int main(int argc, const char* argv[])
{
    double A, x_0, dx_0, Q, wd, sec_freq;
    long n_pnts, n_wait, pnt_density;

    opt::options_description params("Simulation Parameters");
    params.add_options()
            ("help,h", "Show Usage")
            (     "n_wait",opt::value<long>  (     &n_wait)->default_value(    100), "Number of Cycles Ignored For Transients To Disappear")
            (     "n_pnts",opt::value<long>  (     &n_pnts)->default_value(   10000), "Total Number of Cycles")
            ("pnt_density",opt::value<long>  (&pnt_density)->default_value(   1000), "Solution Accuracy (dt)")
            (          "A",opt::value<double>(          &A)->default_value(    1.35), "Amplitude")
            (        "x_0",opt::value<double>(        &x_0)->default_value(    0.2), "Initial Angle")
            (        "v_0",opt::value<double>(       &dx_0)->default_value(    0.0), "Initial Angular Frequency")
            (          "Q",opt::value<double>(          &Q)->default_value(    0.5), "Quality Factor")
            (        "w_d",opt::value<double>(         &wd)->default_value(2.0/3.0), "Driving Frequency")
            (   "sec_freq",opt::value<double>(   &sec_freq)->default_value(2.0/3.0), "Sectioning Frequency");

    opt::variables_map vm;
    opt::store(opt::parse_command_line(argc, argv, params), vm);

    // Output help message if requested from the commandline
    if (vm.count("help")) {
        std::cout << params << std::endl;
        return 1;
    } // Necessary to retrieve the values from 'program_options'
    else {
        opt::notify(vm);
    }

    // Constants
    const long double dt = 2.0 * M_PI / (sec_freq * pnt_density);
    const long double t_i = 2.0 * M_PI / sec_freq;
    const long double t_f = (2.0 * M_PI * n_pnts) / sec_freq;
    const long double N = (n_pnts - 1) * pnt_density;

    // Initial Condition
    state_t x{x_0, dx_0};
    state_t dx_dt(2);

    // Data Series
    state_t x_series;
    state_t dx_series;
    state_t t_series;
    x_series.reserve(N);
    dx_series.reserve(N);
    t_series.reserve(N);

    // Data Recorder
    auto observer = [&](const state_t& x, const double t) {
        x_series.push_back(x.front());
        dx_series.push_back(x.back());
        t_series.push_back(t);
    };

    cout << "Running Simulation...\n";
    ofstream file("../data/poincare.dat");
    file << "# t \\theta \\frac{d\\theta}{dt}\n";
    file << setprecision(numeric_limits<long double>::digits10 + 1);

    // ODE System
    std::function<void(const state_t&, state_t&, double)> sys = [&](const state_t& x, state_t& dx_dt, const double t) {
        dx_dt.front() = x.back();
        dx_dt.back() = -sin(x.front()) - Q * x.back() + A * sin(wd * t);
    };

    // Integrate
    boost::numeric::odeint::integrate_n_steps(rk4(), sys, x, t_i, dt, N, observer);

    // Restrict the domain to [-pi, pi]
    for(auto &theta : x_series){
        theta = fmod(theta + M_PI, 2.0 * M_PI) - M_PI;
    }

    // Write data to file
    for(size_t i=0; i<x_series.size(); i++){
        if((i > pnt_density) && (i / pnt_density > n_wait) && (i % pnt_density == 0)){
            file << std::scientific << t_series.at(i) << " " << x_series.at(i) << " " << dx_series.at(i) << "\n";
        }
    }

    cout << "Done.\n\n";
    return 0;
}


//            Copyright Daniel Trebbien 2010.
// Distributed under the Boost Software License, Version 1.0.
//   (See accompanying file LICENSE_1_0.txt or the copy at
//         http://www.boost.org/LICENSE_1_0.txt)

#include <fstream>
#include <iostream>
#include <map>
#include <vector>
#include <string>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/connected_components.hpp>
#include <boost/graph/exception.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/read_dimacs.hpp>
#include <boost/graph/stoer_wagner_min_cut.hpp>
#include <boost/graph/property_maps/constant_property_map.hpp>
#include <boost/property_map/property_map.hpp>
#include <boost/test/unit_test.hpp>
#include <boost/tuple/tuple.hpp>

typedef boost::adjacency_list<boost::vecS, boost::vecS, boost::undirectedS, boost::no_property, boost::property<boost::edge_weight_t, int> > undirected_graph;
typedef boost::property_map<undirected_graph, boost::edge_weight_t>::type weight_map_type;
typedef boost::property_traits<weight_map_type>::value_type weight_type;

typedef boost::adjacency_list<boost::vecS, boost::vecS, boost::undirectedS> undirected_unweighted_graph;

std::string test_dir;

boost::unit_test::test_suite* init_unit_test_suite( int argc, char* argv[] ) {
  if (argc != 2) {
    std::cerr << "Usage: " << argv[0] << " path-to-libs-graph-test" << std::endl;
    throw boost::unit_test::framework::setup_error("Invalid command line arguments");
  }
  test_dir = argv[1];
  return 0;
}

struct edge_t
{
  unsigned long first;
  unsigned long second;
};

// the example from Stoer & Wagner (1997)
BOOST_AUTO_TEST_CASE(test0)
{
  typedef boost::graph_traits<undirected_graph>::vertex_descriptor vertex_descriptor;
  typedef boost::graph_traits<undirected_graph>::edge_descriptor edge_descriptor;

  edge_t edges[] = {{0, 1}, {1, 2}, {2, 3},
    {0, 4}, {1, 4}, {1, 5}, {2, 6}, {3, 6}, {3, 7}, {4, 5}, {5, 6}, {6, 7}};
  weight_type ws[] = {2, 3, 4, 3, 2, 2, 2, 2, 2, 3, 1, 3};
  undirected_graph g(edges, edges + 12, ws, 8, 12);

  weight_map_type weights = get(boost::edge_weight, g);
  std::map<int, bool> parity;
  boost::associative_property_map<std::map<int, bool> > parities(parity);
  int w = boost::stoer_wagner_min_cut(g, weights, boost::parity_map(parities));
  BOOST_CHECK_EQUAL(w, 4);
  const bool parity0 = get(parities, 0);
  BOOST_CHECK_EQUAL(parity0, get(parities, 1));
  BOOST_CHECK_EQUAL(parity0, get(parities, 4));
  BOOST_CHECK_EQUAL(parity0, get(parities, 5));
  const bool parity2 = get(parities, 2);
  BOOST_CHECK_NE(parity0, parity2);
  BOOST_CHECK_EQUAL(parity2, get(parities, 3));
  BOOST_CHECK_EQUAL(parity2, get(parities, 6));
  BOOST_CHECK_EQUAL(parity2, get(parities, 7));
}

BOOST_AUTO_TEST_CASE(test1)
{
  { // if only one vertex, can't run `boost::stoer_wagner_min_cut`
    typedef boost::graph_traits<undirected_graph>::vertex_descriptor vertex_descriptor;
    typedef boost::graph_traits<undirected_graph>::edge_descriptor edge_descriptor;

    undirected_graph g;
    add_vertex(g);

    BOOST_CHECK_THROW(boost::stoer_wagner_min_cut(g, get(boost::edge_weight, g)), boost::bad_graph);
  }{ // three vertices with one multi-edge
    typedef boost::graph_traits<undirected_graph>::vertex_descriptor vertex_descriptor;
    typedef boost::graph_traits<undirected_graph>::edge_descriptor edge_descriptor;

    edge_t edges[] = {{0, 1}, {1, 2}, {1, 2}, {2, 0}};
    weight_type ws[] = {3, 1, 1, 1};
    undirected_graph g(edges, edges + 4, ws, 3, 4);

    weight_map_type weights = get(boost::edge_weight, g);
    std::map<int, bool> parity;
    boost::associative_property_map<std::map<int, bool> > parities(parity);
    std::map<vertex_descriptor, vertex_descriptor> assignment;
    boost::associative_property_map<std::map<vertex_descriptor, vertex_descriptor> > assignments(assignment);
    int w = boost::stoer_wagner_min_cut(g, weights, boost::parity_map(parities).vertex_assignment_map(assignments));
    BOOST_CHECK_EQUAL(w, 3);
    const bool parity2 = get(parities, 2),
      parity0 = get(parities, 0);
    BOOST_CHECK_NE(parity2, parity0);
    BOOST_CHECK_EQUAL(parity0, get(parities, 1));
  }
}

// example by Daniel Trebbien
BOOST_AUTO_TEST_CASE(test2)
{
  typedef boost::graph_traits<undirected_graph>::vertex_descriptor vertex_descriptor;
  typedef boost::graph_traits<undirected_graph>::edge_descriptor edge_descriptor;

  edge_t edges[] = {{5, 2}, {0, 6}, {5, 6},
    {3, 1}, {0, 1}, {6, 3}, {4, 6}, {2, 4}, {5, 3}};
  weight_type ws[] = {1, 3, 4, 6, 4, 1, 2, 5, 2};
  undirected_graph g(edges, edges + 9, ws, 7, 9);

  std::map<int, bool> parity;
  boost::associative_property_map<std::map<int, bool> > parities(parity);
  int w = boost::stoer_wagner_min_cut(g, get(boost::edge_weight, g), boost::parity_map(parities));
  BOOST_CHECK_EQUAL(w, 3);
  const bool parity2 = get(parities, 2);
  BOOST_CHECK_EQUAL(parity2, get(parities, 4));
  const bool parity5 = get(parities, 5);
  BOOST_CHECK_NE(parity2, parity5);
  BOOST_CHECK_EQUAL(parity5, get(parities, 3));
  BOOST_CHECK_EQUAL(parity5, get(parities, 6));
  BOOST_CHECK_EQUAL(parity5, get(parities, 1));
  BOOST_CHECK_EQUAL(parity5, get(parities, 0));
}

// example by Daniel Trebbien
BOOST_AUTO_TEST_CASE(test3)
{
  typedef boost::graph_traits<undirected_graph>::vertex_descriptor vertex_descriptor;
  typedef boost::graph_traits<undirected_graph>::edge_descriptor edge_descriptor;

  edge_t edges[] = {{3, 4}, {3, 6}, {3, 5}, {0, 4}, {0, 1}, {0, 6}, {0, 7},
    {0, 5}, {0, 2}, {4, 1}, {1, 6}, {1, 5}, {6, 7}, {7, 5}, {5, 2}, {3, 4}};
  weight_type ws[] = {0, 3, 1, 3, 1, 2, 6, 1, 8, 1, 1, 80, 2, 1, 1, 4};
  undirected_graph g(edges, edges + 16, ws, 8, 16);

  weight_map_type weights = get(boost::edge_weight, g);
  std::map<int, bool> parity;
  boost::associative_property_map<std::map<int, bool> > parities(parity);
  int w = boost::stoer_wagner_min_cut(g, weights, boost::parity_map(parities));
  BOOST_CHECK_EQUAL(w, 7);
  const bool parity1 = get(parities, 1);
  BOOST_CHECK_EQUAL(parity1, get(parities, 5));
  const bool parity0 = get(parities, 0);
  BOOST_CHECK_NE(parity1, parity0);
  BOOST_CHECK_EQUAL(parity0, get(parities, 2));
  BOOST_CHECK_EQUAL(parity0, get(parities, 3));
  BOOST_CHECK_EQUAL(parity0, get(parities, 4));
  BOOST_CHECK_EQUAL(parity0, get(parities, 6));
  BOOST_CHECK_EQUAL(parity0, get(parities, 7));
}

BOOST_AUTO_TEST_CASE(test4)
{
  typedef boost::graph_traits<undirected_unweighted_graph>::vertex_descriptor vertex_descriptor;
  typedef boost::graph_traits<undirected_unweighted_graph>::edge_descriptor edge_descriptor;

  edge_t edges[] = {{0, 1}, {1, 2}, {2, 3},
    {0, 4}, {1, 4}, {1, 5}, {2, 6}, {3, 6}, {3, 7}, {4, 5}, {5, 6}, {6, 7},
    {0, 4}, {6, 7}};
  undirected_unweighted_graph g(edges, edges + 14, 8);

  std::map<vertex_descriptor, bool> parity;
  boost::associative_property_map<std::map<vertex_descriptor, bool> > parities(parity);
  std::map<vertex_descriptor, vertex_descriptor> assignment;
  boost::associative_property_map<std::map<vertex_descriptor, vertex_descriptor> > assignments(assignment);
  int w = boost::stoer_wagner_min_cut(g, boost::make_constant_property<edge_descriptor>(weight_type(1)), boost::vertex_assignment_map(assignments).parity_map(parities));
  BOOST_CHECK_EQUAL(w, 2);
  const bool parity0 = get(parities, 0);
  BOOST_CHECK_EQUAL(parity0, get(parities, 1));
  BOOST_CHECK_EQUAL(parity0, get(parities, 4));
  BOOST_CHECK_EQUAL(parity0, get(parities, 5));
  const bool parity2 = get(parities, 2);
  BOOST_CHECK_NE(parity0, parity2);
  BOOST_CHECK_EQUAL(parity2, get(parities, 3));
  BOOST_CHECK_EQUAL(parity2, get(parities, 6));
  BOOST_CHECK_EQUAL(parity2, get(parities, 7));
}

// The input for the `test_prgen` family of tests comes from a program, named
// `prgen`, that comes with a package of min-cut solvers by Chandra Chekuri,
// Andrew Goldberg, David Karger, Matthew Levine, and Cliff Stein. `prgen` was
// used to generate input graphs and the solvers were used to verify the return
// value of `boost::stoer_wagner_min_cut` on the input graphs.
//
// http://www.columbia.edu/~cs2035/code.html
//
// Note that it is somewhat more difficult to verify the parities because
// "`prgen` graphs" often have several min-cuts. This is why only the cut
// weight of the min-cut is verified.

// 3 min-cuts
BOOST_AUTO_TEST_CASE(test_prgen_20_70_2)
{
  typedef boost::graph_traits<undirected_graph>::vertex_descriptor vertex_descriptor;
  typedef boost::graph_traits<undirected_graph>::edge_descriptor edge_descriptor;

  std::ifstream ifs((test_dir + "/prgen_input_graphs/prgen_20_70_2.net").c_str());
  undirected_graph g;
  boost::read_dimacs_min_cut(g, get(boost::edge_weight, g), boost::dummy_property_map(), ifs);

  std::map<vertex_descriptor, std::size_t> component;
  boost::associative_property_map<std::map<vertex_descriptor, std::size_t> > components(component);
  BOOST_CHECK_EQUAL(boost::connected_components(g, components), 1U); // verify the connectedness assumption

  BOOST_AUTO(distances, (boost::make_shared_array_property_map(num_vertices(g), weight_type(0), get(boost::vertex_index, g))));
  typedef std::vector<vertex_descriptor>::size_type index_in_heap_type;
  BOOST_AUTO(indicesInHeap, (boost::make_shared_array_property_map(num_vertices(g), index_in_heap_type(-1), get(boost::vertex_index, g))));
  boost::d_ary_heap_indirect<vertex_descriptor, 22, BOOST_TYPEOF(indicesInHeap), BOOST_TYPEOF(distances), std::greater<weight_type> > pq(distances, indicesInHeap);

  int w = boost::stoer_wagner_min_cut(g, get(boost::edge_weight, g), boost::max_priority_queue(pq));
  BOOST_CHECK_EQUAL(w, 3407);
}

// 7 min-cuts
BOOST_AUTO_TEST_CASE(test_prgen_50_40_2)
{
  typedef boost::graph_traits<undirected_graph>::vertex_descriptor vertex_descriptor;
  typedef boost::graph_traits<undirected_graph>::edge_descriptor edge_descriptor;

  std::ifstream ifs((test_dir + "/prgen_input_graphs/prgen_50_40_2.net").c_str());
  undirected_graph g;
  boost::read_dimacs_min_cut(g, get(boost::edge_weight, g), boost::dummy_property_map(), ifs);

  std::map<vertex_descriptor, std::size_t> component;
  boost::associative_property_map<std::map<vertex_descriptor, std::size_t> > components(component);
  BOOST_CHECK_EQUAL(boost::connected_components(g, components), 1U); // verify the connectedness assumption

  int w = boost::stoer_wagner_min_cut(g, get(boost::edge_weight, g));
  BOOST_CHECK_EQUAL(w, 10056);
}

// 6 min-cuts
BOOST_AUTO_TEST_CASE(test_prgen_50_70_2)
{
  typedef boost::graph_traits<undirected_graph>::vertex_descriptor vertex_descriptor;
  typedef boost::graph_traits<undirected_graph>::edge_descriptor edge_descriptor;

  std::ifstream ifs((test_dir + "/prgen_input_graphs/prgen_50_70_2.net").c_str());
  undirected_graph g;
  boost::read_dimacs_min_cut(g, get(boost::edge_weight, g), boost::dummy_property_map(), ifs);

  std::map<vertex_descriptor, std::size_t> component;
  boost::associative_property_map<std::map<vertex_descriptor, std::size_t> > components(component);
  BOOST_CHECK_EQUAL(boost::connected_components(g, components), 1U); // verify the connectedness assumption

  int w = boost::stoer_wagner_min_cut(g, get(boost::edge_weight, g));
  BOOST_CHECK_EQUAL(w, 21755);
}


//EuropeanOption.hpp
//
//Modification date: 6/17/15

#ifndef EuropeanOption_hpp
#define EuropeanOption_hpp

#include "Option.hpp"
#include "GlobalFunctions.hpp"
#include <boost/math/distributions/normal.hpp>
#include <vector>

class EuropeanOption : public Option
{
protected:
	double T;			//time till maturity

public:
	//Constructors
	EuropeanOption() : Option(), T(0.25)	{	}
	EuropeanOption(double new_S, double new_K, double new_T, double new_r, double new_sig) : Option(new_S,new_K,new_r,new_sig), T(new_T)	{	}		//default b=r Black and Scholes stock option model (1973)
	EuropeanOption(double new_S, double new_K, double new_T, double new_r, double new_sig, double new_b) : Option(new_S, new_K, new_r, new_sig, new_b), T(new_T)	{	}
	EuropeanOption(const EuropeanOption& source) : Option(source), T(source.T)	{	}

	//Destructor
	virtual ~EuropeanOption() {}

	//Operator overloads
	EuropeanOption& operator = (const EuropeanOption& source);

	//Getters
	double get_T() { return T; }

	virtual void Print()const = 0;

	//Setters
	void set_T(double new_T) { T = new_T; }

	//Pricers (pure virtual functions)
	virtual double Price()const = 0;
	virtual double PricePCP(double PC)const = 0;
	virtual double Price_S(double S1) = 0;
	virtual double Price_K(double K1) = 0;
	virtual double Price_T(double T1) = 0;
	virtual double Price_r(double r1) = 0;
	virtual double Price_sig(double sig1) = 0;
	virtual double Price_b(double b1) = 0;

	//Checkers
	bool PutCallParity(double C, double P)const;

	//Greeks
	virtual double Delta() const = 0;
	double Gamma() { return GammaGF(S, K, T, r, sig, b); }		//Gamma is the same for put and call options
	double Gamma(double h) { return GammaGF(S, h, K, T, r, sig, b); }
	double Vega() { return VegaGF(S, K, T, r, sig, b); }		//Vega is the same for put and call options, no need to define it in derived classes
	virtual double Theta() const = 0;
	virtual double Rho() const = 0;

};

class EuropeanCallOption : public EuropeanOption
{
public:
	//Constructors
	EuropeanCallOption() : EuropeanOption() {}
	EuropeanCallOption(double new_S, double new_K, double new_T, double new_r, double new_sig) : EuropeanOption(new_S, new_K, new_T, new_r, new_sig) {}
	EuropeanCallOption(double new_S, double new_K, double new_T, double new_r, double new_sig, double new_b) : EuropeanOption(new_S, new_K, new_T, new_r, new_sig, new_b) {}
	EuropeanCallOption(const EuropeanOption& source) : EuropeanOption(source) {}

	//Destructor
	virtual ~EuropeanCallOption() {}

	EuropeanOption& operator = (const EuropeanOption& source);

	void Print()const;

	//Pricers
	double Price()const	{	return CallPrice(S, K, T, r, sig, b);	}
	double Price_S(double S1)	{ return CallPrice(S1, K, T, r, sig, b); }
	double Price_K(double K1)	{ return CallPrice(S, K1, T, r, sig, b); }
	double Price_T(double T1)	{ return CallPrice(S, K, T1, r, sig, b); }
	double Price_r(double r1)	{ return CallPrice(S, K, T, r1, sig, b); }
	double Price_sig(double sig1)	{ return CallPrice(S, K, T, r, sig1, b); }
	double Price_b(double b1)	{ return CallPrice(S, K, T, r, sig, b1); }
	double PricePCP(double P)const	{ return CallPricePCP(P, S, K, T, r, b); }

	//Pricing function that returns vector. Used in combination with function pointer for better versatility
	typedef double(EuropeanCallOption::*FunctionPointer)(double);
	std::vector<double> Price(double LowerLimit, double UpperLimit, int Num, FunctionPointer Ptr );

	//Greeks
	double Delta()const { return CallDelta( S, K, T, r, sig, b); }
	double Delta_S(double S1) { return CallDelta(S1, K, T, r, sig, b); }		//function to override spot price
	double Theta() const { return CallTheta(S, K, T, r, sig, b); }
	double Rho() const { return CallRho(S, K, T, r, sig, b); }
	double Delta(double h)const { return CallDelta(S, h, K, T, r, sig, b); }
	//double Gamma() { return GammaGF(S, K, T, r, sig, b); }
	//double Gamma(double h) { return GammaGF(S, h, K, T, r, sig, b); }

};

class EuropeanPutOption : public EuropeanOption
{
public:
	//Constructors
	EuropeanPutOption() : EuropeanOption() {}
	EuropeanPutOption(double new_S, double new_K, double new_T, double new_r, double new_sig) : EuropeanOption(new_S, new_K, new_T, new_r, new_sig) {}
	EuropeanPutOption(double new_S, double new_K, double new_T, double new_r, double new_sig, double new_b) : EuropeanOption(new_S, new_K, new_T, new_r, new_sig, new_b) {}
	EuropeanPutOption(const EuropeanOption& source) : EuropeanOption(source) {}

	//Destructor
	virtual ~EuropeanPutOption() {}

	EuropeanOption& operator = (const EuropeanOption& source);

	void Print()const;

	//Pricers
	double Price()const		{	return PutPrice(S, K, T, r, sig, b);	}
	double PricePCP(double C)const		{	return PutPricePCP(C, S, K, T, r, b);	}
	double Price_S(double S1)	{ return PutPrice(S1, K, T, r, sig, b); }
	double Price_K(double K1)	{ return PutPrice(S, K1, T, r, sig, b); }
	double Price_T(double T1)	{ return PutPrice(S, K, T1, r, sig, b); }
	double Price_r(double r1)	{ return PutPrice(S, K, T, r1, sig, b); }
	double Price_sig(double sig1)	{ return PutPrice(S, K, T, r, sig1, b); }
	double Price_b(double b1)	{ return PutPrice(S, K, T, r, sig, b1); }

	//Pricing function that returns vector. Used in combination with function pointer for better versatility
	typedef double(EuropeanPutOption::*FunctionPointer)(double);
	std::vector<double> Price(double LowerLimit, double UpperLimit, int Num,const FunctionPointer Ptr);

	//Greeks
	double Delta()const { return PutDelta(S, K, T, r, sig, b); }
	double Delta_S(double S1) { return PutDelta(S1, K, T, r, sig, b); }																		//function to override spot price
	double Theta() const { return PutTheta(S, K, T, r, sig, b); }
	double Rho() const { return PutRho(S, K, T, r, sig, b); }
	double Delta(double h)const { return PutDelta(S, h, K, T, r, sig, b); }

};

#endif


//==================================================================================================
/**
  EVE - Expressive Vector Engine
  Copyright : EVE Contributors & Maintainers
  SPDX-License-Identifier: MIT
**/
//==================================================================================================
#include "test.hpp"
#include <eve/module/bessel.hpp>
#include <boost/math/special_functions/bessel.hpp>
#include <boost/math/special_functions/bessel_prime.hpp>
#include <eve/detail/diff_div.hpp>

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

 EVE_TEST( "Check behavior of sph_bessel_y0 on wide"
        , eve::test::simd::ieee_reals
        , eve::test::generate(eve::test::randoms(0.0, 5.5),
                              eve::test::randoms(5.5, 9.5),
                              eve::test::randoms(9.5, 60.0))
         )
   <typename T>(T const& a0, T const& a1, T const& a2)
{
  using v_t = eve::element_type_t<T>;
  auto eve__sph_bessel_y0 =  [](auto x) { return eve::sph_bessel_y0(x); };
  auto std__sph_bessel_y0 =  [](auto x)->v_t { return boost::math::sph_neumann(0u, double(x)); };

  if constexpr( eve::platform::supports_invalids )
  {
    TTS_ULP_EQUAL(eve__sph_bessel_y0(eve::inf(eve::as<v_t>())), eve::zero(eve::as<v_t>()), 0);
    TTS_ULP_EQUAL(eve__sph_bessel_y0(eve::nan(eve::as<v_t>())), eve::nan(eve::as<v_t>()), 0);
    TTS_ULP_EQUAL(eve__sph_bessel_y0(eve::inf(eve::as< T>())),  eve::zero(eve::as< T>()), 0);
    TTS_ULP_EQUAL(eve__sph_bessel_y0(eve::nan(eve::as< T>())), eve::nan(eve::as< T>()), 0);
  }
  TTS_ULP_EQUAL(eve__sph_bessel_y0(v_t(500)), std__sph_bessel_y0(v_t(500)), 6.0);
  TTS_ULP_EQUAL(eve__sph_bessel_y0(v_t(10)), std__sph_bessel_y0(v_t(10))  , 6.0);
  TTS_ULP_EQUAL(eve__sph_bessel_y0(v_t(5)),  std__sph_bessel_y0(v_t(5))   , 6.0);
  TTS_ULP_EQUAL(eve__sph_bessel_y0(v_t(2)),  std__sph_bessel_y0(v_t(2))   , 6.0);
  TTS_ULP_EQUAL(eve__sph_bessel_y0(v_t(1.5)),std__sph_bessel_y0(v_t(1.5)) , 6.0);
  TTS_ULP_EQUAL(eve__sph_bessel_y0(v_t(0.5)),std__sph_bessel_y0(v_t(0.5)) , 6.0);
  TTS_ULP_EQUAL(eve__sph_bessel_y0(v_t(1)),  std__sph_bessel_y0(v_t(1))   , 6.0);
  TTS_ULP_EQUAL(eve__sph_bessel_y0(v_t(0)),  eve::minf(eve::as<v_t>()), 0.0);

  TTS_ULP_EQUAL(eve__sph_bessel_y0( T(500)),  T(std__sph_bessel_y0(v_t(500)) ), 6.0);
  TTS_ULP_EQUAL(eve__sph_bessel_y0( T(10)) ,  T(std__sph_bessel_y0( v_t(10)) ), 6.0);
  TTS_ULP_EQUAL(eve__sph_bessel_y0( T(5))  ,  T(std__sph_bessel_y0( v_t(5))  ), 6.0);
  TTS_ULP_EQUAL(eve__sph_bessel_y0( T(2))  ,  T(std__sph_bessel_y0( v_t(2))  ), 6.0);
  TTS_ULP_EQUAL(eve__sph_bessel_y0( T(1.5)),  T(std__sph_bessel_y0( v_t(1.5))), 6.0);
  TTS_ULP_EQUAL(eve__sph_bessel_y0( T(0.5)),  T(std__sph_bessel_y0( v_t(0.5))), 6.0);
  TTS_ULP_EQUAL(eve__sph_bessel_y0( T(1))  ,  T(std__sph_bessel_y0( v_t(1))  ), 6.0);
  TTS_ULP_EQUAL(eve__sph_bessel_y0( T(0))  , eve::minf(eve::as< T>()), 0.0);

  TTS_ULP_EQUAL(eve__sph_bessel_y0(a0), map(std__sph_bessel_y0, a0), 10.0);
  TTS_ULP_EQUAL(eve__sph_bessel_y0(a1), map(std__sph_bessel_y0, a1), 10.0);
  TTS_ULP_EQUAL(eve__sph_bessel_y0(a2), map(std__sph_bessel_y0, a2), 10.0);

};

EVE_TEST( "Check behavior of diff(sph_bessel_y0) on wide"
        , eve::test::simd::ieee_reals
        , eve::test::generate(eve::test::randoms(1.0, 10.0))
        )
  <typename T>(T a0 )
{
  auto eve__diff_bessel_y0 =  [](auto x) { return eve::diff(eve::sph_bessel_y0)(x); };
  auto df = [](auto x){return eve::detail::centered_diffdiv(eve::sph_bessel_y0, x); };

  TTS_RELATIVE_EQUAL(eve__diff_bessel_y0(a0),   df(a0), 1.0e-2);
};


#include <iostream>
#include <type.hpp>

#include <Eigen/Eigen>

#include "Geometry/ParameterDesign/Bezier.hpp"
#include "imgui/implot.h"
#include "Visualization/Visualizer.h"
#include "Geometry/ParameterDesign/ParameterCurve.h"
#include "Geometry/ParameterDesign/Rational.h"

class BezierVisualizer :public Visualizer
{
protected:
	void AddPoint();
	void DragPoint();

	void draw(bool* p_open) override;

	std::vector<Point2d> points;
	std::vector<Float> weights;
	bool updated = true;

	void evaluate_bezier()
	{
		using V = Eigen::Vector2d;
		auto temp_points = std::vector<Eigen::Vector2d>(3);

		points[0] = V(points[0].x(), 0);
		points[1] = V(0, points[1].y());
		temp_points[0] = points[0];
		temp_points[1] = V(points[0].x(), points[1].y());
		temp_points[2] = V(2 * points[0].x(), 2 * points[1].y());

		weights = { 1,1,0 };
		RationalBSpline<3> RBS(temp_points, weights);
		RBS.evaluate();

		for (int i = 0; i < Length; ++i)
		{
			xs[i] = RBS(ctr_points[i]).x();
			ys[i] = RBS(ctr_points[i]).y();
		}
		weights = { 1,-1,0 };
		temp_points[1] *= -1;
		RBS = RationalBSpline<3>(temp_points, weights);

		RBS.evaluate();
		for (int i = 0; i < Length; ++i)
		{
			xs[i + Length] = RBS(1.0 - ctr_points[i]).x();
			ys[i + Length] = RBS(1.0 - ctr_points[i]).y();
		}

		ctr_xs.resize(points.size());
		ctr_ys.resize(points.size());

		for (int i = 0; i < points.size(); ++i)
		{
			ctr_xs[i] = points[i].x();
			ctr_ys[i] = points[i].y();
		}
	}

public:
	BezierVisualizer() {
		for (int i = 0; i < Length; ++i)
		{
			ctr_points[i] = Float(i) / (Length - 1);
			//ctr_points[i + Length] = Float(i) / (Length - 1);
		}

		points.resize(2);
		points[0] = Eigen::Vector2d(2, 0);
		points[1] = Eigen::Vector2d(0, 1);
	}
	const float Length = 1001;

	std::vector<float> ctr_points = std::vector<float>(Length);
	std::vector<float> xs = std::vector<float>(2 * Length);
	std::vector<float> ys = std::vector<float>(2 * Length);

	std::vector<float> ctr_xs = std::vector<float>(0);
	std::vector<float> ctr_ys = std::vector<float>(0);
};

void BezierVisualizer::AddPoint()
{
	//if (ImGui::IsMouseClicked(ImGuiMouseButton_Right))
	//{
	//	auto pos = ImPlot::GetPlotMousePos();
	//	points.emplace_back(pos.x, pos.y);
	//	updated = true;
	//}
}

void BezierVisualizer::DragPoint()
{
	for (int i = 0; i < points.size(); ++i)
	{
		auto& point = points[i];
		updated |= ImPlot::DragPoint(("Point " + std::to_string(i)).c_str(), &point.x(), &point.y());
	}
}

void BezierVisualizer::draw(bool* p_open)
{
	if (updated)
	{
		evaluate_bezier();
		updated = false;
	}
	if (ImGui::BeginTabBar("Homework 2")) {
		if (ImGui::BeginTabItem("Interpolation"))
		{
			if (ImPlot::BeginPlot("Line Plot", "x", "f(x)", ImGui::GetContentRegionAvail(), ImPlotFlags_NoBoxSelect | ImPlotFlags_NoMenus)) {
				int delta = 667;

				ImPlot::PlotLine("Bezier", &xs[0], &ys[0], Length + delta);

				ImPlot::PlotLine("Bezier", &xs[Length + delta], &ys[Length + delta], Length - delta);
				//if (!ctr_xs.empty())
				//{
				//	ImPlot::PlotLine("ControlBox", &ctr_xs[0], &ctr_ys[0], ctr_xs.size());
				//}
				AddPoint();
				DragPoint();

				ImPlot::EndPlot();
			}

			ImGui::EndTabItem();
		}

		ImGui::EndTabBar();
	}
	ImGui::End();
}

int main()
{
	BezierVisualizer visualizer;
	visualizer.RenderLoop();
}


/* This file is part of the Tomographer project, which is distributed under the
 * terms of the MIT license.
 *
 * The MIT License (MIT)
 *
 * Copyright (c) 2016 ETH Zurich, Institute for Theoretical Physics, Philippe Faist
 * Copyright (c) 2017 Caltech, Institute for Quantum Information and Matter, Philippe Faist
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in
 * all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 * SOFTWARE.
 */

#include <cmath>

#include <string>
#include <iostream>
#include <random>

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

// definitions for Tomographer test framework -- this must be included before any
// <Eigen/...> or <tomographer/...> header
#include "test_tomographer.h"

#include <tomographer/mathtools/random_unitary.h>

// -----------------------------------------------------------------------------
// fixture(s)

// -----------------------------------------------------------------------------
// test suites

BOOST_AUTO_TEST_SUITE(test_mathtools_randomUnitary)

BOOST_AUTO_TEST_CASE(basic)
{
  Eigen::MatrixXcd U(7,7);

  std::mt19937 rng(43423); // seeded, deterministic random number generator

  // check that the given U is unitary, for a couple tries

  for (int k = 0; k < 1000; ++k) {
    BOOST_MESSAGE("Running randomUnitary() for the "<<k<<"-th time") ;

    Tomographer::MathTools::randomUnitary(U, rng);

    MY_BOOST_CHECK_FLOATS_EQUAL((U * U.adjoint() - Eigen::MatrixXcd::Identity(7,7)).norm(), 0, 1e-12);
    MY_BOOST_CHECK_FLOATS_EQUAL((U.adjoint() * U - Eigen::MatrixXcd::Identity(7,7)).norm(), 0, 1e-12);
  }
}

//
// Test that if we average rho over many random unitaries from randomUnitary, we get the
// maximally mixed state; this then shows/indicates(?) that randomUnitary is indeed
// distributed according to the Haar measures.
//
BOOST_AUTO_TEST_CASE(twirl_rho_gives_identity)
{
  Eigen::Matrix3cd rho;
  rho << 0.2, 0, 0,
         0, 0.5, 0,
         0, 0, 0.3 ;

  std::mt19937 rng(4832342u);

  Eigen::Matrix3cd U;

  Eigen::Matrix3cd rhoTwirled(Eigen::Matrix3cd::Zero());

  const int n_points = 10000;
  for (int k = 0; k < n_points; ++k) {
    Tomographer::MathTools::randomUnitary(U, rng);

    rhoTwirled.noalias() += U*rho*U.adjoint();
  }

  rhoTwirled /= n_points;

  BOOST_MESSAGE("rho averaged over random unitaries is  rhoTwirled = \n" << rhoTwirled);

  MY_BOOST_CHECK_EIGEN_EQUAL(rhoTwirled, Eigen::Matrix3cd::Identity()/3.0, 0.5/std::sqrt((double)n_points));
}

BOOST_AUTO_TEST_SUITE_END()


/* Small utility functions useful for point-based registration */
#ifndef UTIL_INCLUDED
#define UTIL_INCLUDED

#include <Eigen/Dense>

bool isApproxEqual(double a, double b, double eps);

bool isApproxEqual(double a, double b);

Eigen::Vector3d find_pointset_average(const Eigen::MatrixXd& pointset);

Eigen::MatrixXd residuals_from_point(const Eigen::MatrixXd& pointset, const Eigen::Vector3d& point);

Eigen::VectorXd distances_between_pointsets(const Eigen::MatrixXd& pointset, const Eigen::MatrixXd& pointset_dash);

double root_mean_square(const Eigen::VectorXd& v);

Eigen::Matrix4d compose_final_transform(const Eigen::Matrix3d& rotation, const Eigen::Vector3d& translation);

Eigen::MatrixXd apply_transform(const Eigen::MatrixXd& pointset, const Eigen::Matrix4d& transform);

Eigen::MatrixXd load_pointcloud_from_file(std::string filename);

Eigen::Matrix4d load_transform_from_file(std::string filename);

void write_matrix_to_file(const Eigen::MatrixXd& matrix, std::string filename);
#endif


#ifndef POLICY_HPP
#define POLICY_HPP

#include <limits>
#include "helpers.hpp"
#include "stats.hpp"
#include <boost/math/distributions/geometric.hpp>
#include <boost/math/distributions/normal.hpp>
#include <boost/shared_ptr.hpp>

using boost::shared_ptr;

template<class PT>
class PolicyBase
{
public:
  PolicyBase(unsigned long horizon, const int arms) :
    totalrewards(arms, 0), sample_index(arms, 0), horizon(horizon), totalsampled(arms,0), totalscore(0), arms(arms) {}

  void simulate(const Table& rewards, const unsigned long t, const double mustar,
    Stats& stats)
  {
    int arm = (static_cast<PT*>(this))->choosearm(t);
    totalsampled[arm] += 1;
    double outcome = rewards[arm][sample_index[arm]];
    totalrewards[arm] += outcome;
    sample_index[arm]++;
    totalscore += outcome;
    const double instantregret = (t*mustar) - totalscore;
    #pragma omp critical
    {
      stats(instantregret, t);
    }
  }
//protected:
  vector<double> totalrewards;
  vector<int> sample_index;
  unsigned long horizon;
  vector<double> totalsampled;
  double totalscore;
  const int arms;
};

template<class PT>
class PolicyCollection
{
public:

  void simulate(Table& rewards, const unsigned long t, const double mustar,
    vector<Stats>& totalregret)
  {
    for (unsigned int i = 0; i < pcs.size(); ++i)
    {
      pcs[i]->simulate(rewards, t, mustar, totalregret[i]);
    }
  }

  vector<shared_ptr<PT> > pcs;
};

class ThompsonPolicy : public PolicyBase<ThompsonPolicy>
{
public:
  ThompsonPolicy(RandomSampleHelper& bh, unsigned long horizon, int arms, bool gaussian) :
    PolicyBase<ThompsonPolicy>(horizon, arms), bh(bh), gaussian(gaussian) {}
  int choosearm(int t)
  {
    int result = -1;
    double max = -std::numeric_limits<double>::max();
    for(int a = 0; a < arms; ++a)
    {
      if (totalsampled[a] == totalrewards[a])
        return a;
      const double samp = gaussian ? bh.sampleGaussian(totalrewards[a]/(1.0+totalsampled[a]), sqrt(1.0/(1.0+totalsampled[a]))) :
        bh.sampleBeta(totalrewards[a] + 1, totalsampled[a] - totalrewards[a] + 1);
      if(samp > max)
      {
        result = a;
        max = samp;
      }
    }
    return result;
  }

private:
  RandomSampleHelper& bh;
  bool gaussian;
};

class UCB1Policy : public PolicyBase<UCB1Policy>
{
public:
  UCB1Policy(unsigned long horizon, int arms) :
    PolicyBase<UCB1Policy>(horizon, arms) {}

  int choosearm(int t)
  {
    int result = -1;
    double max = -std::numeric_limits<double>::max();
    if(t <= arms)
      return t-1;
    for(int a = 0; a < arms; ++a)
    {
      const double avg = double(totalrewards[a])/double(totalsampled[a]);
      const double ucb = avg + sqrt(2*log(double(t))/double(totalsampled[a]));
      if(ucb > max)
      {
        result = a;
        max = ucb;
      }
    }
    return result;
  }
};

class BayesUCBPolicy : public PolicyBase<BayesUCBPolicy>
{
public:
  BayesUCBPolicy(unsigned long horizon, int arms, bool gaussian) :
    PolicyBase<BayesUCBPolicy>(horizon, arms), gaussian(gaussian) {}

  int choosearm(int t)
  {
    int result = -1;
    double max = -std::numeric_limits<double>::max();
    if(t <= arms)
      return t-1;
    for(int a = 0; a < arms; ++a)
    {
      const double gamma = 1.0 - 1.0 / double(t);
      double ucb = 0;
      if (gaussian)
      {
        double mun = totalrewards[a]/(1.0+totalsampled[a]);
        double sigman = sqrt(1.0/(1.0+totalsampled[a]));
        boost::math::normal_distribution<double, policies::policy<> > normdist(mun,sigman);
        ucb = boost::math::quantile(normdist, gamma);
      }
      else
      {
        ucb = ibeta_inv(totalrewards[a]+1, totalsampled[a]-totalrewards[a] + 1, gamma);
      }
      if(ucb > max)
      {
        result = a;
        max = ucb;
      }
    }
    return result;
  }
private:
  bool gaussian;
};

class NaivePolicy : public PolicyBase<NaivePolicy>
{
public:
  NaivePolicy(unsigned long horizon, int arms) :
    PolicyBase<NaivePolicy>(horizon, arms) {}

  int choosearm(int t)
  {
    int result = -1;
    double max = -numeric_limits<double>::max();
    if(t <= arms)
      return t-1;
    #ifdef PRINT_POLICY
    cout << "round [" << t << "]. ";
    #endif
    for(int a = 0; a < arms; ++a)
    {
      const double avg = double(totalrewards[a])/double(totalsampled[a]);
      #ifdef PRINT_POLICY
      cout << "avg for arm [" << a << "] is [" << avg << "]. ";
      #endif
      if(avg > max)
      {
        result = a;
        max = avg;
      }
    }
    #ifdef PRINT_POLICY
    cout << "picked arm [" << result << "]" << endl;
    #endif
    return result;
  }
};

class GittinsPolicy : public PolicyBase<GittinsPolicy>
{
public:
  GittinsPolicy(const Table3D& indices, unsigned long horizon, int arms, double b_param, bool gaussian) :
    PolicyBase<GittinsPolicy>(horizon, arms), indices(indices), gaussian(gaussian), b_param(b_param)
  {
  }

  int choosearm(int it)
  {
    int result = -1;
    double max = -1;
    int offset = (int) b_param;
    for(int a = 0; a < arms; ++a)
    {
      double gindex = 0;
      double g = 1.0 - 1.0/(b_param + double(it));
      if (indices.size() == 0)
      {
        if (gaussian)
          gindex = approximategi2(g, totalrewards[a]/(1.0+totalsampled[a]), 1.0/sqrt(1+totalsampled[a]));
        else
          gindex = approximategi(g, totalrewards[a]+1, totalsampled[a]-totalrewards[a]+1);
      }
      else {
        if (gaussian)
          gindex = approximategi2(g, totalrewards[a]/(1.0+totalsampled[a]), 1.0/sqrt(1+totalsampled[a]));
        else
          gindex = indices[it + offset -1][totalrewards[a]][totalsampled[a]-totalrewards[a]];
      }
      if(gindex > max)
      {
        result = a;
        max = gindex;
      }
      #ifdef PRINT_POLICY
      cout << "a" << a << " = " << long(totalrewards[a]) << ". b = " << long(totalsampled[a]-totalrewards[a]) << ". gindex = " << gindex << ". ";
      #endif
    }
    #ifdef PRINT_POLICY
    cout << endl;
    #endif
    return result;
  }
private:
  const Table3D& indices;

  double approximategi(double g, double a, double b)
  {
    const double mu = a/(a+b);
    const double var = a*b/((a+b)*(a+b)*(a+b+1));
    const static double c = -log(g);
    return mu + sqrt(var)*psi(var/c);
  }

  double psi(const double s)
  {
    if(s<=0.2)
      return sqrt(s/2.0);
    if(s <= 1)
      return 0.49 - 0.11/sqrt(s);
    if (s <= 5)
      return 0.63 - 0.26/sqrt(s);
    if (s <= 15)
      return 0.77 - 0.58/sqrt(s);
    return sqrt(2*log(s) - log(log(s)) - log(16*M_PI));
  }

  double approximategi2(double g, double mun, double sigman)
  {
    return g == 0.0 ? 0.0 : mun + geefunc(sigman, g);
  }

  double geefunc(double s, double gamma)
  {
    return sqrt(-log(gamma))*bfunc(-s*s/log(gamma));
  }

  double bfunc(double s)
  {
    if (s <= 1.0/7.0)
      return s/sqrt(2);
    if (s <= 100)
      return exp(-0.02645*(log(s)*log(s)) + 0.89106*log(s) - 0.4873);
    return sqrt(s)*sqrt(2*log(s) - log(log(s)) - log(16*PI));
  }

  bool gaussian;
  double b_param;
};

class IDSMinFunc
{
public:
  IDSMinFunc(double di, double dj, double gi, double gj)
    : gi(gi), gj(gj), di(di), dj(dj)
  {}

  double operator() (double q)
  {
    return (q*di + (1-q)*dj)*(q*di + (1-q)*dj)/(q*gi + (1-q)*gj);
  }

  void print()
  {
    cout << "gi:" << gi << ". gj:" << gj;
    cout << "di:" << di << ". dj:" << dj;
    cout << endl;
  }
private:
  double gi, gj, di, dj;
};

class IDSPolicy : public PolicyBase<IDSPolicy>
{
  const vector<double>& uniforms;
  int POINTS;
  double RANGE;
  double STEPSIZE;
public:
  IDSPolicy(unsigned long horizon, const vector<double>& uniforms, int arms, bool gaussian_, int bpoints) :
    PolicyBase<IDSPolicy>(horizon, arms),
    uniforms(uniforms),
    POINTS(gaussian_ ? 8000 : bpoints),
    fi(arms, vector<double>(POINTS, 0)),
    Fi(arms, vector<double>(POINTS, 0)),
    Qi(arms, vector<double>(POINTS, 0)),
    F(POINTS, 0),
    gaussian(gaussian_),
    lastret(-1)
  {
    RANGE = 40.0;
    STEPSIZE = gaussian ? RANGE/double(POINTS) : 1.0/double(POINTS);
    boost::math::normal_distribution<> normdist(0,1);
    for(int i = 0; i < POINTS; ++i)
    {
      double Fprod = 1;
      for(int a =0; a < arms; ++a)
      {
        double x = (gaussian ? -RANGE/2.0 : 0) + STEPSIZE*(i+0.5);
        Fi[a][i] = gaussian ? cdf(normdist, x) : ibeta(1.0,1.0,x);
        Fprod *= Fi[a][i];
        fi[a][i] = gaussian ? pdf(normdist, x) : ibeta_derivative(1.0,1.0,x);
        Qi[a][i] = gaussian ? 0 : 0.5*ibeta(1.0+1.0,1.0,x);
      }
      F[i] = Fprod;
    }
  }

  int choosearm(int t)
  {

    vector<double> delta(arms, 0);
    vector<double> gee(arms, 0);
    computeDeltasAndGees(delta, gee);

    int istar=-1,jstar=-1;
    double qstar=-1.0;
    setupRandomization(gee, delta, qstar, istar, jstar);
    int ret= uniforms[t-1] < qstar ? istar : jstar;
    assert (ret >= 0 && ret < arms);

    //remember the last arm that was pulled, so we can update relevant values of fi, Fi, etc. on the next iteration
    lastret = ret;
    return ret;
  }

  double probOptimal(int arm)
  {
    double result = 0;
    for(int i = 0; i < POINTS; ++i)
    {
      double fprod = 1;
      for (int a = 0; a < arms; ++a)
      {
        fprod *= a == arm ? 1.0 : Fi[a][i];
      }
      result += fi[arm][i]*fprod;
    }
    return STEPSIZE*result;
  }

  static void setupRandomization(const vector<double>& gee,
                          const vector<double>& delta,
                          double& qstar,
                          int& istar,
                          int& jstar)
  {
    int arms = gee.size();
    double opt = numeric_limits<double>::max();
    for(int j = 0; j < arms-1; ++j)
    {
      for(int i = j+1; i < arms; ++i)
      {
        IDSMinFunc minfun(delta[i], delta[j], gee[i], gee[j]);
        std::pair<double,double> res = brent_find_minima(minfun, 0.0, 1.0, 20);
        assert((res.second < opt) || !(i == 1 && j == 0));
        if(res.second < opt)
        {
          opt = res.second;
          istar = i; jstar = j;
          qstar = res.first;
        }
      }
    }
  }

  void computeDeltasAndGees(vector<double>& delta, vector<double>& gee)
  {
    vector<boost::math::normal_distribution<> > normdists;
    vector<double> muns;
    vector<double> sigmans;
    for (int a = 0; a < arms; ++a)
    {
      double mu = totalrewards[a]/(1.0 + totalsampled[a]);
      double sigma = sqrt(1.0/(1.0 + totalsampled[a]));
      assert(!gaussian || abs(mu) < RANGE/2.0 - sigma);
      muns.push_back(mu);
      sigmans.push_back(sigma);
      normdists.push_back(boost::math::normal_distribution<>(mu, sigma));
    }
    vector<double> as(arms, 0);
    vector<double> bs(arms, 0);
    vector<double> proboptimal(arms, 0);
    for(int i=0; i < arms;++i)
    {
      as[i] = totalrewards[i]+1;
      bs[i] = totalsampled[i]-totalrewards[i]+1;
    }

    if (lastret >= 0)
    {
      const boost::math::normal_distribution<>& newnormdist = normdists[lastret];
      const int newa = totalrewards[lastret]+1;
      const int newb = totalsampled[lastret] - totalrewards[lastret]+1;
      for(int i = 0; i < POINTS; ++i)
      {
        double x =  (gaussian ? -RANGE/2.0 : 0.0) + (i+0.5)*STEPSIZE;

        fi[lastret][i] = gaussian ? pdf(newnormdist, x) : ibeta_derivative(newa,newb,x);
        Fi[lastret][i] = gaussian ? cdf(newnormdist, x) : ibeta(newa,newb,x);
        Qi[lastret][i] = gaussian ? 0 : double(newa)/double(newa+newb)*ibeta(newa+1,newb,x);
        F[i] = 1;
        for(int a = 0; a < arms; ++a)
        {
          F[i] *= Fi[a][i];
        }
      }
    }

    for(int a =0; a < arms; ++a)
      proboptimal[a] = probOptimal(a);
    #ifndef NDEBUG
    double totalprob = 0;
    for (int a = 0; a < arms; ++a)
    {
     totalprob += proboptimal[a];
    }
    if (abs(totalprob - 1.0) >=  1e-1)
    {
      cerr << "probabilities must sum to one when in fact totalprob = " << totalprob << endl;
      for (int a = 0; a < arms; ++a)
      {
        cerr << "totalsampled[" << a << "] = " << totalsampled[a] << ". proboptimal[" << a << "] = " << proboptimal[a] << endl;
        cerr << "totalrewards[" << a << "] = " << totalrewards[a] << endl;
      }
      exit(1);
    }
    #endif
    Table Ms(arms,vector<double>(arms,0));
    for(int i = 0; i < arms;++i)
    {
      for(int j = 0; j < arms; ++j)
      {
        double sum=0;
        if(i==j)
        {
          for(int p=0;p < POINTS; ++p)
          {
            double x = (gaussian ? - RANGE/2.0 : 0) + (p+0.5)*STEPSIZE;
            sum += x*fi[i][p]*F[p]/max(Fi[i][p],0.0000001);
          }
          sum*=STEPSIZE;
          Ms[i][j] = sum/proboptimal[i];
          assert(gaussian || (Ms[i][j] < 1 && Ms[i][j] > 0));
          continue;
        }
        for(int p =0; p < POINTS;++p)
        {
          sum +=gaussian ? fi[i][p]*F[p]*fi[j][p]/max(Fi[i][p]*Fi[j][p],0.000001) : fi[i][p]*F[p]*Qi[j][p]/max(Fi[i][p]*Fi[j][p],0.000001);
          if(isnan(sum))
          {
            cerr << "sum is nan! probably cuz Fi["<<i<<"]["<<p<<"]=" << Fi[i][p] << ". ";
            cerr << "and Fi["<<j<<"]["<<p<<"]=" << Fi[j][p] << endl;
            cerr << "fi[" << i << "][" << p <<"]="<<fi[i][p] << endl;
            cerr << "Qi[" << j << "][" << p <<"]="<<Qi[j][p] << endl;
            cerr << "F[" << p <<"]="<< F[p] << endl;
            cerr << "the result is=" << fi[i][p]*F[p]*Qi[j][p]/max(Fi[i][p]*Fi[j][p],0.000001) << endl;
            cerr << "the denominator is=" << max(Fi[i][p]*Fi[j][p],0.000001) << endl;
            cerr << "the numerator is " << fi[i][p]*F[p]*Qi[j][p] << endl;
            exit(1);
          }
        }
        sum*=STEPSIZE;
        Ms[i][j] = gaussian ? muns[j] - sum * sigmans[j]*sigmans[j]/proboptimal[i] :  sum/proboptimal[i];
        assert(!isnan(Ms[i][j]));
      }
    }
    double rhostar=0;
    for(int i = 0; i < arms;++i)
    {
      rhostar += proboptimal[i]*Ms[i][i];
    }
    for(int i = 0; i < arms; ++i)
    {
      delta[i] = gaussian ? rhostar - muns[i] : rhostar - as[i]/(as[i]+bs[i]);
      double geeval=0;
      for(int j = 0; j < arms;++j)
      {
        geeval += gaussian ? proboptimal[j] * (Ms[j][i] - muns[i])*(Ms[j][i] - muns[i]) :  proboptimal[j]*kl(Ms[j][i], as[i]/(as[i]+bs[i]));
        assert(!isnan(geeval));
      }
      gee[i] = geeval;
    }
  }

  Table fi;
  Table Fi;
  Table Qi;
  vector<double> F;
  bool gaussian;
  int lastret;
};

class GaussianIDSPolicy : public PolicyBase<GaussianIDSPolicy>
{

public:
  GaussianIDSPolicy(unsigned long horizon, RandUnif& rng, int arms) :
    PolicyBase<GaussianIDSPolicy>(horizon, arms), unif(rng), horizon(horizon)
  {
  }

private:
  RandUnif& unif;
  unsigned long horizon;
};

class InterestingPolicy : public PolicyBase<InterestingPolicy>
{
public:
  InterestingPolicy(unsigned long horizon, int arms, double a_param, double b_param, bool gaussian, int numSteps = 1) :
    PolicyBase<InterestingPolicy>(horizon, arms), a_param(a_param), b_param(b_param), gaussian(gaussian), numSteps(numSteps)
  {
  }

  int choosearm(int it)
  {
    double t = double(it);
    int result = -1;
    double max = -numeric_limits<double>::max();

    double  gamma_t = 1.0 - a_param/(b_param + t);

    for(int a = 0; a < arms; ++a)
    {
      #ifdef PRINT_POLICY
      cout << "discount for " << a << " is " << gamma << " ";
      #endif
      const double index = gaussian ? ::computeAGIGaussian(gamma_t, totalrewards[a]/(1+totalsampled[a]), 1.0/sqrt(1+totalsampled[a]))
         : ::computeApproxGI(gamma_t, totalrewards[a] + 1, (long)(totalsampled[a] - totalrewards[a]) + 1, numSteps);
      if(index > max)
      {
        result = a;
        max = index;
      }
      #ifdef PRINT_POLICY
      cout << "a" << a << " = " << long(totalrewards[a]) << ". b = " << long(totalsampled[a]-totalrewards[a]) << ". gindex = " << index << ". ";
      #endif
    }
    #ifdef PRINT_POLICY
    cout << endl;
    #endif
    return result;
  }
private:
  const double a_param, b_param;
  bool gaussian;
  int numSteps;
};

#endif


#include "timer.hh"
#include <Eigen/Dense>
#include <cstdlib>
#include <iostream>
#include <vector>

using namespace std;
using namespace Eigen;

typedef MatrixXf Matrix;
typedef RowVectorXf RowVector;
typedef VectorXf ColVector;

class NeuralNetwork
{
  vector<ColVector*> neuronLayers; // different layers of output network
  vector<ColVector*> cacheLayers;  // values of layers before activation
  vector<ColVector*> deltas;       // stores the error contribution of each neurons
  vector<MatrixXf*> weights;       // the weights of connections between layers
  vector<uint> topology;
  double learningRate;
  // enum { NeedsToAlign = (sizeof(neuronLayers)%16)==0 };

public:
  // EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign)

  NeuralNetwork( vector<uint> topology, double learningRate = 0.005f );

  ColVector forward_propagation( ColVector& input );
  void backward_propagation( ColVector& output );
  void error_calculation( ColVector& output );
  void update_weights();

  double activationFunction( double x ) { return max( x, 0.0f ); }

  ColVector activationFunction( ColVector& x ) { return x.cwiseMax( 0 ); }

  double activationFunctionDerivative( double x )
  {
    if ( x >= 0 )
      return 1;
    else
      return 0;
  }

  void print()
  {
    const IOFormat CleanFmt( 4, 0, ", ", "\n", "[", "]" );
    for ( uint i = 1; i < topology.size(); i++ ) {
      cout << "Weights Layer " << i << ":\n"
           << ( *weights[i - 1] ).format( CleanFmt ) << "\n\n"
           << "cacheLayers: Layer " << i << ":\n"
           << ( *cacheLayers[i] ).format( CleanFmt ) << "\n\n"
           << "neuronLayers Layer " << i << ":\n"
           << ( *neuronLayers[i] ).format( CleanFmt ) << "\n\n";
    }
  }
};

NeuralNetwork::NeuralNetwork( vector<uint> topology_, double learningRate_ )
  : neuronLayers()
  , cacheLayers()
  , deltas()
  , weights()
  , topology( topology_ )
  , learningRate( learningRate_ )
{
  topology = topology_;
  learningRate = learningRate_;
  for ( uint i = 0; i < topology.size(); i++ ) {
    if ( i != topology.size() - 1 )
      neuronLayers.push_back( new ColVector( topology[i] + 1 ) );
    // +1 for biases
    else
      neuronLayers.push_back( new ColVector( topology[i] ) );

    // initialize cache and delta vectors
    cacheLayers.push_back( new ColVector( neuronLayers[i]->rows() ) );
    deltas.push_back( new ColVector( neuronLayers[i]->rows() ) );

    // setting up biases nodes
    if ( i != topology.size() - 1 ) {
      neuronLayers.back()->coeffRef( topology[i] ) = 1.0;
      cacheLayers.back()->coeffRef( topology[i] ) = 1.0;
    }

    // initialize weights matrix
    if ( i > 0 ) {
      if ( i != topology.size() - 1 ) {
        weights.push_back( new MatrixXf( topology[i] + 1, topology[i - 1] + 1 ) );

        // random weights initialisation
        weights.back()->setRandom();

        // no "normal" weights into biases
        weights.back()->row( topology[i] ).setZero();
        weights.back()->coeffRef( topology[i], topology[i - 1] ) = 1.0;
      } else {
        weights.push_back( new MatrixXf( topology[i], topology[i - 1] + 1 ) );
        weights.back()->setRandom();
      }
    }
  }
}

ColVector NeuralNetwork::forward_propagation( ColVector& input )
{
  // setting the input layer of the NN
  // block takes 4 arguments : startRow, startCol, blockRows, blockCols

  neuronLayers.front()->block( 0, 0, neuronLayers.front()->size() - 1, 1 ) = input;

  // forward propagation
  for ( uint i = 1; i < topology.size() - 1; i++ ) {
    ( *cacheLayers[i] ) = ( *weights[i - 1] ) * ( *neuronLayers[i - 1] );
    ( *neuronLayers[i] ) = activationFunction( *cacheLayers[i] );
    // resetting back the bias NN node
    neuronLayers[i]->coeffRef( topology[i] ) = 1.0;
  }
  // no activation at last layer
  uint i = topology.size() - 1;
  ( *cacheLayers[i] ) = ( *weights[i - 1] ) * ( *neuronLayers[i - 1] );
  ( *neuronLayers[i] ) = *cacheLayers[i];

  return *neuronLayers[topology.size() - 1];
}

void program_body()
{
  vector<uint> TOPOLOGY = { 5, 3, 2, 1 };
  ColVector input( 5 );
  input << 1.0f, -2.0f, 3.0f, -2.0f, 0.0f;
  NeuralNetwork nn( TOPOLOGY );
  uint64_t start = Timer::timestamp_ns();
  ColVector output = nn.forward_propagation( input );
  uint64_t end = Timer::timestamp_ns();
  cout << end - start << endl;

  const IOFormat CleanFmt( 4, 0, ", ", "\n", "[", "]" );

  cout << "input:\n"
       << input.format( CleanFmt ) << "\n\n"
       << "output:\n"
       << output.format( CleanFmt ) << "\n\n";
  nn.print();
}

int main()
{
  try {
    ios::sync_with_stdio( false );
    program_body();
    return EXIT_SUCCESS;
  } catch ( const exception& e ) {
    cerr << e.what() << "\n";
    return EXIT_FAILURE;
  }
}


#ifndef BART_SRC_CONVERGENCE_MOMENTS_CONVERGENCE_CHECKER_L_INFINITY_NORM_HPP_
#define BART_SRC_CONVERGENCE_MOMENTS_CONVERGENCE_CHECKER_L_INFINITY_NORM_HPP_

#include "convergence/convergence_checker.hpp"
#include <deal.II/lac/vector.h>

namespace bart::convergence::moments {

/*! \brief Checks for convergence between flux moments using the percentage
 * change in the \f$L_{\infty}\f$ norm, compared to the current iteration (\f$i\f$):
 *
 * \f[
 *
 * \Delta_i = \frac{|\phi_i - \phi_{i-1}|_{\infty}}{|\phi_{i}|_{\infty}}
 *
 * \f]
 *
 * Convergence is achieved if \f$\Delta_i \leq \Delta_{\text{max}}\f$.
 * */
class ConvergenceCheckerLInfinityNorm : public ConvergenceChecker<dealii::Vector<double>> {
 public:
  using Vector = dealii::Vector<double>;
  explicit ConvergenceCheckerLInfinityNorm(const double max_delta = 1e-6) { max_delta_ = CheckNonNegative(max_delta); };

  auto SetMaxDelta(const double& to_set) -> void override { max_delta_ = CheckNonNegative(to_set); };
  auto IsConverged(const Vector &current_iteration, const Vector &previous_iteration) -> bool override {
    Vector difference(current_iteration);
    difference.add(-1, previous_iteration);
    delta_ = difference.linfty_norm()/current_iteration.linfty_norm();
    is_converged_ = delta_ <= max_delta_;
    return is_converged_;
  }

 private:
  auto CheckNonNegative(const double to_check) -> double {
    AssertThrow(to_check > 0, dealii::ExcMessage("Error in ConvergenceCheckerLInfinityNorm, max delta value to set must "
                                                 "be greater than 0"))
    return to_check;
  }
};

} // namespace bart::convergence::moments

#endif //BART_SRC_CONVERGENCE_MOMENTS_CONVERGENCE_CHECKER_L_INFINITY_NORM_HPP_


#include <boost/math/constants/constants.hpp>
#include "EnergyResolution.hh"
#include "TypesFunctions.hh"
#include <fmt/format.h>
#include <string.h>

constexpr double pi = boost::math::constants::pi<double>();

EnergyResolution::EnergyResolution(bool propagate_matrix) :
EnergyResolution({"Eres_a" , "Eres_b" , "Eres_c"}, propagate_matrix){

}

EnergyResolution::EnergyResolution(const std::vector<std::string>& pars, bool propagate_matrix) :
HistSmearSparse(propagate_matrix)
{
  if(pars.size()!=3u){
    throw std::runtime_error("Energy resolution should have exactly 3 parameters");
  }
  variable_(&m_a, pars[0]);
  variable_(&m_b, pars[1]);
  variable_(&m_c, pars[2]);

  transformation_("matrix")
      .input("Edges", /*inactive*/true)
      .output("FakeMatrix")
      .types(TypesFunctions::ifHist<0>, TypesFunctions::if1d<0>, TypesFunctions::toMatrix<0,0,0>)
      .types(&EnergyResolution::getEdges)
      .func(&EnergyResolution::calcMatrix);

  add_transformation();
  add_input();
  set_open_input();
}

void EnergyResolution::getEdges(TypesFunctionArgs& fargs) {
  m_edges = fargs.args[0].edges.data();
}

double EnergyResolution::relativeSigma(double Etrue) const noexcept {
  return sqrt(pow(m_a, 2)+ pow(m_b, 2)/Etrue + pow(m_c/Etrue, 2));
}

double EnergyResolution::resolution(double Etrue, double Erec) const noexcept {
  static const double twopisqr = std::sqrt(2*pi);
  const double sigma = Etrue * relativeSigma(Etrue);
  const double reldiff = (Etrue - Erec)/sigma;

  return std::exp(-0.5*pow(reldiff, 2))/(twopisqr*sigma);
}

void EnergyResolution::calcMatrix(FunctionArgs& fargs) {
  m_sparse_cache.setZero();

  auto& ret = fargs.rets[0];
  auto* edges = m_edges;
  auto bins = ret.type.shape[0];
  m_sparse_cache.resize(bins, bins);

  /* fill the cache matrix with probalilities for number of events to leak to other bins */
  /* colums corressponds to reconstrucred energy and rows to true energy */
  auto bin_center = [edges](size_t index){ return (edges[index+1] + edges[index])/2; };
  for (size_t etrue = 0; etrue < bins; ++etrue) {
    double Etrue = bin_center(etrue);
    double dEtrue = edges[etrue+1] - edges[etrue];

    bool right_edge_reached{false};
    /* precalculating probabilities for events in given bin to leak to
     * neighbor bins  */
    for (size_t erec = 0; erec < bins; ++erec) {
      double Erec = bin_center(erec);
      double rEvents = dEtrue*resolution(Etrue, Erec);

      if (rEvents < 1E-10) {
        if (right_edge_reached) {
           break;
        }
        continue;
      }
      m_sparse_cache.insert(erec, etrue) = rEvents;
      if (!right_edge_reached) {
        right_edge_reached = true;
      }
    }
  }
  m_sparse_cache.makeCompressed();

  if ( m_propagate_matrix )
    fargs.rets[0].mat = m_sparse_cache;
}


#include "kmers/dirichlet-sampler.hpp"
#include <Eigen/Dense>
#include <iostream>

int main() {
  std::random_device dev;
  std::mt19937 rng(dev());

  uint64_t N = 20;
  Eigen::VectorXd alpha = Eigen::VectorXd::Ones(N);
  Eigen::VectorXd theta = kmers::dirichlet_rng(alpha, rng);
  std::vector<double> theta_sv(&theta[0], &theta[0] + N);
  std::vector<uint64_t> y = kmers::multinomial_rng(10000, theta_sv, rng);
  for (int n = 0; n < N; ++n) {
    std::cout << "theta[" << n << "] = " << theta[n]
	      << ";  y[" << n << "] = " << y[n]
	      << std::endl;
  }
}


#include <blitz/array.h>
#include <random/uniform.h>
#include <bp_util.h>

using namespace blitz;
using namespace ranlib;

template <typename T>
Array<T, 1> cnd(Array<T, 1> & x)
{
    int samples = x.numElements();
    Array<T, 1> l(samples), k(samples), w(samples), res(samples);
    Array<bool, 1> mask(samples);
    T a1 = 0.31938153,
      a2 =-0.356563782,
      a3 = 1.781477937,
      a4 =-1.821255978,
      a5 = 1.330274429,
      pp = 2.5066282746310002; // sqrt(2.0*PI)

    l = abs(x);
    k = 1.0 / (1.0 + 0.2316419 * l);
    w = 1.0 - 1.0 / pp * exp(-1.0*l*l/2.0) * \
        (a1*k + \
         a2*(pow(k,(T)2)) + \
         a3*(pow(k,(T)3)) + \
         a4*(pow(k,(T)4)) + \
         a5*(pow(k,(T)5)));

    mask    = x < 0.0;
    res     = (w * cast<T>(!mask) + (1.0-w)* cast<T>(mask));
    return res;
}

template <typename T>
T* pricing(size_t samples, size_t iterations, char flag, T x, T d_t, T r, T v)
{
    Array<T, 1> d1(samples), d2(samples), res(samples);
    T* p    = (T*)malloc(sizeof(T)*samples);    // Intermediate results
    T t     = d_t;                              // Initial delta

    Array<T, 1> s(samples);                     // Initial uniform sampling
    Uniform<T> rand;                            // values between 58-62
    rand.seed((unsigned int)time(0));
    s = rand.random() *4.0 +58.0;

    for(size_t i=0; i<iterations; i++) {
        d1 = (log(s/x) + (r+v*v/2.0)*t) / (v*sqrt(t));
        d2 = d1-v*sqrt(t);
        if (flag == 'c') {
            res = s * cnd(d1) - x * exp(-r * t) * cnd(d2);
        } else {
            Array<T, 1> tmp1(samples), tmp2(samples);
            tmp1 = -1.0*d2;
            tmp2 = -1.0*d1;

            res = x * exp(-r*t) * cnd(tmp1) - s*cnd(tmp2);
        }
        t += d_t;                               // Increment delta
        p[i] = sum(res) / (T)samples;           // Result from timestep
    }

    return p;
}

//FLOP count: 2*s+i*(s*8+2*s*23) where s is samples and i is iterations

int main(int argc, char* argv[])
{
    bp_util_type bp = bp_util_create(argc, argv, 2);
    if (bp.args.has_error) {
        return 1;
    }
    const size_t samples    = bp.args.sizes[0];
    const size_t iterations = bp.args.sizes[1];

    bp.timer_start();
    double* prices = pricing(
        samples, iterations,
        'c', 65.0, 1.0 / 365.0,
        0.08, 0.3
    );
    bp.timer_stop();

    bp.print("black_scholes(cpp11_blitz)");
    if (bp.args.verbose) {
        cout << ", \"output\": [";
        for(size_t i=0; i<iterations; i++) {
            cout << prices[i];
            if (iterations-1!=i) {
                cout << ", ";
            }
        }
        cout << "]" << endl;
    }

    free(prices);
    return 0;
}


// find_root_example.cpp

// Copyright Paul A. Bristow 2007.

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

// Example of using root finding.

// Note that this file contains Quickbook mark-up as well as code
// and comments, don't change any of the special comment mark-ups!

//[root_find1
/*`
First we need some includes to access the normal distribution
(and some std output of course).
*/

#include <boost/math/tools/roots.hpp> // root finding.

#include <boost/math/distributions/normal.hpp> // for normal_distribution
  using boost::math::normal; // typedef provides default type is double.

#include <iostream>
  using std::cout; using std::endl; using std::left; using std::showpoint; using std::noshowpoint;
#include <iomanip>
  using std::setw; using std::setprecision;
#include <limits>
  using std::numeric_limits;
//] //[/root_find1]

namespace boost{ namespace math { namespace tools
{

template <class F, class T, class Tol>
inline std::pair<T, T> bracket_and_solve_root(F f, // functor
                                              const T& guess,
                                              const T& factor,
                                              bool rising,
                                              Tol tol, // binary functor specifying termination when tol(min, max) becomes true.
                                              // eps_tolerance most suitable for this continuous function
                                              boost::uintmax_t& max_iter); // explicit (rather than default) max iterations.
// return interval as a pair containing result.

namespace detail
{

  // Functor for finding standard deviation:
  template <class RealType, class Policy>
  struct standard_deviation_functor
  {
     standard_deviation_functor(RealType m, RealType s, RealType d)
        : mean(m), standard_deviation(s)
     {
     }
     RealType operator()(const RealType& sd)
     { // Unary functor - the function whose root is to be found.
        if(sd <= tools::min_value<RealType>())
        { //
           return 1;
        }
        normal_distribution<RealType, Policy> t(mean, sd);
        RealType qa = quantile(complement(t, alpha));
        RealType qb = quantile(complement(t, beta));
        qa += qb;
        qa *= qa;
        qa *= ratio;
        qa -= (df + 1);
        return qa;
     } // operator()
     RealType mean;
     RealType standard_deviation;
  }; // struct standard_deviation_functor
} // namespace detail

template <class RealType, class Policy>
RealType normal_distribution<RealType, Policy>::find_standard_deviation(
      RealType difference_from_mean,
      RealType mean,
      RealType sd,
      RealType hint) // Best guess available - current sd if none better?
{
   static const char* function = "boost::math::normal_distribution<%1%>::find_standard_deviation";

   // Check for domain errors:
   RealType error_result;
   if(false == detail::check_probability(
      function, sd, &error_result, Policy())
      )
      return error_result;

   if(hint <= 0)
   { // standard deviation can never be negative.
      hint = 1;
   }

   detail::standard_deviation_functor<RealType, Policy> f(mean, sd, difference_from_mean);
   tools::eps_tolerance<RealType> tol(policies::digits<RealType, Policy>());
   boost::uintmax_t max_iter = 100;
   std::pair<RealType, RealType> r = tools::bracket_and_solve_root(f, hint, RealType(2), false, tol, max_iter, Policy());
   RealType result = r.first + (r.second - r.first) / 2;
   if(max_iter == 100)
   {
      policies::raise_evaluation_error<RealType>(function, "Unable to locate solution in a reasonable time:"
         " either there is no answer to how many degrees of freedom are required"
         " or the answer is infinite.  Current best guess is %1%", result, Policy());
   }
   return result;
} // find_standard_deviation
} // namespace tools
} // namespace math
} // namespace boost

int main()
{
  cout << "Example: Normal distribution, root finding.";
  try
  {

//[root_find2

/*`A machine is set to pack 3 kg of ground beef per pack.
Over a long period of time it is found that the average packed was 3 kg
with a standard deviation of 0.1 kg.
Assuming the packing is normally distributed,
we can find the fraction (or %) of packages that weigh more than 3.1 kg.
*/

double mean = 3.; // kg
double standard_deviation = 0.1; // kg
normal packs(mean, standard_deviation);

double max_weight = 3.1; // kg
cout << "Percentage of packs > " << max_weight << " is "
<< cdf(complement(packs, max_weight)) << endl; // P(X > 3.1)

double under_weight = 2.9;
cout <<"fraction of packs <= " << under_weight << " with a mean of " << mean
  << " is " << cdf(complement(packs, under_weight)) << endl;
// fraction of packs <= 2.9 with a mean of 3 is 0.841345
// This is 0.84 - more than the target 0.95
// Want 95% to be over this weight, so what should we set the mean weight to be?
// KK StatCalc says:
double over_mean = 3.0664;
normal xpacks(over_mean, standard_deviation);
cout << "fraction of packs >= " << under_weight
<< " with a mean of " << xpacks.mean()
  << " is " << cdf(complement(xpacks, under_weight)) << endl;
// fraction of packs >= 2.9 with a mean of 3.06449 is 0.950005
double under_fraction = 0.05;  // so 95% are above the minimum weight mean - sd = 2.9
double low_limit = standard_deviation;
double offset = mean - low_limit - quantile(packs, under_fraction);
double nominal_mean = mean + offset;

normal nominal_packs(nominal_mean, standard_deviation);
cout << "Setting the packer to " << nominal_mean << " will mean that "
  << "fraction of packs >= " << under_weight
  << " is " << cdf(complement(nominal_packs, under_weight)) << endl;

/*`
Setting the packer to 3.06449 will mean that fraction of packs >= 2.9 is 0.95.

Setting the packer to 3.13263 will mean that fraction of packs >= 2.9 is 0.99,
but will more than double the mean loss from 0.0644 to 0.133.

Alternatively, we could invest in a better (more precise) packer with a lower standard deviation.

To estimate how much better (how much smaller standard deviation) it would have to be,
we need to get the 5% quantile to be located at the under_weight limit, 2.9
*/
double p = 0.05; // wanted p th quantile.
cout << "Quantile of " << p << " = " << quantile(packs, p)
  << ", mean = " << packs.mean() << ", sd = " << packs.standard_deviation() << endl; //
/*`
Quantile of 0.05 = 2.83551, mean = 3, sd = 0.1

With the current packer (mean = 3, sd = 0.1), the 5% quantile is at 2.8551 kg,
a little below our target of 2.9 kg.
So we know that the standard deviation is going to have to be smaller.

Let's start by guessing that it (now 0.1) needs to be halved, to a standard deviation of 0.05
*/
normal pack05(mean, 0.05);
cout << "Quantile of " << p << " = " << quantile(pack05, p)
  << ", mean = " << pack05.mean() << ", sd = " << pack05.standard_deviation() << endl;

cout <<"Fraction of packs >= " << under_weight << " with a mean of " << mean
  << " and standard deviation of " << pack05.standard_deviation()
  << " is " << cdf(complement(pack05, under_weight)) << endl;
//
/*`
Fraction of packs >= 2.9 with a mean of 3 and standard deviation of 0.05 is 0.9772

So 0.05 was quite a good guess, but we are a little over the 2.9 target,
so the standard deviation could be a tiny bit more. So we could do some
more guessing to get closer, say by increasing to 0.06
*/

normal pack06(mean, 0.06);
cout << "Quantile of " << p << " = " << quantile(pack06, p)
  << ", mean = " << pack06.mean() << ", sd = " << pack06.standard_deviation() << endl;

cout <<"Fraction of packs >= " << under_weight << " with a mean of " << mean
  << " and standard deviation of " << pack06.standard_deviation()
  << " is " << cdf(complement(pack06, under_weight)) << endl;
/*`
Fraction of packs >= 2.9 with a mean of 3 and standard deviation of 0.06 is 0.9522

Now we are getting really close, but to do the job properly,
we could use root finding method, for example the tools provided, and used elsewhere,
in the Math Toolkit, see
[link math_toolkit.toolkit.internals1.roots2  Root Finding Without Derivatives].

But in this normal distribution case, we could be even smarter and make a direct calculation.
*/
//] [/root_find2]

  }
  catch(const std::exception& e)
  { // Always useful to include try & catch blocks because default policies
    // are to throw exceptions on arguments that cause errors like underflow, overflow.
    // Lacking try & catch blocks, the program will abort without a message below,
    // which may give some helpful clues as to the cause of the exception.
    std::cout <<
      "\n""Message from thrown exception was:\n   " << e.what() << std::endl;
  }
  return 0;
}  // int main()

/*

Output is:

*/


// ========================================================================= //
// Filename      :  test_neural_network.cpp
// Creation Date :  2016-6-14
// Created by    :  anfranek
// ========================================================================= //

#include <gtest/gtest.h>

#include <Eigen/Core>

#include <neural_network.h>

/**
 * @brief TEST neural_network, NAND
 *
 * NAND implemented as neural network containing one neuron.
 */
TEST(neural_network, NAND)
{
    NeuralNetwork<double, 2, 1> network;
    network.getLayer<0>().weights << -2, -2;
    network.getLayer<0>().biases << 3;

    EXPECT_GT(network.feedforward(Eigen::Vector2d(0, 0))[0], 0.5);
    EXPECT_GT(network.feedforward(Eigen::Vector2d(0, 1))[0], 0.5);
    EXPECT_GT(network.feedforward(Eigen::Vector2d(1, 0))[0], 0.5);
    EXPECT_LT(network.feedforward(Eigen::Vector2d(1, 1))[0], 0.5);
}

/**
 * @brief TEST neural_network, NAND_big_weights
 *
 * NAND implemented as neural network containing one neuron.
 * By using large weights and biases the results should be more distinct. I.e. 1 or 0.
 */
TEST(neural_network, NAND_big_weights)
{
    NeuralNetwork<double, 2, 1> network;
    network.getLayer<0>().weights << -2000, -2000;
    network.getLayer<0>().biases << 3000;

    EXPECT_DOUBLE_EQ(network.feedforward(Eigen::Vector2d(0, 0))[0], 1);
    EXPECT_DOUBLE_EQ(network.feedforward(Eigen::Vector2d(0, 1))[0], 1);
    EXPECT_DOUBLE_EQ(network.feedforward(Eigen::Vector2d(1, 0))[0], 1);
    EXPECT_DOUBLE_EQ(network.feedforward(Eigen::Vector2d(1, 1))[0], 0);
}

/**
 * @brief TEST neural_network, two_bit_adder
 *
 * Two bit adder implemented as neural network.
 * The network is rather complicated since only fully connected networks are possible as of yet.
 */
TEST(neural_network, two_bit_adder)
{
    NeuralNetwork<double, 2, 3, 3, 2> network;
    network.getLayer<0>().weights << 1000, 0, -2000, -2000, 0, 1000;
    network.getLayer<0>().biases << 0, 3000, 0;
    network.getLayer<1>().weights << -2000, -2000, 0, 0, -2000, -2000, 0, 1000, 0;
    network.getLayer<1>().biases << 3000, 3000, 0;
    network.getLayer<2>().weights << -2000, -2000, 0, 0, 0, -4000;
    network.getLayer<2>().biases << 3000, 3000;
    EXPECT_EQ(network.feedforward(Eigen::Vector2d(-0.01, -0.01)), Eigen::Vector2d(0, 0));
    EXPECT_EQ(network.feedforward(Eigen::Vector2d(-0.01, 1)), Eigen::Vector2d(1, 0));
    EXPECT_EQ(network.feedforward(Eigen::Vector2d(1, -0.01)), Eigen::Vector2d(1, 0));
    EXPECT_EQ(network.feedforward(Eigen::Vector2d(1, 1)), Eigen::Vector2d(0, 1));
}


#pragma once

#include <cmath>

#ifdef __POPC__
#undef USE_EIGEN
#else
#ifdef USE_EIGEN
#include <Eigen/Dense>
#endif
#include <iostream>
#endif

namespace light {

#ifdef USE_EIGEN
using Vector = Eigen::Vector3f;
#else
  struct Vector {
  float x, y, z;
  Vector() {}
  Vector(float x0, float y0, float z0) : x(x0), y(y0), z(z0) {}
  Vector operator + (const Vector &b) const {
    return Vector(x + b.x, y + b.y, z + b.z);
  }
  Vector& operator += (const Vector &b) {
    x += b.x;
    y += b.y;
    z += b.z;
    return *this;
  }
  Vector operator * (float b) const { return Vector(x*b, y*b, z*b); }
  Vector& operator *= (float s) {
    x *= s;
    y *= s;
    z *= s;
    return *this;
  }
  Vector operator - (const Vector &b) const {
    return Vector(x-b.x, y-b.y, z-b.z);
  }
  Vector operator - () const { return Vector(-x, -y, -z); }
  Vector operator / (float b) const { return Vector(x/b, y/b, z/b); }
  Vector cwiseProduct(const Vector &b) const {
    return Vector(x*b.x, y*b.y, z*b.z);
  }
  Vector normalized() const { return *this * (1.f/sqrtf(x*x + y*y + z*z)); }
  float squaredNorm() const { return x*x + y*y + z*z; }
  float norm() const { return sqrtf(squaredNorm()); }
  float dot(const Vector &b) const { return x*b.x + y*b.y + z*b.z; }
  Vector cross(const Vector &b) const {
    return Vector(y*b.z - z*b.y, z*b.x - x*b.z, x*b.y - y*b.x);
  }
  const float& operator () (size_t i) const { return i == 0 ? x : (i == 1 ? y : z); }
  Vector abs() const {
    return Vector(fabsf(x), fabsf(y), fabsf(z));
  }
  const Vector& array() const { return *this; } // For Eigen compatibility only
  bool isZero() const { return x == 0.f && y == 0.f && z == 0.f; }
  bool isNonZero() const { return x != 0.f || y != 0.f || z != 0.f; }
};

#endif

} // end namespace light

#ifndef __POPC__
#ifndef USE_EIGEN
inline
std::ostream& operator << (std::ostream &os, const light::Vector &v) {
  os << v.x << " " << v.y << " " << v.z;
  return os;
}
#endif
#endif


#include "stdafx.h"
#include "shape_sphere.h"
#include <boost/math/constants/constants.hpp>

namespace overdrive {
	namespace render {
		namespace shape {
			Sphere::Sphere(
				float radius,
				unsigned int slices,
				unsigned int stacks
			):
				mVertexBuffer((slices + 1) * (stacks + 1)),
				mIndexBuffer((slices * 2 * (stacks - 1)) * 3)
			{
				{
					using attributes::PositionNormalTexCoord;
					using boost::math::float_constants::pi;

					float theta;
					float phi;

					float thetaFactor = 2.0f * pi / slices;
					float phiFactor = pi / stacks;

					glm::vec3 position;
					glm::vec3 normal;
					glm::vec2 texCoord;

					auto vertices = mVertexBuffer.map();

					unsigned int index = 0;

					for (unsigned int i = 0; i <= slices; ++i) {
						theta = i * thetaFactor;

						texCoord.s = static_cast<float>(i) / stacks;

						for (unsigned int j = 0; j <= stacks; ++j) {
							phi = j * phiFactor;

							texCoord.t = static_cast<float>(j) / stacks;

							normal.x = sinf(phi) * cosf(theta);
							normal.y = sinf(phi) * sinf(theta);
							normal.z = cosf(phi);

							position = normal * radius;

							vertices[index++] = PositionNormalTexCoord{ position, normal, texCoord };
						}
					}
				}

				{
					auto indices = mIndexBuffer.map();

					unsigned int index = 0;

					for (GLuint i = 0; i < slices; ++i) {
						GLuint stackStart = i * (stacks + 1);
						GLuint nextStackStart = (i + 1) * (stacks + 1);

						for (GLuint j = 0; j < stacks; ++j) {
							if (j == 0) {
								indices[index    ] = stackStart;
								indices[index + 1] = stackStart + 1;
								indices[index + 2] = nextStackStart + 1;

								index += 3;
							}
							else if (j == (stacks - 1)) {
								indices[index    ] = stackStart + j;
								indices[index + 1] = stackStart + j + 1;
								indices[index + 2] = nextStackStart + j;

								index += 3;
							}
							else {
								indices[index    ] = stackStart + j;
								indices[index + 1] = stackStart + j + 1;
								indices[index + 2] = nextStackStart + j + 1;
								indices[index + 3] = nextStackStart + j;
								indices[index + 4] = stackStart + j;
								indices[index + 5] = nextStackStart + j + 1;

								index += 6;
							}
						}
					}
				}

				mVAO.attach(mIndexBuffer);
				mVAO.attach(mVertexBuffer);
			}

			void Sphere::draw() {
				mVAO.draw();
			}
		}
	}
}


#include <boost/math/constants/constants.hpp>
#include "EnergyResolutionInput.hh"
#include "TypeClasses.hh"
#include "TypesFunctions.hh"
#include <fmt/format.h>
#include <string.h>

constexpr double pi = boost::math::constants::pi<double>();

using namespace TypeClasses;

EnergyResolutionInput::EnergyResolutionInput(bool propagate_matrix) :
HistSmearSparse(propagate_matrix)
{
  transformation_("matrix")
      .input("Edges", /*inactive*/true)     // Input bin edges [N]
      .input("RelSigma")                    // Relative Sigma value for each bin center [N-1]
      .output("FakeMatrix")
      .types(new CheckKindT<double>(DataKind::Hist, 0), new CheckKindT<double>(DataKind::Points, 1))
      .types(new CheckNdimT<double>(1), new CheckSameTypesT<double>({0,1}, "shape"))
      .types(TypesFunctions::toMatrix<0,0,0>)
      .func(&EnergyResolutionInput::calcMatrix);

  add_transformation();
  add_input();
  set_open_input();
}

double EnergyResolutionInput::resolution(double Etrue, double Erec, double RelSigma) const noexcept {
  static const double twopisqr = std::sqrt(2*pi);
  const double sigma = Etrue * RelSigma;
  const double reldiff = (Etrue - Erec)/sigma;

  return std::exp(-0.5*pow(reldiff, 2))/(twopisqr*sigma);
}

void EnergyResolutionInput::calcMatrix(FunctionArgs& fargs) {
  m_sparse_cache.setZero();

  auto& args = fargs.args;
  auto* edges = args[0].type.edges.data();
  auto& relsigmas = args[1].x;
  auto& ret = fargs.rets[0];
  size_t nbins = ret.type.shape[0];

  m_sparse_cache.resize(nbins, nbins);

  /* fill the cache matrix with probalilities for number of events to leak to other bins */
  /* colums corressponds to reconstrucred energy and rows to true energy */
  auto bin_center = [edges](size_t index){ return (edges[index+1] + edges[index])/2; };
  for (size_t etrue = 0; etrue < nbins; ++etrue) {
    auto Etrue   = bin_center(etrue);
    auto dEtrue  = edges[etrue+1] - edges[etrue];
    auto relsigma=relsigmas[etrue];

    bool right_edge_reached{false};
    /* precalculating probabilities for events in given bin to leak to
     * neighbor bins  */
    for (size_t erec = 0; erec < nbins; ++erec) {
      auto Erec = bin_center(erec);
      auto rEvents = dEtrue*resolution(Etrue, Erec, relsigma);

      if (rEvents < 1E-10) {
        if (right_edge_reached) {
           break;
        }
        continue;
      }
      m_sparse_cache.insert(erec, etrue) = rEvents;
      if (!right_edge_reached) {
        right_edge_reached = true;
      }
    }
  }
  m_sparse_cache.makeCompressed();

  if ( m_propagate_matrix )
    fargs.rets[0].mat = m_sparse_cache;
}


#include <ros/ros.h>
#include <geometry_msgs/PoseArray.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <tf/transform_datatypes.h>
#include <boost/random.hpp>
#include <boost/random/normal_distribution.hpp>
#include <cmath>
#include <cylbot_motion_model/multivariate_normal.h>
#include <iostream>

#define NUM_POSES 1000

geometry_msgs::PoseArray pose_array;

Eigen::internal::scalar_normal_dist_op<double> randN;
//Eigen::internal::scalar_normal_dist_op<double>::rng.seed(1);

void initialPoseCallback(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr& pose_with_cov)
{
	std::cout << "TESTING" << std::endl;
	ROS_INFO("Received a new pose");

	Eigen::VectorXd mean(3);
	Eigen::MatrixXd covar(3, 3);

	mean << pose_with_cov->pose.pose.position.x,
		pose_with_cov->pose.pose.position.y,
		tf::getYaw(pose_with_cov->pose.pose.orientation);
	covar << pose_with_cov->pose.covariance[0], 0,                                  0,
		     0,                                 pose_with_cov->pose.covariance[7],  0,
		     0,                            0,                                       pose_with_cov->pose.covariance[35];

	Eigen::MatrixXd normTransform(3, 3);
	Eigen::LLT<Eigen::MatrixXd> cholSolver(covar);

	if(cholSolver.info() == Eigen::Success)
	{
		normTransform = cholSolver.matrixL();
	}
	else
	{
		Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> eigenSolver(covar);
		normTransform = eigenSolver.eigenvectors() * eigenSolver.eigenvalues().cwiseSqrt().asDiagonal();
	}

	Eigen::MatrixXd samples = (normTransform * Eigen::MatrixXd::NullaryExpr(3, NUM_POSES, randN)).colwise() + mean;

	ROS_INFO_STREAM("\nMean:\n" << mean << "\n");
	ROS_INFO_STREAM("\nCov:\n" << covar << "\n");

	// remove existing pose estimates
	pose_array.poses.clear();
	for(int i=0; i<NUM_POSES; i++)
	{
		geometry_msgs::Pose pose;
		pose.position.x = samples(0, i);
		pose.position.y = samples(1, i);
		pose.position.z = 0;

		// generate random yaw then convert it to a quaternion
		pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(0, 0, samples(2, i));

		pose_array.poses.push_back(pose);
	}
}

int main(int argc, char** argv)
{
	ros::init(argc, argv, "pose_array_test");
	ros::NodeHandle nh;

	ros::Publisher pose_array_pub = nh.advertise<geometry_msgs::PoseArray>("pose_array_test", 1);
	ros::Subscriber initial_pose_sub = nh.subscribe<geometry_msgs::PoseWithCovarianceStamped>("/initialpose", 10, &initialPoseCallback, ros::TransportHints().tcpNoDelay(false));

	// random number generator
	boost::mt19937 rng;

	// creates a normal distribution with 0 mean and std dev of 1
	boost::normal_distribution<> norm_dist(0.0, 1.0);

	// create a method of sampling the distribution
	boost::variate_generator<boost::mt19937, boost::normal_distribution<> > norm_rng(rng, norm_dist);

	pose_array.header.frame_id = "/map";

	ros::Rate loop_rate(10);
	while(ros::ok())
	{
		ros::spinOnce();

		pose_array.header.stamp = ros::Time::now();
		pose_array_pub.publish(pose_array);
		loop_rate.sleep();
	}

}