/** @file statistics.cc
 * @author David F. Gleich
 * @date 2006-04-21
 * @copyright Stanford University, 2006-2008
 * Graph statistics wrappers
 */

/** History
 *  2007-04-18: Implemented topological order function
 *  2007-07-05: Implemented matchings
 *  2007-07-05: Implemented core_numbers
 *  2007-07-11: Implemented directed and weighted clustering coefficients
 *  2007-07-12: Implemented dominator tree
 */

#include "include/matlab_bgl.h"

#include <yasmic/simple_csr_matrix_as_graph.hpp>
#include <yasmic/simple_row_and_column_matrix_as_graph.hpp>
#include <yasmic/iterator_utility.hpp>

#include <vector>

#include <boost/graph/iteration_macros.hpp>
//#include <boost/graph/betweenness_centrality.hpp>
#include <yasmic/boost_mod/betweenness_centrality.hpp>
#include <boost/graph/topological_sort.hpp>

#include <boost/iterator/reverse_iterator.hpp>

#include <boost/graph/max_cardinality_matching.hpp>
#include <yasmic/boost_mod/core_numbers.hpp>
#include <boost/graph/dominator_tree.hpp>

#include <iostream>

#include <math.h>

template <class Vertex, class IndMap>
struct in_indicator_pred
	: public std::unary_function<Vertex, bool>
{
	in_indicator_pred(IndMap indmap, Vertex src)
		: i(indmap), u(src)
	{}

	IndMap i;
    Vertex u;

	bool operator() (const Vertex &v) const
	{
		return (i[v] > 0) && (u != v);
	}
};

/* Clustering Coefficients Code */
template <class Graph, class CCMap, class IndMap>
void cluster_coefficients(const Graph& g, CCMap cc, IndMap ind)
{
	using namespace boost;

	BGL_FORALL_VERTICES_T(v,g,Graph)
	{
		cc[v] = 0;
		ind[v] = 0;
	}

	BGL_FORALL_VERTICES_T(v,g,Graph)
	{
		BGL_FORALL_ADJ_T(v,w,g,Graph)
		{
			ind[w] = 1;
		}

        ind[v] = 0;

		typename property_traits<CCMap>::value_type cur_cc = 0;
		typename graph_traits<Graph>::degree_size_type d = out_degree(v, g);

		BGL_FORALL_ADJ_T(v,w,g,Graph)
		{
            // if we are adjacent to ourselves, skip the iteration
            if (v == w) { --d; continue; }

            in_indicator_pred<
    			typename graph_traits<Graph>::vertex_descriptor,
			    IndMap> p(ind,w);

			typename graph_traits<Graph>::adjacency_iterator ai, aiend;
			boost::tie(ai, aiend) = adjacent_vertices(w,g);

			// count if this is in the indicator predicate
			cur_cc += (int)count_if(ai, aiend, p);
		}

        if (d > 1)
        {
		    cc[v] = (double)cur_cc/(double)((d*(d-1)));
        }
        else
        {
            cc[v] = 0.0;
        }

		// reset the indicator
		BGL_FORALL_ADJ_T(v,w,g,Graph)
		{
			ind[w] = 0;
		}
	}
}

template <class Graph, class CCMap, class EdgeWeightMap, class IndMap>
void undirected_clustering_coefficients(const Graph& g, CCMap cc, EdgeWeightMap wm,
    IndMap ind)
{
    using namespace boost;
    typedef typename graph_traits<Graph>::vertex_descriptor vertex;
    typename graph_traits<Graph>::vertex_iterator vi,vi_end;
    for (tie(vi,vi_end)=vertices(g);vi!=vi_end;++vi) {
        put(cc,*vi,0);
        put(ind,*vi,0);
    }
    // a lazy cache for the temp weights for each vertex, could be more
    // efficient using the maximum degree
    std::vector<typename property_traits<EdgeWeightMap>::value_type> cache(num_vertices(g));
    std::vector<typename graph_traits<Graph>::degree_size_type> degs;
    for (tie(vi,vi_end)=vertices(g);vi!=vi_end;++vi) {
        vertex v = *vi;
        typename graph_traits<Graph>::out_edge_iterator oi,oi_end;
        typename graph_traits<Graph>::out_edge_iterator oi2,oi2_end;
        for (tie(oi,oi_end)=out_edges(v,g);oi!=oi_end;++oi) {
            // check to ignore self edges
            if (target(*oi,g) != v) {
                put(ind,target(*oi,g),1);
                cache[target(*oi,g)] = pow(get(wm,*oi),1.0/3.0);
            }
        }
        typename property_traits<CCMap>::value_type cur_cc = 0;
        typename graph_traits<Graph>::degree_size_type d = out_degree(v,g);
        for (tie(oi,oi_end)=out_edges(v,g);oi!=oi_end;++oi) {
            vertex w = target(*oi,g);
            if (v==w) { --d; }
            for (tie(oi2,oi2_end)=out_edges(w,g);oi2!=oi2_end;++oi2) {
                if (target(*oi2,g) == w) { continue; }
                if (get(ind,target(*oi2,g))) {
                    // cache is cached with its power
                    cur_cc += pow(get(wm,*oi2),1.0/3.0)*pow(get(wm,*oi),1.0/3.0)*cache[target(*oi2,g)];
                }
            }
        }
        if (d > 1) {
            put(cc,v,cur_cc/((typename property_traits<CCMap>::value_type)(d*(d-1))));
        } else {
            put(cc,v,(typename property_traits<CCMap>::value_type)0);
        }
        for (tie(oi,oi_end)=out_edges(v,g);oi!=oi_end;++oi) {
            put(ind,target(*oi,g),0);
            cache[target(*oi,g)] = 0;
        }
    }
}

// graph must be a bidirectional graph
template <class Graph, class CCMap, class EdgeWeightMap, class IndMap>
void directed_clustering_coefficients(const Graph& g,
    CCMap cc, EdgeWeightMap wm, IndMap ind)
{
    using namespace boost;
    typedef typename property_traits<CCMap>::value_type value_type;
    typedef typename graph_traits<Graph>::vertex_descriptor vertex;
    typename graph_traits<Graph>::vertex_iterator vi,vi_end;
    for (tie(vi,vi_end)=vertices(g);vi!=vi_end;++vi) {
        put(cc,*vi,0);
        put(ind,*vi,0);
    }
    // a lazy cache for the temp weights for each vertex, could be more
    // efficient using the maximum degree
    std::vector<typename property_traits<EdgeWeightMap>::value_type> cache(num_vertices(g));
    std::vector<typename graph_traits<Graph>::degree_size_type> degs(num_vertices(g));
    typename graph_traits<Graph>::edge_iterator ei,ei_end;
    for (tie(ei,ei_end)=edges(g);ei!=ei_end;++ei) {
        vertex u = source(*ei,g);
        vertex v = target(*ei,g);
        if (u==v) { continue; }
        degs[u]++;
        degs[v]++;
    }
    for (tie(vi,vi_end)=vertices(g);vi!=vi_end;++vi) {
        vertex v = *vi;
        typename graph_traits<Graph>::out_edge_iterator oi,oi_end;
        typename graph_traits<Graph>::out_edge_iterator oi2,oi2_end;
        typename graph_traits<Graph>::in_edge_iterator ii, ii_end;
        for (tie(ii,ii_end)=in_edges(v,g);ii!=ii_end;++ii) {
            if (source(*ii,g) != v) {
                put(ind,source(*ii,g),1);
                cache[source(*ii,g)] += pow(get(wm,*ii),1.0/3.0);
            }
        }
        // we've precomputed the set of in-edges, so we know how
        // to get back to v to complete a triangle that ends
        // with an edge to v.
        typename graph_traits<Graph>::degree_size_type bilateral_edges = 0;
        value_type cur_cc_cyc=0, cur_cc_mid=0, cur_cc_in=0, cur_cc_out=0;
        // cycles are out-out-out.
        for (tie(oi,oi_end)=out_edges(v,g);oi!=oi_end;++oi) {
            vertex w = target(*oi,g);
            if (v == w) { continue; }
            value_type cache_w = pow(get(wm,*oi),1.0/3.0);
            for (tie(oi2,oi2_end)=out_edges(w,g);oi2!=oi2_end;++oi2) {
                vertex u = target(*oi2,g);
                if (u == v) { ++bilateral_edges; continue; }
                if (u == w) { continue; }
                if (get(ind,u)) {
                    // cache is cached with its power
                    cur_cc_cyc += pow(get(wm,*oi2),1.0/3.0)*cache_w*cache[u];
                }
            }
        }
        for (tie(oi,oi_end)=out_edges(v,g);oi!=oi_end;++oi) {
            vertex w = target(*oi,g);
            if (v == w) { continue; }
            value_type cache_w = pow(get(wm,*oi),1.0/3.0);
            for (tie(ii,ii_end)=in_edges(w,g);ii!=ii_end;++ii) {
                vertex u = source(*ii,g);
                if (u == w) { continue; }
                if (get(ind,u)) {
                    // cache is cached with its power
                    cur_cc_mid += pow(get(wm,*ii),1.0/3.0)*cache_w*cache[u];
                }
            }
        }
        for (tie(ii,ii_end)=in_edges(v,g);ii!=ii_end;++ii) {
            vertex w = source(*ii,g);
            if (v == w) { continue; }
            value_type cache_w = pow(get(wm,*ii),1.0/3.0);
            for (tie(oi,oi_end)=out_edges(w,g);oi!=oi_end;++oi) {
                vertex u = target(*oi,g);
                if (u == w) { continue; }
                if (get(ind,u)) {
                    // cache is cached with its power
                    cur_cc_in += pow(get(wm,*oi),1.0/3.0)*cache_w*cache[u];
                }
            }
        }

        // reset the cache
        for (tie(ii,ii_end)=in_edges(v,g);ii!=ii_end;++ii) {
            if (source(*ii,g) != v) {
                put(ind,source(*ii,g),0);
                cache[source(*ii,g)] = 0;
            }
        }

        // re-init the cache with out-edges
        for (tie(oi,oi_end)=out_edges(v,g);oi!=oi_end;++oi) {
            // check to ignore self edges
            if (target(*oi,g) != v) {
                put(ind,target(*oi,g),1);
                cache[target(*oi,g)] += pow(get(wm,*oi),1.0/3.0);
            }
        }

        for (tie(oi,oi_end)=out_edges(v,g);oi!=oi_end;++oi) {
            vertex w = target(*oi,g);
            if (v == w) { continue; }
            value_type cache_w = pow(get(wm,*oi),1.0/3.0);
            for (tie(oi2,oi2_end)=out_edges(w,g);oi2!=oi2_end;++oi2) {
                vertex u = target(*oi2,g);
                if (u == w) { continue; }
                if (get(ind,u)) {
                    // cache is cached with its power
                    cur_cc_out += pow(get(wm,*oi2),1.0/3.0)*cache_w*cache[u];
                }
            }
        }

        // store the value
        typename graph_traits<Graph>::degree_size_type
            norm_factor = degs[v]*(degs[v] - 1) - 2*bilateral_edges;
        if (norm_factor > 0) {
            put(cc,v,
                (value_type)(cur_cc_cyc + cur_cc_mid + cur_cc_in + cur_cc_out)/
                    ((value_type)(norm_factor)));
        } else {
            put(cc,v,(value_type)0);
        }

        //std::cout << std::endl;
        //std::cout << v << " " <<  cur_cc_cyc << " " << cur_cc_mid << " " << cur_cc_in << " " << cur_cc_out << " " << degs[v] << " " << bilateral_edges << std::endl;

        for (tie(oi,oi_end)=out_edges(v,g);oi!=oi_end;++oi) {
            // check to ignore self edges
            if (target(*oi,g) != v) {
                put(ind,target(*oi,g),0);
                cache[target(*oi,g)] = 0;
            }
        }
    }
}

int clustering_coefficients(
    mbglIndex nverts, mbglIndex *ja, mbglIndex *ia, /* connectivity params */
    double* ccfs, int directed)
{
    using namespace yasmic;
    using namespace boost;

    typedef simple_csr_matrix<mbglIndex,double> crs_matrix;
    crs_matrix g(nverts, nverts, ia[nverts], ia, ja, NULL);

    std::vector<int> indicator_map(num_vertices(g));

    if (directed) {
        std::vector<mbglIndex> ati(nverts+1);
        std::vector<mbglIndex> atj(ia[nverts]);
        std::vector<mbglIndex> atid(ia[nverts]);

        build_row_and_column_from_csr(g, &ati[0], &atj[0], &atid[0]);

        typedef simple_row_and_column_matrix<mbglIndex,double> bidir_graph;
        bidir_graph bg(nverts, nverts, ia[nverts], ia, ja, NULL, &ati[0], &atj[0], &atid[0]);

        directed_clustering_coefficients(bg,
            make_iterator_property_map(ccfs, get(vertex_index,bg)),
            boost::detail::constant_value_property_map<double>(1.0),
            make_iterator_property_map(indicator_map.begin(),get(vertex_index,bg)));
    } else {
        undirected_clustering_coefficients(g,
            make_iterator_property_map(ccfs, get(vertex_index,g)),
            boost::detail::constant_value_property_map<double>(1.0),
            make_iterator_property_map(indicator_map.begin(), get(vertex_index,g)));
    }

    return (0);
}

int weighted_clustering_coefficients(
    mbglIndex nverts, mbglIndex *ja, mbglIndex *ia, double *weight, /* connectivity params */
    double* ccfs)
{
    using namespace yasmic;
    using namespace boost;

    typedef simple_csr_matrix<mbglIndex,double> crs_weighted_graph;
    crs_weighted_graph g(nverts, nverts, ia[nverts], ia, ja, weight);

    std::vector<int> indicator_map(num_vertices(g));

    undirected_clustering_coefficients(g,
        make_iterator_property_map(
            ccfs, get(vertex_index,g)),
        get(edge_weight,g),
        make_iterator_property_map(
            indicator_map.begin(), get(vertex_index,g)));

    return (0);
}

int directed_clustering_coefficients(
    mbglIndex nverts, mbglIndex *ja, mbglIndex *ia, double *weight, /* connectivity params */
    double* ccfs)
{
    using namespace yasmic;
    using namespace boost;

    typedef simple_csr_matrix<mbglIndex,double> crs_matrix;
    crs_matrix g(nverts, nverts, ia[nverts], ia, ja, weight);

    // early return for trivial input
    if (nverts==0) { return (0); }

    std::vector<mbglIndex> ati(nverts+1);
    std::vector<mbglIndex> atj(ia[nverts]);
    std::vector<mbglIndex> atid(ia[nverts]);

    build_row_and_column_from_csr(g, &ati[0], &atj[0], &atid[0]);

    typedef simple_row_and_column_matrix<mbglIndex,double> bidir_graph;
    bidir_graph bg(nverts, nverts, ia[nverts], ia, ja, weight, &ati[0], &atj[0], &atid[0]);

    std::vector<int> indicator_map(num_vertices(g));

    directed_clustering_coefficients(bg,
        make_iterator_property_map(
            ccfs, get(vertex_index,bg)),
        get(edge_weight,bg),
        make_iterator_property_map(
            indicator_map.begin(),
            get(vertex_index,bg)));

    return (0);
}

int betweenness_centrality(
    mbglIndex nverts, mbglIndex *ja, mbglIndex *ia, double *weight, /* connectivity params */
    double *centrality, double *ecentrality)
{
    using namespace yasmic;
    using namespace boost;

    typedef simple_csr_matrix<mbglIndex,double> crs_graph;
    crs_graph g(nverts, nverts, ia[nverts], ia, ja, weight);

    if (weight)
    {
        if (ecentrality) {
            brandes_betweenness_centrality(g,
	        		centrality_map(centrality)
                    .weight_map(
                        make_iterator_property_map(weight,
                            get(edge_index,g)))
                    .edge_centrality_map(
                        make_iterator_property_map(
                            ecentrality, get(edge_index, g))));
        } else {
            brandes_betweenness_centrality(g,
	        		centrality_map(centrality)
                    .weight_map(
                        make_iterator_property_map(weight,
                            get(edge_index,g))));
        }
    }
    else
    {
        if (ecentrality) {
            brandes_betweenness_centrality(g,
			    	centrality_map(centrality)
                    .edge_centrality_map(
                        make_iterator_property_map(
                            ecentrality, get(edge_index, g))));
        } else {
            brandes_betweenness_centrality(g,
			    	make_iterator_property_map(centrality,
				    	get(vertex_index, g)));
        }
    }

    return (0);
}

/**
 * Test for a topological order or topological sort of a graph.
 *
 * This function will also test if the graph is a dag.  If the
 * rev_order parameter is null, then the actual topological order
 * is ignored and the function just tests for a dag.
 *
 * @param nverts the number of vertices in the graph
 * @param ja the connectivity for each vertex
 * @param ia the row connectivity points into ja
 * @param topo_order a topological order of the vertices
 * @param is_dag tests if the graph is a dag (optional)
 */
int topological_order(
    mbglIndex nverts, mbglIndex *ja, mbglIndex *ia, /* connectivity params */
    mbglIndex *topo_order, int *is_dag)
{
    using namespace yasmic;
    using namespace boost;

    typedef simple_csr_matrix<mbglIndex,double> crs_graph;
    crs_graph g(nverts, nverts, ia[nverts], ia, ja, NULL);

    if (is_dag) {
        *is_dag = 1;
    }

    if (topo_order == NULL)
    {
        return (-1);
    }
    else
    {
        typedef reverse_iterator<mbglIndex*>
            reverse_iter;

        // note for the future, boost's reverse iterator is wild in that
        // it actually dereferences the PRIOR element, so the correct
        // range of a boost reverse itertor is the same as the original
        // iterator.  Consequently, the last element is actual topo_order +
        // nverts because the previous element (what * returns) is the
        // correct element.
        reverse_iter output(topo_order + (nverts));

        try {
            topological_sort(g, output);
        } catch (not_a_dag) {
            if (is_dag) {
                *is_dag = 0;
            }
        }
    }

    return (0);
}

template <typename Graph,
        typename MateMap,
        typename VertexIndexMap>
bool matching_help(const Graph& g, MateMap mate, VertexIndexMap vm,
                   int initial_matching, int augmenting_path, int verify)
{
    using namespace boost;
    if (initial_matching == 1)
    {
        if (augmenting_path == 1)
            if (verify == 1)
                return matching<Graph,MateMap,VertexIndexMap,
                        no_augmenting_path_finder,
                        empty_matching,
                        no_matching_verifier>
                        (g, mate, vm);
            else
                return matching<Graph,MateMap,VertexIndexMap,
                        no_augmenting_path_finder,
                        empty_matching,
                        maximum_cardinality_matching_verifier>
                        (g, mate, vm);
        else
            if (verify == 1)
                return matching<Graph,MateMap,VertexIndexMap,
                        edmonds_augmenting_path_finder,
                        empty_matching,
                        no_matching_verifier>
                        (g, mate, vm);
            else
                return matching<Graph,MateMap,VertexIndexMap,
                        edmonds_augmenting_path_finder,
                        empty_matching,
                        maximum_cardinality_matching_verifier>
                        (g, mate, vm);
    }
    else if (initial_matching == 2)
    {
        if (augmenting_path == 1)
            if (verify == 1)
                return matching<Graph,MateMap,VertexIndexMap,
                        no_augmenting_path_finder,
                        greedy_matching,
                        no_matching_verifier>
                        (g, mate, vm);
            else
                return matching<Graph,MateMap,VertexIndexMap,
                        no_augmenting_path_finder,
                        greedy_matching,
                        maximum_cardinality_matching_verifier>
                        (g, mate, vm);
        else
            if (verify == 1)
                return matching<Graph,MateMap,VertexIndexMap,
                        edmonds_augmenting_path_finder,
                        greedy_matching,
                        no_matching_verifier>
                        (g, mate, vm);
            else
                return matching<Graph,MateMap,VertexIndexMap,
                        edmonds_augmenting_path_finder,
                        greedy_matching,
                        maximum_cardinality_matching_verifier>
                        (g, mate, vm);
    }
    else
    {
        if (augmenting_path == 1)
            if (verify == 1)
                return matching<Graph,MateMap,VertexIndexMap,
                        no_augmenting_path_finder,
                        extra_greedy_matching,
                        no_matching_verifier>
                        (g, mate, vm);
            else
                return matching<Graph,MateMap,VertexIndexMap,
                        no_augmenting_path_finder,
                        extra_greedy_matching,
                        maximum_cardinality_matching_verifier>
                        (g, mate, vm);
        else
            if (verify == 1)
                return matching<Graph,MateMap,VertexIndexMap,
                        edmonds_augmenting_path_finder,
                        extra_greedy_matching,
                        no_matching_verifier>
                        (g, mate, vm);
            else
                return matching<Graph,MateMap,VertexIndexMap,
                        edmonds_augmenting_path_finder,
                        extra_greedy_matching,
                        maximum_cardinality_matching_verifier>
                        (g, mate, vm);
    }
}

/** Wrap a boost graph library call to matching.
 *
 * A matching is a subset of edges with no common vertices in an undirected
 * graph.
 *
 * A maximum cardinality matching has the largest number of edges over all
 * possible matchings.
 *
 * The graph must be undirected.
 *
 * @param nverts the number of vertices in the graph
 * @param ja the connectivity for each vertex
 * @param ia the row connectivity points into ja
 * @param mate an array of size nverts which stores the matching vertex index
 *  or null vertex if there is no match
 * @param initial_matching indicates the initial matching used in the algorithm
 *  1: no_matching
 *  2: greedy_matching
 *  3: extra_greedy_matching
 * @param augmenting_path indicates the algorithm used to find augmenting paths
 *  1: no_augmenting_path_finder
 *  2: edmonds_augmenting_path_finder
 * @param verify indicates if we verify the algorithm ouput
 *  1: no_matching_verifier
 *  2: maximum_cardinality_matching_verifier
 * @param verified the output of the verification algorithm (if run), or
 *  false otherwise
 * @param null_vertex the special index to indicate an unmatched vertex
 * @return an error code if possible
 *   0: indicates success
 *  -1: indicates a parameter error with initial_matching, augmenting_path, or verify
 */
int maximum_cardinality_matching(
    mbglIndex nverts, mbglIndex *ja, mbglIndex *ia, /* connectivity params */
    mbglIndex* mate, int initial_matching, int augmenting_path, int verify,
    int *verified, mbglIndex *null_vertex)
{
    using namespace yasmic;
    using namespace boost;

    typedef simple_csr_matrix<mbglIndex,double> crs_graph;
    crs_graph g(nverts, nverts, ia[nverts], ia, ja, NULL);

    if (initial_matching < 1 || initial_matching > 3 ||
        augmenting_path < 1 || augmenting_path > 2 ||
        verify < 1 || verify > 2)
    {
        return (-1);
    }

    bool max_matching =
        matching_help(g,mate,get(vertex_index,g),
            initial_matching,augmenting_path,verify);

    if (verified) {
        *verified = 0;
        if (verify > 1) { *verified = max_matching; }
    }

    if (null_vertex) {
        *null_vertex = graph_traits<crs_graph>::null_vertex();
    }

    return (0);
}

template <typename Graph, typename MateMap, typename VertexIndexMap>
bool verify_matching_help(const Graph& g, MateMap mate, VertexIndexMap vm)
{
    return boost::maximum_cardinality_matching_verifier<
                    Graph,MateMap,VertexIndexMap>::verify_matching(g,mate,vm);
}

/*
 * mate = nverts if unmatched
 */
int test_maximum_cardinality_matching(
    mbglIndex nverts, mbglIndex *ja, mbglIndex *ia, /* connectivity params */
    mbglIndex* mate, int *verified)
{
    using namespace yasmic;
    using namespace boost;

    typedef simple_csr_matrix<mbglIndex,double> crs_graph;
    crs_graph g(nverts, nverts, ia[nverts], ia, ja, NULL);

    // set the null_vertex to the true null
    mbglIndex true_null = graph_traits<crs_graph>::null_vertex();
    for (mbglIndex v=0;v<num_vertices(g);++v) {
        if (mate[v] == nverts) { mate[v] = true_null; }
    }

    bool max_matching = verify_matching_help(g, mate, get(vertex_index,g));
    if (verified) { *verified = 0; }
    if (max_matching && verified) { *verified = 1; }

    return (0);
}

/** Compute the core_numbers of a graph
 *
 * For an undirected graph, this function computes the core number of each
 * vertex.  The core number is the minimum number such that removing all
 * vertices of degree <= cn[k] removes vertex k.  For a directed graph
 * we compute the in-degree core number.
 *
 * @param nverts the number of vertices in the graph
 * @param ja the connectivity for each vertex
 * @param ia the row connectivity points into ja
 * @param cn an array of core numbers, length nverts
 * @param rt an array of removal times, length nverts
 * @return an error code if possible
 */
int core_numbers(
    mbglIndex nverts, mbglIndex *ja, mbglIndex *ia, /* connectivity params */
    mbglDegreeType *cn, int *rt)
{
    // History
    //
    // 30 July 2007
    // added removal time visitor
    // changed to mbglDegreeType
    using namespace yasmic;
    using namespace boost;

    if (!cn || !rt) { return (-1); }

    typedef simple_csr_matrix<mbglIndex,double> crs_graph;
    crs_graph g(nverts, nverts, ia[nverts], ia, ja, NULL);

    int time = 0;

    core_numbers(g, make_iterator_property_map(cn, get(vertex_index,g)),
        make_core_numbers_visitor(stamp_times(rt, time, on_examine_vertex())));

    return (0);
}

/** Compute the weighted core numbers of a graph
 *
 * For an undirected graph, this function computes the core number of each
 * vertex.  The core number is the minimum number such that removing all
 * vertices of weighted in-degree <= cn[k] removes vertex k.  For a
 * directed graph we compute the weighted in-degree core number.
 *
 * @param nverts the number of vertices in the graph
 * @param ja the connectivity for each vertex
 * @param ia the row connectivity points into ja
 * @param cn an array of core numbers, length nverts
 * @param rt an array of removal times, length nverts
 * @return an error code if possible
 */
int weighted_core_numbers(
    mbglIndex nverts, mbglIndex *ja, mbglIndex *ia, double *weight, /* connectivity params */
    double *cn, int *rt)
{
    // History
    //
    // 30 July 2007
    // added removal time visitor
    using namespace yasmic;
    using namespace boost;

    typedef simple_csr_matrix<mbglIndex,double> crs_graph;
    crs_graph g(nverts, nverts, ia[nverts], ia, ja, weight);

    int time=0;

    weighted_core_numbers(g, make_iterator_property_map(cn, get(vertex_index,g)),
        make_core_numbers_visitor(stamp_times(rt, time, on_examine_vertex())));

    return (0);
}

int dominator_tree(
    mbglIndex nverts, mbglIndex *ja, mbglIndex *ia,
    mbglIndex src, mbglIndex *pred)
{
    //
    // History
    //
    // 12 July 2007
    // Initial implementation
    //
    // 23 July 2007
    // Added iterator property map call for pred.
    //
    using namespace yasmic;
    using namespace boost;

    typedef simple_csr_matrix<mbglIndex,double> crs_graph;
    crs_graph g(nverts, nverts, ia[nverts], ia, ja, NULL);

    std::vector<mbglIndex> ati(nverts+1);
    std::vector<mbglIndex> atj(ia[nverts]);
    std::vector<mbglIndex> atid(ia[nverts]);

    build_row_and_column_from_csr(g, &ati[0], &atj[0], &atid[0]);

    typedef simple_row_and_column_matrix<mbglIndex,double> bidir_graph;
    bidir_graph bg(nverts, nverts, ia[nverts], ia, ja, NULL, &ati[0], &atj[0], &atid[0]);

    // modify the output to conform to what matlab_bgl expects
    mbglIndex null_vertex = graph_traits<bidir_graph>::null_vertex();
    for (mbglIndex i=0; i< nverts; i++) { pred[i] = null_vertex; }

    lengauer_tarjan_dominator_tree(bg, src,
        make_iterator_property_map(pred, get(vertex_index,bg)));

    pred[src] = src;

    // modify the output to conform to what matlab_bgl expects
    for (mbglIndex i=0; i< nverts; i++) {
        if (pred[i] == null_vertex) { pred[i] = i; }
    }

    return (0);
}


#include <iostream>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/filtered_graph.hpp>
#include <boost/graph/graph_utility.hpp>
using namespace boost;
using namespace std;
//In this example we will  keep only edges with positive edge weight.
//First, we create a predicate function object.

//"typename" is used for specifying that a dependent name in a template definition or declaration is a type.
template <typename EdgeWeightMap>
struct positive_edge_weight
{
  //constructor
  positive_edge_weight() { }
  positive_edge_weight(EdgeWeightMap weight) : m_weight(weight) { }

  template <typename Edge>

  bool operator()(const Edge& e) const
  {
    //testing if weight is positive
    return 0 < get(m_weight, e);
  }

  EdgeWeightMap m_weight;
};

//Now we create a graph and print out the filtered graph.
int main()
{

  //defining the graph by an adjacency_list
  typedef adjacency_list<vecS, vecS, directedS, no_property, property<edge_weight_t, int> > Graph;

  //define a syntax for mapping key objects (edge_weight_t) to corresponding value objects (EdgeWeightMap).
  typedef property_map<Graph, edge_weight_t>::type EdgeWeightMap;

  // a distinct type whose value is restricted to one of several explicitly named constants
  enum { A, B, C, D, E, N };

  const char* name = "ABCDE";

  Graph g(N);
  add_edge(A, B, 2, g);
  add_edge(A, C, 4, g);
  add_edge(C, D, 1, g);
  add_edge(D, B, -3, g);
  add_edge(C, E, 0, g);
  add_edge(E, C, 0, g);

  //filter()bprocesses a data structure (typically a list) in some order to produce a new data structure containing exactly those elements of the original data structure.
  positive_edge_weight<EdgeWeightMap> filter(get(edge_weight, g));
  filtered_graph<Graph, positive_edge_weight<EdgeWeightMap> > fg(g, filter);

  cout << "filtered edge set: ";
  print_edges(fg, name);

  return 0;
}


// BLAS level 1 (vector) -- complex numbers

#include <iostream>
#include <cmath>
#include <complex>

//#define BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS
//#define BOOST_NO_FUNCTION_TEMPLATE_ORDERING

#include <boost/numeric/bindings/traits/ublas_vector.hpp>
#include <boost/numeric/bindings/atlas/cblas1.hpp>
#include "utils.h"

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

using std::cout;
using std::endl;

typedef double real_t;
typedef std::complex<real_t> cmplx_t;
typedef ublas::vector<cmplx_t> vct_t;

#ifdef BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS
using ublas::inner_prod;
using ublas::norm_2;
using ublas::conj;
#endif

int main() {

  int n = 6;

  cout << endl;
  vct_t v (n);
  init_v (v, times_plus<cmplx_t> (cmplx_t (1, -1), cmplx_t (0, .1)));
  print_v (v, "v");

  atlas::scal (2.0, v);
  print_v (v, "2.0 v");

  atlas::scal (cmplx_t (-1, 0), v);
  print_v (v, "(-1, 0) v");

  atlas::scal (cmplx_t (0, 1), v);
  print_v (v, "(0, 1) v");

  atlas::set (cmplx_t (1, -1), v);
  print_v (v, "v");
  vct_t v1 (n);
  atlas::set (cmplx_t (0, -1), v1);
  print_v (v1, "v1");

  cout << endl;
  cout << "v^T v1 = " << atlas::dot (v, v1) << " == "
    << atlas::dotu (v, v1) << " == "
    << inner_prod (v, v1) << endl;
  cout << "v^T v = " << atlas::dot (v, v) << " == "
    << atlas::dotu (v, v) << " == "
    << inner_prod (v, v) << endl;
  cout << "v1^T v1 = " << atlas::dot (v1, v1) << " == "
    << atlas::dotu (v1, v1) << " == "
    << inner_prod (v1, v1) << endl;

  cout << endl;
  cout << "v^H v1 = " << atlas::dotc (v, v1) << " == "
    << inner_prod (conj (v), v1) << " == "
    << inner_prod (v1, conj (v)) << " != "
    << inner_prod (v, conj (v1)) << endl;
  cout << "v^H v = " << atlas::dotc (v, v) << " == "
    << inner_prod (conj (v), v) << " == "
    << inner_prod (v, conj (v)) << endl;
  cout << "v1^H v1 = " << atlas::dotc (v1, v1) << " == "
    << inner_prod (conj (v1), v1) << " == "
    << inner_prod (v1, conj (v1)) << endl;

  cout << endl;
  cout << "||v||_1 = " << atlas::asum (v) << endl;
  cout << "||v||_2 = " << atlas::nrm2 (v) << " == "
    << norm_2 (v) << endl;

  cout << endl;
}


#include <stdlib.h>
#include <iostream>
#include <fstream>
#include <cmath>
#include <ctime>
#include <chrono>
//#include <random>

#include <boost/multiprecision/cpp_int.hpp>
#include <boost/random.hpp>
//#include <gmp.h>
#include <boost/multiprecision/gmp.hpp>
//#include <boost/multiprecision/random.hpp>

using namespace boost::multiprecision;
using namespace boost::random;
using namespace std;

#include "ecc_gadget.hpp"
#include "scalarmul.hpp"

//using namespace libsnark;
using namespace std;

int main()
{
  typedef libff::alt_bn128_pp ppT;
  typedef libff::Fr<ppT> FieldT;

  // Initialize the curve parameters
  ppT::init_public_params();
  init_curveparams();

  // Create protoboard

  libff::start_profiling();

  cout << "Keypair" << endl;

  protoboard<FieldT> pb;
  pb_variable<FieldT> outx, outy;
  pb_variable<FieldT> s;

  // The constant base point P
  //const FieldT Px = curveParams<FieldT>::Gx;
  //const FieldT Py = curveParams<FieldT>::Gy;

  // Allocate variables

  outx.allocate(pb, "outx");
  outy.allocate(pb, "outy");
  s.allocate(pb, "s");

  // This sets up the protoboard variables so that the first n of them
  // represent the public input and the rest is private input

  pb.set_input_sizes(2);

  // Initialize the gadget
  scalarmul_gadget<FieldT> sm(pb, outx, outy, s, 256);
  sm.generate_r1cs_constraints();

  const r1cs_constraint_system<FieldT> constraint_system = pb.get_constraint_system();

  const r1cs_gg_ppzksnark_keypair<default_r1cs_gg_ppzksnark_pp> keypair = r1cs_gg_ppzksnark_generator<default_r1cs_gg_ppzksnark_pp>(constraint_system);

  // Add witness values

  cout << "Prover" << endl;

  //srand(time(0));
  //pb.val(s) = 0;
  //for(int i = 0; i < 10; i++){
      //pb.val(s) += FieldT::random_element();
      //cout << pb.val(s) << " " << sizeof(pb.val(s)) << "\n" ;
  //}
  pb.val(s) = FieldT::random_element();
  sm.generate_r1cs_witness();

  const r1cs_gg_ppzksnark_proof<default_r1cs_gg_ppzksnark_pp> proof = r1cs_gg_ppzksnark_prover<default_r1cs_gg_ppzksnark_pp>(keypair.pk, pb.primary_input(), pb.auxiliary_input());

  cout << "Verifier" << endl;

  bool verified = r1cs_gg_ppzksnark_verifier_strong_IC<default_r1cs_gg_ppzksnark_pp>(keypair.vk, pb.primary_input(), proof);

  cout << "Number of R1CS constraints: " << constraint_system.num_constraints() << endl;
  cout << "Primary (public) input: " << pb.primary_input() << endl;
  cout << "Auxiliary (private) input length: " << pb.auxiliary_input().size() << endl;
//  cout << "Auxiliary (private) input: " << pb.auxiliary_input() << endl;
  cout << "Verification status: " << verified << endl;
  cout << "Computing " << pb.val(s) << "*G" << endl;

  //pb.val(s) += FieldT::random_element();

  ofstream pkfile("pk_scalarmul");
  pkfile << keypair.pk;
  pkfile.close();
  ofstream vkfile("vk_scalarmul");
  vkfile << keypair.vk;
  vkfile.close();
  ofstream pffile("proof_scalarmul");
  pffile << proof;
  pffile.close();

  cout << pb.val(s) << "*G" << " = (" << pb.val(outx) << ", " << pb.val(outy) << ")" << endl;
  //cout << "G" << " = (" << Px << ", " << Py << ")" << endl;
  //cout << FieldT::numbits;
  return 0;
}


/*!@file
 * @copyright This code is licensed under the 3-clause BSD license.
 *   Copyright ETH Zurich, Laboratory of Physical Chemistry, Reiher Group.
 *   See LICENSE.txt for details.
 */

#include <boost/test/unit_test.hpp>

#include "Molassembler/Temple/Optimization/Lbfgs.h"

using namespace Scine::Molassembler;

template<typename FloatType>
struct GradientBasedChecker {
  unsigned iterLimit = 100;
  FloatType gradientLimit = 1e-5;

  template<typename StepValues>
  bool shouldContinue(unsigned iteration, const StepValues& step) {
    return (
      iteration < iterLimit
      && step.gradients.current.norm() > gradientLimit
    );
  }
};

BOOST_AUTO_TEST_CASE(LBFGSSimpleMinimization, *boost::unit_test::label("Temple")) {
  /* Booth function
   * Ellipsoid f(x, y) = (x + 2y - 7)² + (2x + y - 5)²
   * Minimum at x = 1, y = 3
   */
  const auto gradientTestFunction = [](const Eigen::VectorXd& parameters, double& value, Eigen::Ref<Eigen::VectorXd> gradients) {
    const double& x = parameters[0];
    const double& y = parameters[1];
    const double firstBracket = x + 2 * y - 7;
    const double secondBracket = 2 * x + y - 5;

    value = (
      std::pow(firstBracket, 2)
      + std::pow(secondBracket, 2)
    );
    gradients[0] = 2 * firstBracket + 4 * secondBracket;
    gradients[1] = 4 * firstBracket + 2 * secondBracket;
  };

  Temple::Lbfgs<double, 16> optimizer;
  GradientBasedChecker<double> gradientChecker;
  Eigen::VectorXd positions(2);
  positions[0] = 0.25 * M_PI;
  positions[1] = 0.75 * M_PI;

  const auto result = optimizer.minimize(positions, gradientTestFunction, gradientChecker);

  BOOST_CHECK_MESSAGE(
    result.iterations < 100,
    "Expected convergence in less than 100 cycles, got " << result.iterations
  );
  BOOST_CHECK(std::fabs(positions[0] - 1.0) < 1e-3);
  BOOST_CHECK(std::fabs(positions[1] - 3.0) < 1e-3);
}

BOOST_AUTO_TEST_CASE(LBFGSSimpleMaximization, *boost::unit_test::label("Temple")) {
  /* Very simple parabola -((x-4)² + (y-2)²) + 4
   * Maximum at 4, 2
   */
  const auto gradientTestFunction = [](const Eigen::VectorXd& parameters, double& value, Eigen::Ref<Eigen::VectorXd> gradients) {
    const double& x = parameters[0];
    const double& y = parameters[1];

    value = -(std::pow(x - 4, 2) + std::pow(y - 2, 2)) + 4;
    gradients[0] = -2 * (x - 4);
    gradients[1] = -2 * (y - 2);
  };

  Temple::Lbfgs<double, 16> optimizer;
  GradientBasedChecker<double> gradientChecker;

  Eigen::VectorXd positions(2);
  positions << 2.0, -1.0;

  Eigen::VectorXd expectedMaximum(2);
  expectedMaximum << 4.0, 2.0;

  const auto result = optimizer.maximize(positions, gradientTestFunction, gradientChecker);

  BOOST_CHECK_MESSAGE(
    result.iterations < 100,
    "Expected convergence in less than 100 cycles, got " << result.iterations
  );

  for(unsigned i = 0; i < 2; ++i) {
    BOOST_CHECK_MESSAGE(
      std::fabs(positions[i] - expectedMaximum[i]) < 1e-3,
      "Position parameter " << i << " is not at the expected maximum."
      << " Expected " << expectedMaximum[i] << ", got " << positions[i]
    );
  }
}

BOOST_AUTO_TEST_CASE(LBFGSBraninMinimization, *boost::unit_test::label("Temple")) {
  /* f(x, y) = (-1.275 * x² / pi² + 4 x / pi + y - 6)² + (10 - 5 / (4 pi)) * cos(x) + 10
   * Minima at (2n pi, 2n pi)
   */
  const auto gradientTestFunction = [](const Eigen::VectorXd& parameters, double& value, Eigen::Ref<Eigen::VectorXd> gradients) {
    const double& x = parameters[0];
    const double& y = parameters[1];

    constexpr double alpha = -1.275 / (M_PI * M_PI);
    constexpr double beta = 4 / M_PI;
    constexpr double cosinePrefactor = (10 - 5 / (4 * M_PI));

    const double polynomialBracket = (alpha * x * x + beta * x + y - 6);

    value = (
      std::pow(polynomialBracket, 2)
      + cosinePrefactor * std::cos(x)
      + 10
    );

    gradients[0] = 2 * polynomialBracket * (2 * alpha * x + beta) - cosinePrefactor * std::sin(x);
    gradients[1] = 2 * polynomialBracket;
  };

  using OptimizerType = Temple::Lbfgs<double, 16>;
  OptimizerType optimizer;
  GradientBasedChecker<double> gradientChecker;
  Eigen::VectorXd positions(2);
  positions << M_PI - 0.1, M_PI - 0.1;

  constexpr double expectedResult = 0.39788735772973816;

  const auto result = optimizer.minimize(
    positions,
    gradientTestFunction,
    gradientChecker
  );

  BOOST_CHECK_MESSAGE(
    result.iterations < 100,
    "Expected convergence in less than 100 cycles, got " << result.iterations
  );

  BOOST_CHECK_MESSAGE(
    std::fabs(result.value - expectedResult) < 1e-3,
    "Expected minimum of value " << expectedResult << ", got " << result.value << " instead"
  );
}

BOOST_AUTO_TEST_CASE(LBFGSBoxedMinimization, *boost::unit_test::label("Temple")) {
  /* f(x, y) = - cos x - 0.5 cos y
   * Minima at (2n pi, 2n pi)
   * In box min [0.1, 0], max [pi, pi] minimum should be [0.1, 0]
   */
  const auto gradientTestFunction = [](const Eigen::VectorXd& parameters, double& value, Eigen::Ref<Eigen::VectorXd> gradients) {
    const double& x = parameters[0];
    const double& y = parameters[1];

    value = (
      - std::cos(x)
      - 0.5 * std::cos(y)
    );
    gradients[0] = std::sin(x);
    gradients[1] = 0.5 * std::sin(y);
  };

  using OptimizerType = Temple::Lbfgs<double, 16>;

  OptimizerType optimizer;
  GradientBasedChecker<double> gradientChecker;
  Eigen::VectorXd positions(2);
  Eigen::VectorXd boxMinima(2);
  Eigen::VectorXd boxMaxima(2);
  boxMinima << 0.1, 0;
  positions << 0.6, 0.5;
  boxMaxima << M_PI, M_PI;

  const typename OptimizerType::Box box {
    boxMinima,
    boxMaxima
  };

  const auto result = optimizer.minimize(
    positions,
    box,
    gradientTestFunction,
    gradientChecker
  );

  BOOST_CHECK_MESSAGE(
    result.iterations < 100,
    "Expected convergence in less than 100 cycles, got " << result.iterations
  );
  BOOST_CHECK_MESSAGE(
    std::fabs(positions[0] - boxMinima[0]) < 1e-3,
    "Expected x_min = x_box_min (0.1), but is " << positions[0] << " instead"
  );
  BOOST_CHECK_MESSAGE(
    std::fabs(positions[1] - boxMinima[1]) < 1e-3,
    "Expected y_min = y_box_min (0), but is " << positions[1] << " instead"
  );
}


/// @file bealab/scilib/stats.hpp
/// Analysis of statistical distributions and random number generation.

#ifndef _BEALAB_STATS_
#define	_BEALAB_STATS_

#include <random>
#include <ctime>
#include <boost/math/distributions/normal.hpp>
#include <bealab/core/blas.hpp>
#include <bealab/scilib/calculus.hpp>
#include <bealab/scilib/linalg.hpp>

namespace bealab
{
/// @defgroup stats Statistics
/// Analysis of statistical distributions and random number generation.
/// @{

//------------------------------------------------------------------------------
/// @name Univariate distributions
typedef boost::math::normal_distribution<double> normal;						///< Normal distribution
typedef std::uniform_real_distribution<double> uniform;							///< Uniform continuous distribution
typedef std::uniform_int_distribution<int> uniform_discrete;					///< Uniform discrete distribution
typedef std::bernoulli_distribution bernoulli;									///< Bernoulli distribution
/// @}

//------------------------------------------------------------------------------
/// @name Multivariate normal distribution

/// Distribution class
class multivariate_normal {

	rvec _mean;																	///< Mean vector
	rmat _covariance;															///< Covariance matrix
	rmat _covariance_factor;													///< Covariance factor (i.e., X such that C = X * trans(X))

public:

	/// Constructor
	multivariate_normal( const rvec& m, const rmat& Cf ) : _mean(m), _covariance_factor(Cf)
	{
		assert( m.size()  == Cf.size1() );
	}

	/// Get the mean vector
	const rvec& mean() const { return _mean; }

	/// Get the covariance matrix
	const rmat covariance() const
	{
		if( _covariance.size1() > 0 )
			return _covariance;
		else
			return _covariance_factor * trans(_covariance_factor);
	}

	/// Set the covariance matrix
	void covariance( const rmat& cov )
	{
		_covariance = cov;
	}

	/// Dimension of the random vector
	uint size() const { return _mean.size(); }

	/// Generate a random vector
	template<class generator>
	rvec operator()( generator& gen )
	{
		int M = size();
		rvec rv(M);
		std::normal_distribution<double> dist( 0, 1 );
		for( int m = 0; m < M; m++ )
			rv(m) = dist(gen);
		return _covariance_factor * rv + _mean;
	}
};

/// Probability density function
double pdf( const multivariate_normal& dist, const rvec& x );

/// Cumulative distribution function over a square region
double cdf( const multivariate_normal& dist, rvec a, rvec b );

/// Cumulative distribution function over a square region
double cdf( const multivariate_normal& dist, const rvec& b );

/// @}

//------------------------------------------------------------------------------
/// @name Random number generation

/// Private namespace for internal use.
namespace _stats
{

/// Random engine
extern std::mt19937 engine;
//extern std::default_random_engine engine;

/// Normal distribution mapping for random number generation
std::normal_distribution<double> stats2random(
		const boost::math::normal_distribution<double>& dist );

/// multivariate normal distribution mapping for random number generation
inline
multivariate_normal stats2random(
		const multivariate_normal& dist )
{ return dist; }

/// Uniform distribution mapping for random number generation
inline
std::uniform_real_distribution<double> stats2random(
		const std::uniform_real_distribution<double>& dist )
{ return dist; }

/// Uniform discrete distribution mapping for random number generation
inline
std::uniform_int_distribution<int> stats2random(
		const std::uniform_int_distribution<int>& dist )
{ return dist; }

/// Bernoulli distribution mapping for random number generation
inline
std::bernoulli_distribution stats2random(
		const std::bernoulli_distribution& dist )
{ return dist; }

}

/// Initializes the engine with a random seed (obtained from std::time()).
void randomize( unsigned int seed = std::time(0) );

/// Obtain a random sample from any specified distribution.
template<class T>
auto rand( T distribution ) -> decltype(_stats::stats2random(distribution)(_stats::engine))
{
	return _stats::stats2random ( distribution )( _stats::engine );
}

/// Obtain a vector of random samples from any specified distribution.
template<class T>
auto rand( T distribution, int M )
	-> vec<decltype(_stats::stats2random(distribution)(_stats::engine))>
{
	typedef decltype(_stats::stats2random(distribution)(_stats::engine)) R;
	vec<R> rv(M);
	for( int m = 0; m < M; m++ )
		rv(m) = _stats::stats2random(distribution)(_stats::engine);
	return rv;
}

/// Obtain a matrix of random samples from any specified distribution.
template<class T>
auto rand( T distribution, int M, int N )
	-> mat<decltype(_stats::stats2random(distribution)(_stats::engine))>
{
	typedef decltype(_stats::stats2random(distribution)(_stats::engine)) R;
	mat<R> rv(M,N);
	for( int m = 0; m < M; m++ )
		for( int n = 0; n < N; n++ )
			rv(m,n) = _stats::stats2random(distribution)(_stats::engine);
	return rv;
}

/// Obtain a random sample from the normal(0,1) distribution.
inline double randn() { return rand( normal(0,1) ); }

/// Obtain a random vector from the normal(0,1) distribution.
inline rvec randn( int M ) { return rand( normal(0,1), M ); }

/// Obtain a random matrix from the normal(0,1) distribution.
inline rmat randn( int M, int N ) { return rand( normal(0,1), M, N ); }

/// Obtain a random sample from the uniform(0,1) distribution.
inline double randu() { return rand( uniform(0,1) ); }

/// Obtain a random vector from the uniform(0,1) distribution.
inline rvec randu( int M ) { return rand( uniform(0,1), M ); }

/// Obtain a random matrix from the uniform(0,1) distribution.
inline rmat randu( int M, int N ) { return rand( uniform(0,1), M, N ); }
/// @}

//------------------------------------------------------------------------------
/// @name Analysis of statistical distributions
using boost::math::pdf;
using boost::math::cdf;
using boost::math::complement;
using boost::math::mean;
using boost::math::standard_deviation;
using boost::math::variance;
/// @}

/// @}
}
#endif


#include "svd.h"

#include <Eigen/SVD>

void eigenSVD(const Eigen::MatrixXd &A, Eigen::MatrixXd &U, Eigen::VectorXd &sigma, Eigen::MatrixXd &V) {
    Eigen::JacobiSVD<Eigen::MatrixXd> svd(A, Eigen::ComputeFullU | Eigen::ComputeFullV);
    U = svd.matrixU();
    sigma = svd.singularValues();
    V = svd.matrixV();
}


#include <pybind11/pybind11.h>
#include <pybind11/eigen.h>
#include <pybind11/stl.h>
#include <ceres/ceres.h>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <sophus/se3.hpp>

namespace py = pybind11;

// Example autodiff cost function from ceres tutorial
struct ExampleFunctor {
  template<typename T>
  bool operator()(const T *const x, T *residual) const {
    residual[0] = T(10.0) - x[0];
    return true;
  }

  static ceres::CostFunction *Create() {
    return new ceres::AutoDiffCostFunction<ExampleFunctor,
                                           1,
                                           1>(new ExampleFunctor);
  }
};

// cSE3 conversion functions: to/from pointer/vector
template<typename T>
void cSE3convert(const T* data, Sophus::SE3<T>& X) {
  X = Sophus::SE3<T>(Eigen::Quaternion<T>(data+3),
                     Eigen::Map<const Eigen::Matrix<T,3,1>>(data));
}
template<typename T>
void cSE3convert(const Eigen::Matrix<T,7,1> &Xvec, Sophus::SE3<T>& X) {
  X = Sophus::SE3<T>(Eigen::Quaternion<T>(Xvec.data()+3),
                     Eigen::Map<const Eigen::Matrix<T,3,1>>(Xvec.data()));
}
template<typename T>
void cSE3convert(const Sophus::SE3<T>& X, Eigen::Matrix<T,7,1> &Xvec) {
  Xvec.template block<3,1>(0,0) = Eigen::Matrix<T,3,1>(X.translation().data());
  Xvec.template block<4,1>(3,0) = Eigen::Matrix<T,4,1>(X.so3().data());
}

// AutoDiff cost function (factor) for the difference between a measured 3D
// relative transform, Xij = (tij_, qij_), and the relative transform between two
// estimated poses, Xi_hat and Xj_hat. Weighted by measurement covariance, Qij_.
class cSE3Functor
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  // store measured relative pose and inverted covariance matrix
  cSE3Functor(Eigen::Matrix<double,7,1> &Xij_vec, Eigen::Matrix<double,6,6> &Qij)
  {
    Sophus::SE3<double> Xij;
    cSE3convert(Xij_vec, Xij);
    Xij_inv_ = Xij.inverse();
    Qij_inv_ = Qij.inverse();
  }

  // templated residual definition for both doubles and jets
  // basically a weighted implementation of boxminus using Eigen templated types
  template<typename T>
  bool operator()(const T* _Xi_hat, const T* _Xj_hat, T* _res) const
  {
    // assign memory to usable objects
    Sophus::SE3<T> Xi_hat, Xj_hat;
    cSE3convert(_Xi_hat, Xi_hat);
    cSE3convert(_Xj_hat, Xj_hat);
    Eigen::Map<Eigen::Matrix<T,6,1>> r(_res);

    // compute current estimated relative pose
    const Sophus::SE3<T> Xij_hat = Xi_hat.inverse() * Xj_hat;

    // compute residual via boxminus (i.e., the logarithmic map of the error pose)
    // weight with inverse covariance
    r = Qij_inv_.cast<T>() * (Xij_inv_.cast<T>() * Xij_hat).log();

    return true;
  }

  // cost function generator--ONLY FOR PYTHON WRAPPER
  static ceres::CostFunction *Create(Eigen::Matrix<double,7,1> &Xij, Eigen::Matrix<double,6,6> &Qij) {
    return new ceres::AutoDiffCostFunction<cSE3Functor,
                                           6,
                                           7,
                                           7>(new cSE3Functor(Xij, Qij));
  }

private:
  Sophus::SE3<double> Xij_inv_;
  Eigen::Matrix<double,6,6> Qij_inv_;
};

// AutoDiff cost function (factor) for the difference between a range measurement
// rij, and the relative range between two estimated poses, Xi_hat and Xj_hat.
// Weighted by measurement variance, qij_.
class RangeFunctor
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  // store measured range and inverted variance
  RangeFunctor(double &rij, double &qij)
  {
    rij_ = rij;
    qij_inv_ = 1.0 / qij;
  }

  // templated residual definition for both doubles and jets
  template<typename T>
  bool operator()(const T* _Xi_hat, const T* _Xj_hat, T* _res) const
  {
    // assign memory to usable objects
    Eigen::Matrix<T,3,1> ti_hat(_Xi_hat), tj_hat(_Xj_hat);

    // range measurement error, scaled by inverse variance
    *_res = static_cast<T>(qij_inv_) * (static_cast<T>(rij_) - (tj_hat - ti_hat).norm());

    return true;
  }

  // cost function generator--ONLY FOR PYTHON WRAPPER
  static ceres::CostFunction *Create(double &rij, double &qij) {
    return new ceres::AutoDiffCostFunction<RangeFunctor,
                                           1,
                                           7,
                                           7>(new RangeFunctor(rij, qij));
  }

private:
  double rij_;
  double qij_inv_;
};

// AutoDiff cost function (factor) for the difference between an altitude
// measurement hi, and the altitude of an estimated pose, Xi_hat.
// Weighted by measurement variance, qij_.
class AltFunctor
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  // store measured range and inverted variance
  AltFunctor(double &hi, double &qi)
  {
    hi_ = hi;
    qi_inv_ = 1.0 / qi;
  }

  // templated residual definition for both doubles and jets
  template<typename T>
  bool operator()(const T* _Xi_hat, T* _res) const
  {
    // assign memory to usable objects
    T hi_hat = *(_Xi_hat + 2);

    // altitude measurement error, scaled by inverse variance
    *_res = static_cast<T>(qi_inv_) * (static_cast<T>(hi_) - hi_hat);

    return true;
  }

  // cost function generator--ONLY FOR PYTHON WRAPPER
  static ceres::CostFunction *Create(double &hi, double &qi) {
    return new ceres::AutoDiffCostFunction<AltFunctor,
                                           1,
                                           7>(new AltFunctor(hi, qi));
  }

private:
  double hi_;
  double qi_inv_;
};

// AutoDiff cost function (factor) for the difference between a measured 3D
// relative transform, Xij_ = (tij_, qij_), and the relative transform between two
// estimated poses, Xi_hat and Xj_hat. Weighted by measurement covariance, Qij_.
class Transform3DFunctor
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  // store measured relative pose and covariance matrix
  Transform3DFunctor(Eigen::Matrix<double,7,1> &X, Eigen::Matrix<double,6,6> &Q)
  {
    tij_ = X.block<3,1>(0,0);
    qij_ = Eigen::Quaterniond(X.block<4,1>(3,0));
    // residual weighted by inverse covariance
    Wij_ = Q.inverse();
  }

  // templated residual definition for both doubles and jets
  // basically a weighted implementation of boxminus using Eigen templated types
  template<typename T>
  bool operator()(const T* Xi_hat, const T* Xj_hat, T* res) const
  {
    // assign memory to usable objects
    Eigen::Map<const Eigen::Matrix<T,3,1>> ti_hat(Xi_hat);
    Eigen::Quaternion<T> qi_hat(Xi_hat+3);
    Eigen::Map<const Eigen::Matrix<T,3,1>> tj_hat(Xj_hat);
    Eigen::Quaternion<T> qj_hat(Xj_hat+3);
    Eigen::Map<Eigen::Matrix<T,6,1>> r(res);

    // compute trivial vector subtraction component
    Eigen::Matrix<T,3,1> t_res = tij_ - (tj_hat - ti_hat);

    // compute quaternion difference component
    Eigen::Quaternion<T> qij_hat = qi_hat.inverse() * qj_hat;
    Eigen::Quaternion<T> q_err = qij_hat.inverse() * static_cast<Eigen::Quaternion<T>>(qij_);
    Eigen::Matrix<T,3,1> q_res;
    if (q_err.w() < static_cast<T>(0.0))
    {
      q_err.coeffs() *= static_cast<T>(-1.0);
    }
    const Eigen::Matrix<T,3,1> qv(q_err.vec());
    T sinha = qv.norm();
    if (sinha > static_cast<T>(0.0))
    {
      T angle = static_cast<T>(2.0) * atan2(sinha, q_err.w());
      q_res = qv * (angle / sinha);
    }
    else
    {
      q_res = qv * (static_cast<T>(2.0) / q_err.w());
    }

    // assign to residual and weight by covariance
    r.template block<3,1>(0,0) = t_res;
    r.template block<3,1>(3,0) = q_res;
    r = Wij_ * r;

    return true;
  }

  // cost function generator--ONLY FOR PYTHON WRAPPER
  static ceres::CostFunction *Create(Eigen::Matrix<double,7,1> &X, Eigen::Matrix<double,6,6> &Q) {
    return new ceres::AutoDiffCostFunction<Transform3DFunctor,
                                           6,
                                           7,
                                           7>(new Transform3DFunctor(X, Q));
  }

private:
  Eigen::Matrix<double,3,1> tij_;
  Eigen::Quaternion<double> qij_;
  Eigen::Matrix<double,6,6> Wij_;
};

// AutoDiff cost function (factor) for the difference between a measured quaternion,
// q_, and an estimated quaternion, q_hat.
class QuaterniondFunctor
{
public:
  // store internal measured quaternion
  QuaterniondFunctor(Eigen::Vector4d &x)
  {
    q_ = Eigen::Quaternion<double>(x.data());
    q_inv_ = q_.inverse();
  }

  // templated residual definition for both doubles and jets
  template<typename T>
  bool operator()(const T* _q_hat, T* res) const
  {
    Eigen::Quaternion<T> q_hat(_q_hat);
    Eigen::Map<Eigen::Matrix<T, 3, 1>> r(res);

    // implementation of boxminus using templated Eigen types
    // https://eigen.tuxfamily.org/bz/show_bug.cgi?id=1244
    //  q_err = q^{-1} * q_hat
    Eigen::Quaternion<T> q_err(static_cast<Eigen::Quaternion<T>>(q_inv_)*(q_hat));
    //  q_err -> theta vector
    if (q_err.w() < static_cast<T>(0.0))
    {
      q_err.coeffs() *= static_cast<T>(-1.0);
    }
    const Eigen::Matrix<T,3,1> qv(q_err.vec());
    T sinha = qv.norm();
    if (sinha > static_cast<T>(0.0))
    {
      T angle = static_cast<T>(2.0) * atan2(sinha, q_err.w());
      r = qv * (angle / sinha);
    }
    else
    {
      r = qv * (static_cast<T>(2.0) / q_err.w());
    }

    return true;
  }

  // cost function generator--ONLY FOR PYTHON WRAPPER
  static ceres::CostFunction *Create(Eigen::Vector4d &x) {
    return new ceres::AutoDiffCostFunction<QuaterniondFunctor, 3, 4>(new QuaterniondFunctor(x));
  }

private:
  Eigen::Quaternion<double> q_;
  Eigen::Quaternion<double> q_inv_;
};

// AutoDiff local parameterization for the compact SE3 pose [t q] object.
// The boxplus operator informs Ceres how the manifold evolves and also
// allows for the calculation of derivatives.
struct cSE3Plus {
  // boxplus operator for both doubles and jets
  template<typename T>
  bool operator()(const T* x, const T* delta, T* x_plus_delta) const
  {
    // capture argument memory
    Sophus::SE3<T> X;
    cSE3convert(x, X);
    Eigen::Map<const Eigen::Matrix<T, 6, 1>> dX(delta);
    Eigen::Map<Eigen::Matrix<T, 7, 1>>       Yvec(x_plus_delta);

    // increment pose using the exponential map
    Sophus::SE3<T> Exp_dX = Sophus::SE3<T>::exp(dX);
    Sophus::SE3<T> Y = X * Exp_dX;

    // assign SE3 coefficients to compact vector
    Eigen::Matrix<T,7,1> YvecCoef;
    cSE3convert(Y, YvecCoef);
    Yvec = YvecCoef;

    return true;
  }

  // local parameterization generator--ONLY FOR PYTHON WRAPPER
  static ceres::LocalParameterization *Create() {
    return new ceres::AutoDiffLocalParameterization<cSE3Plus,
                                                    7,
                                                    6>(new cSE3Plus);
  }
};

// AutoDiff local parameterization for the Transform3D object. The boxplus
// operator informs Ceres how the manifold evolves and also allows for the
// calculation of derivatives
struct Transform3DPlus {
  // boxplus operator for both doubles and jets
  template<typename T>
  bool operator()(const T* x, const T* delta, T* x_plus_delta) const
  {
    // capture argument memory
    Eigen::Map<const Eigen::Matrix<T, 3, 1>> t(x);
    Eigen::Map<const Eigen::Matrix<T, 3, 1>> dt(delta);
    const Eigen::Quaternion<T> q(x+3);
    Eigen::Map<const Eigen::Matrix<T, 3, 1>> dq(delta+3);
    Eigen::Map<Eigen::Matrix<T, 7, 1>> Tplus(x_plus_delta);

    // compute trivial vector addition component
    Tplus.template block<3,1>(0,0) = t + dt;

    // compute quaternion oplus component
    Eigen::Quaternion<T> plusq;
    T th = dq.norm();
    if (th > static_cast<T>(0.0))
    {
      Eigen::Matrix<T, 3, 1> u = dq / th;
      T sth2 = sin(th / static_cast<T>(2.0));
      plusq = Eigen::Quaternion<T>(cos(th / static_cast<T>(2.0)), u.x()*sth2, u.y()*sth2, u.z()*sth2);
    }
    else
    {
      Eigen::Matrix<T, 3, 1> r2 = dq / static_cast<T>(2.0);
      plusq = Eigen::Quaternion<T>(static_cast<T>(1.0), r2.x(), r2.y(), r2.z()).normalized();
    }
    Eigen::Quaternion<T> qplusq = q * plusq;
    Tplus.template block<4,1>(3,0) = qplusq.coeffs();

    return true;
  }

  // local parameterization generator--ONLY FOR PYTHON WRAPPER
  static ceres::LocalParameterization *Create() {
    return new ceres::AutoDiffLocalParameterization<Transform3DPlus,
                                                    7,
                                                    6>(new Transform3DPlus);
  }
};

void add_custom_cost_functions(py::module &m) {

  // Use pybind11 code to wrap your own cost function which is defined in C++

  // Transform3DLocalParameterization
  m.def("Transform3DLocalParameterization", &Transform3DPlus::Create);

  // cSE3LocalParameterization
  m.def("cSE3LocalParameterization", &cSE3Plus::Create);

  // cSE3Factor
  m.def("cSE3Factor", &cSE3Functor::Create);

  // RangeFactor
  m.def("RangeFactor", &RangeFunctor::Create);

  // AltFactor
  m.def("AltFactor", &AltFunctor::Create);

  // Transform3DFactor
  m.def("Transform3DFactor", &Transform3DFunctor::Create);

  // Here is an example
  m.def("CreateCustomExampleCostFunction", &ExampleFunctor::Create);

  // QuaterniondFunctor
  m.def("CreateQuaterniondADCostFunction", &QuaterniondFunctor::Create);

}


#include <iostream>
#include <stdio.h>
#include <Eigen/Eigen>
#include <vector>
#include "calib.h"
#include <fstream>

int main()
{
	LeapMotionsCalibration calib;
	std::vector<Eigen::Vector3f> firstSet, secondSet;

	std::ifstream o;
	o.open("calib.txt");
	int pairs;
	o>>pairs;
	for(int i=0;i<pairs;i++)
	{
		Eigen::Vector3f tmp;
		o >> tmp[0]; o >> tmp[1]; o >> tmp[2];
		std::cout<<"( " << tmp[0] <<" " <<tmp[1] << " "<<tmp[2]<<" )"<< std::endl;
		firstSet.push_back(tmp);

		o >> tmp[0]; o >> tmp[1]; o >> tmp[2];
		std::cout<<"[ " << tmp[0] <<" " <<tmp[1] << " "<<tmp[2]<<" ]"<< std::endl;
		secondSet.push_back(tmp);
	}
	o.close();

	calib.readData(firstSet, secondSet);
	Eigen::Matrix4f result = calib.calculateTransformation();
	double error = calib.calculateError(firstSet, secondSet, result);

	std::cout<<"Calculated transformation between two LeapMotion coordinate systems is :"<<std::endl;
	std::cout<<result<<std::endl;
	std::cout<<"RMS error: " << error << std::endl;

	return 0;
}


#define BOOST_TEST_MODULE "test_math_functions"

#ifdef BOOST_TEST_DYN_LINK
#include <boost/test/unit_test.hpp>
#else
#include <boost/test/included/unit_test.hpp>
#endif

#include <boost/mpl/list.hpp>
#include <mjolnir/math/math.hpp>

#include <random>
#include <chrono>
#include <array>
#include <iostream>

typedef boost::mpl::list<double, float> test_targets;
constexpr std::size_t N = 10000;

static_assert(mjolnir::compiletime::pow( 2.0, 4) == 16.0, "mjolnir::compiletime::pow");
static_assert(mjolnir::compiletime::pow(-2.0, 4) == 16.0, "mjolnir::compiletime::pow");
static_assert(mjolnir::compiletime::pow(-2.0, 3) == -8.0, "mjolnir::compiletime::pow");

static_assert(mjolnir::compiletime::abs( 1.0) == 1.0, "mjolnir::compiletime::abs");
static_assert(mjolnir::compiletime::abs(-1.0) == 1.0, "mjolnir::compiletime::abs");
static_assert(mjolnir::compiletime::min( 1.0, 2.0) ==  1.0, "mjolnir::compiletime::min");
static_assert(mjolnir::compiletime::min(-1.0, 0.1) == -1.0, "mjolnir::compiletime::min");
static_assert(mjolnir::compiletime::min(2.0,  1.0) ==  1.0, "mjolnir::compiletime::min");
static_assert(mjolnir::compiletime::min(0.1, -1.0) == -1.0, "mjolnir::compiletime::min");
static_assert(mjolnir::compiletime::max( 1.0, 2.0) ==  2.0, "mjolnir::compiletime::max");
static_assert(mjolnir::compiletime::max(-1.0, 0.1) ==  0.1, "mjolnir::compiletime::max");
static_assert(mjolnir::compiletime::max(2.0,  1.0) ==  2.0, "mjolnir::compiletime::max");
static_assert(mjolnir::compiletime::max(0.1, -1.0) ==  0.1, "mjolnir::compiletime::max");

static_assert(mjolnir::compiletime::clamp( 0.1, -1.0, 1.0) ==  0.1, "mjolnir::compiletime::clamp");
static_assert(mjolnir::compiletime::clamp( 2.0, -1.0, 1.0) ==  1.0, "mjolnir::compiletime::clamp");
static_assert(mjolnir::compiletime::clamp(-2.0, -1.0, 1.0) == -1.0, "mjolnir::compiletime::clamp");

namespace test
{
template<typename T>
decltype(boost::test_tools::tolerance(std::declval<T>())) tolerance();

template<>
decltype(boost::test_tools::tolerance(std::declval<float>()))
tolerance<float>()
{return boost::test_tools::tolerance(3.0f / static_cast<float>(std::pow(2, 12)));}

template<>
decltype(boost::test_tools::tolerance(std::declval<double>()))
tolerance<double>()
{return boost::test_tools::tolerance(2.0 / std::pow(2, 14));}
} // test

BOOST_AUTO_TEST_CASE_TEMPLATE(rsqrt, Real, test_targets)
{
    std::mt19937 mt(123456789);
    std::uniform_real_distribution<Real> uni(0.0001, 100.0);

    for(std::size_t i=0; i < N; ++i)
    {
        const Real x     = uni(mt);
        const Real x_sq  = x * x;
        const Real x_inv = 1 / x;
        BOOST_TEST(x_inv == mjolnir::math::rsqrt(x_sq), test::tolerance<Real>());
    }
}

BOOST_AUTO_TEST_CASE_TEMPLATE(clamp, Real, test_targets)
{
    std::mt19937 mt(123456789);
    std::uniform_real_distribution<Real> uni(-100.0, 100.0);
    const Real maximum =  10.0;
    const Real minimum = -10.0;

    for(std::size_t i=0; i < N; ++i)
    {
        const Real x = uni(mt);
        BOOST_TEST(mjolnir::math::clamp<Real>(x, minimum, maximum) ==
                   std::min(std::max(minimum, x), maximum),
                   test::tolerance<Real>());
    }
}


static bool eigen_did_assert = false;
#define eigen_assert(X) if(!eigen_did_assert && !(X)){ std::cout << "### Assertion raised in " << __FILE__ << ":" << __LINE__ << ":\n" #X << "\n### The following would happen without assertions:\n"; eigen_did_assert = true;}

#include <iostream>
#include <Eigen/Eigen>

#ifndef M_PI
#define M_PI 3.1415926535897932384626433832795
#endif

using namespace Eigen;
using namespace std;

int main(int, char**)
{
  cout.precision(3);
// intentionally remove indentation of snippet
{
MatrixXd X = MatrixXd::Random(4,4);
MatrixXd A = X * X.transpose();
cout << "Here is a random positive-definite matrix, A:" << endl << A << endl << endl;

SelfAdjointEigenSolver<MatrixXd> es(A);
cout << "The inverse square root of A is: " << endl;
cout << es.operatorInverseSqrt() << endl;
cout << "We can also compute it with operatorSqrt() and inverse(). That yields: " << endl;
cout << es.operatorSqrt().inverse() << endl;

}
  return 0;
}


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

//[eccentricity_example
#include <iostream>
#include <iomanip>

#include <boost/graph/undirected_graph.hpp>
#include <boost/graph/exterior_property.hpp>
#include <boost/graph/floyd_warshall_shortest.hpp>
#include <boost/graph/eccentricity.hpp>
#include "helper.hpp"

using namespace std;
using namespace boost;

// The Actor type stores the name of each vertex in the graph.
struct Actor
{
    string name;
};

// Declare the graph type and its vertex and edge types.
typedef undirected_graph<Actor> Graph;
typedef graph_traits<Graph>::vertex_descriptor Vertex;
typedef graph_traits<Graph>::edge_descriptor Edge;

// The name map provides an abstract accessor for the names of
// each vertex. This is used during graph creation.
typedef property_map<Graph, string Actor::*>::type NameMap;

// Declare a matrix type and its corresponding property map that
// will contain the distances between each pair of vertices.
typedef exterior_vertex_property<Graph, int> DistanceProperty;
typedef DistanceProperty::matrix_type DistanceMatrix;
typedef DistanceProperty::matrix_map_type DistanceMatrixMap;

// Declare the weight map so that each edge returns the same value.
typedef constant_property_map<Edge, int> WeightMap;

// Declare a container and its corresponding property map that
// will contain the resulting eccentricities of each vertex in
// the graph.
typedef boost::exterior_vertex_property<Graph, int> EccentricityProperty;
typedef EccentricityProperty::container_type EccentricityContainer;
typedef EccentricityProperty::map_type EccentricityMap;

int
main(int argc, char *argv[])
{
    // Create the graph and a name map that provides access to
    // then actor names.
    Graph g;
    NameMap nm(get(&Actor::name, g));

    // Read the graph from standard input.
    read_graph(g, nm, cin);

    // Compute the distances between all pairs of vertices using
    // the Floyd-Warshall algorithm. Note that the weight map is
    // created so that every edge has a weight of 1.
    DistanceMatrix distances(num_vertices(g));
    DistanceMatrixMap dm(distances, g);
    WeightMap wm(1);
    floyd_warshall_all_pairs_shortest_paths(g, dm, weight_map(wm));

    // Compute the eccentricities for graph - this computation returns
    // both the radius and diameter as well.
    int r, d;
    EccentricityContainer eccs(num_vertices(g));
    EccentricityMap em(eccs, g);
    boost::tie(r, d) = all_eccentricities(g, dm, em);

    // Print the closeness centrality of each vertex.
    graph_traits<Graph>::vertex_iterator i, end;
    for(boost::tie(i, end) = vertices(g); i != end; ++i) {
        cout << setw(12) << setiosflags(ios::left)
                << g[*i].name << get(em, *i) << endl;
    }
    cout << "radius: " << r << endl;
    cout << "diamter: " << d << endl;

    return 0;
}
//]


/**
 * @file finitevolumesineconslaw.cc
 * @brief NPDE homework "FiniteVolumeSineConsLaw" code
 * @author Oliver Rietmann
 * @date 25.04.2019
 * @copyright Developed at ETH Zurich
 */

#include "finitevolumesineconslaw.h"

#include <Eigen/Core>
#include <cmath>

namespace FiniteVolumeSineConsLaw {

/* SAM_LISTING_BEGIN_1 */
constexpr double PI = 3.14159265358979323846;

double f(double x) { return std::sin(PI * x); }

double sineGodFlux(double v, double w) {
  double result;
  //====================
  // Your code goes here
  //====================

  return result;
}
/* SAM_LISTING_END_1 */

/* SAM_LISTING_BEGIN_2 */
Eigen::VectorXd sineClawRhs(const Eigen::VectorXd &mu) {
  int N = mu.size();
  double h = 12.0 / N;
  Eigen::VectorXd result(N);

  //====================
  // Your code goes here
  //====================

  return result;
}
/* SAM_LISTING_END_2 */

/* SAM_LISTING_BEGIN_3 */
bool blowup(const Eigen::VectorXd &mu) {
  return mu.minCoeff() < 0.0 || mu.maxCoeff() > 2.0;
}

unsigned int findTimesteps() {
  const unsigned int N = 600;
  unsigned int ML = 100;
  unsigned int MR = 200;

  //====================
  // Your code goes here
  //====================

  return MR;
}
/* SAM_LISTING_END_3 */

/* SAM_LISTING_BEGIN_4 */
Eigen::VectorXd sineClawReactionRhs(const Eigen::VectorXd &mu, double c) {
  Eigen::VectorXd rhs(mu.size());
  //====================
  // Your code goes here
  //====================
  return rhs;
}
/* SAM_LISTING_END_4 */

}  // namespace FiniteVolumeSineConsLaw


//!file
//! \brief floating-point comparison from Boost.Test
// Copyright Paul A. Bristow 2015.
// Copyright John Maddock 2015.

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

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

#include <boost/math/special_functions/relative_difference.hpp>
#include <boost/math/special_functions/next.hpp>

#include <iostream>
#include <limits> // for std::numeric_limits<T>::epsilon().

int main()
{
  std::cout << "Compare floats using Boost.Math functions/classes" << std::endl;

//[compare_floats_using
/*`Some using statements will ensure that the functions we need are accessible.
*/

  using namespace boost::math;

//`or

  using boost::math::relative_difference;
  using boost::math::epsilon_difference;
  using boost::math::float_next;
  using boost::math::float_prior;

//] [/compare_floats_using]

//[compare_floats_example_1
/*`The following examples display values with all possibly significant digits.
Newer compilers should provide `std::numeric_limits<FPT>::max_digits10`
for this purpose, and here we use `float` precision where `max_digits10` = 9
to avoid displaying a distracting number of decimal digits.

[note Older compilers can use this formula to calculate `max_digits10`
from `std::numeric_limits<FPT>::digits10`:[br]
__spaces `int max_digits10 = 2 + std::numeric_limits<FPT>::digits10 * 3010/10000;`
] [/note]

One can set the display including all trailing zeros
(helpful for this example to show all potentially significant digits),
and also to display `bool` values as words rather than integers:
*/
  std::cout.precision(std::numeric_limits<float>::max_digits10);
  std::cout << std::boolalpha << std::showpoint << std::endl;

//] [/compare_floats_example_1]

//[compare_floats_example_2]
/*`
When comparing values that are ['quite close] or ['approximately equal],
we could use either `float_distance` or `relative_difference`/`epsilon_difference`, for example
with type `float`, these two values are adjacent to each other:
*/

  float a = 1;
  float b = 1 + std::numeric_limits<float>::epsilon();
  std::cout << "a = " << a << std::endl;
  std::cout << "b = " << b << std::endl;
  std::cout << "float_distance = " << float_distance(a, b) << std::endl;
  std::cout << "relative_difference = " << relative_difference(a, b) << std::endl;
  std::cout << "epsilon_difference = " << epsilon_difference(a, b) << std::endl;

/*`
Which produces the output:

[pre
a = 1.00000000
b = 1.00000012
float_distance = 1.00000000
relative_difference = 1.19209290e-007
epsilon_difference = 1.00000000
]
*/
  //] [/compare_floats_example_2]

//[compare_floats_example_3]
/*`
In the example above, it just so happens that the edit distance as measured by `float_distance`, and the
difference measured in units of epsilon were equal.  However, due to the way floating point
values are represented, that is not always the case:*/

  a = 2.0f / 3.0f;   // 2/3 inexactly represented as a float
  b = float_next(float_next(float_next(a))); // 3 floating point values above a
  std::cout << "a = " << a << std::endl;
  std::cout << "b = " << b << std::endl;
  std::cout << "float_distance = " << float_distance(a, b) << std::endl;
  std::cout << "relative_difference = " << relative_difference(a, b) << std::endl;
  std::cout << "epsilon_difference = " << epsilon_difference(a, b) << std::endl;

/*`
Which produces the output:

[pre
a = 0.666666687
b = 0.666666865
float_distance = 3.00000000
relative_difference = 2.68220901e-007
epsilon_difference = 2.25000000
]

There is another important difference between `float_distance` and the
`relative_difference/epsilon_difference` functions in that `float_distance`
returns a signed result that reflects which argument is larger in magnitude,
where as `relative_difference/epsilon_difference` simply return an unsigned
value that represents how far apart the values are.  For example if we swap
the order of the arguments:
*/

  std::cout << "float_distance = " << float_distance(b, a) << std::endl;
  std::cout << "relative_difference = " << relative_difference(b, a) << std::endl;
  std::cout << "epsilon_difference = " << epsilon_difference(b, a) << std::endl;

  /*`
  The output is now:

  [pre
  float_distance = -3.00000000
  relative_difference = 2.68220901e-007
  epsilon_difference = 2.25000000
  ]
*/
  //] [/compare_floats_example_3]

//[compare_floats_example_4]
/*`
Zeros are always treated as equal, as are infinities as long as they have the same sign:*/

  a = 0;
  b = -0;  // signed zero
  std::cout << "relative_difference = " << relative_difference(a, b) << std::endl;
  a = b = std::numeric_limits<float>::infinity();
  std::cout << "relative_difference = " << relative_difference(a, b) << std::endl;
  std::cout << "relative_difference = " << relative_difference(a, -b) << std::endl;

/*`
Which produces the output:

[pre
relative_difference = 0.000000000
relative_difference = 0.000000000
relative_difference = 3.40282347e+038
]
*/
//] [/compare_floats_example_4]

//[compare_floats_example_5]
/*`
Note that finite values are always infinitely far away from infinities even if those finite values are very large:*/

  a = (std::numeric_limits<float>::max)();
  b = std::numeric_limits<float>::infinity();
  std::cout << "a = " << a << std::endl;
  std::cout << "b = " << b << std::endl;
  std::cout << "relative_difference = " << relative_difference(a, b) << std::endl;
  std::cout << "epsilon_difference = " << epsilon_difference(a, b) << std::endl;

/*`
Which produces the output:

[pre
a = 3.40282347e+038
b = 1.#INF0000
relative_difference = 3.40282347e+038
epsilon_difference = 3.40282347e+038
]
*/
//] [/compare_floats_example_5]

//[compare_floats_example_6]
/*`
Finally, all denormalized values and zeros are treated as being effectively equal:*/

  a = std::numeric_limits<float>::denorm_min();
  b = a * 2;
  std::cout << "a = " << a << std::endl;
  std::cout << "b = " << b << std::endl;
  std::cout << "float_distance = " << float_distance(a, b) << std::endl;
  std::cout << "relative_difference = " << relative_difference(a, b) << std::endl;
  std::cout << "epsilon_difference = " << epsilon_difference(a, b) << std::endl;
  a = 0;
  std::cout << "a = " << a << std::endl;
  std::cout << "b = " << b << std::endl;
  std::cout << "float_distance = " << float_distance(a, b) << std::endl;
  std::cout << "relative_difference = " << relative_difference(a, b) << std::endl;
  std::cout << "epsilon_difference = " << epsilon_difference(a, b) << std::endl;

/*`
Which produces the output:

[pre
a = 1.40129846e-045
b = 2.80259693e-045
float_distance = 1.00000000
relative_difference = 0.000000000
epsilon_difference = 0.000000000
a = 0.000000000
b = 2.80259693e-045
float_distance = 2.00000000
relative_difference = 0.000000000
epsilon_difference = 0.000000000]

Notice how, in the above example, two denormalized values that are a factor of 2 apart are
none the less only one representation apart!

*/
//] [/compare_floats_example_6]

#if 0
//[old_compare_floats_example_3
//`The simplest use is to compare two values with a tolerance thus:

  bool is_close = is_close_to(1.F, 1.F + epsilon, epsilon); // One epsilon apart is close enough.
  std::cout << "is_close_to(1.F, 1.F + epsilon, epsilon); is " << is_close << std::endl; // true

  is_close = is_close_to(1.F, 1.F + 2 * epsilon, epsilon); // Two epsilon apart isn't close enough.
  std::cout << "is_close_to(1.F, 1.F + epsilon, epsilon); is " << is_close << std::endl; // false

/*`
[note The type FPT of the tolerance and the type of the values [*must match].

So `is_close(0.1F, 1., 1.)` will fail to compile because "template parameter 'FPT' is ambiguous".
Always provide the same type, using `static_cast<FPT>` if necessary.]
*/

/*`An instance of class `close_at_tolerance` is more convenient
when multiple tests with the same conditions are planned.
A class that stores a tolerance of three epsilon (and the default ['strong] test) is:
*/

  close_at_tolerance<float> three_rounds(3 * epsilon); // 'strong' by default.

//`and we can confirm these settings:

  std::cout << "fraction_tolerance = "
    << three_rounds.fraction_tolerance()
    << std::endl; // +3.57627869e-007
  std::cout << "strength = "
    << (three_rounds.strength() == FPC_STRONG ? "strong" : "weak")
    << std::endl; // strong

//`To start, let us use two values that are truly equal (having identical bit patterns)

  float a = 1.23456789F;
  float b = 1.23456789F;

//`and make a comparison using our 3*epsilon `three_rounds` functor:

  bool close = three_rounds(a, b);
  std::cout << "three_rounds(a, b) = " << close << std::endl; // true

//`Unsurprisingly, the result is true, and the failed fraction is zero.

  std::cout << "failed_fraction = " << three_rounds.failed_fraction() << std::endl;

/*`To get some nearby values, it is convenient to use the Boost.Math __next_float functions,
for which we need an include

  #include <boost/math/special_functions/next.hpp>

and some using declarations:
*/

  using boost::math::float_next;
  using boost::math::float_prior;
  using boost::math::nextafter;
  using boost::math::float_distance;

//`To add a few __ulp to one value:
  b = float_next(a); // Add just one ULP to a.
  b = float_next(b); // Add another one ULP.
  b = float_next(b); // Add another one ULP.
  // 3 epsilon would pass.
  b = float_next(b); // Add another one ULP.

//`and repeat our comparison:

  close = three_rounds(a, b);
  std::cout << "three_rounds(a, b) = " << close << std::endl; // false
  std::cout << "failed_fraction = " << three_rounds.failed_fraction()
    << std::endl;  // abs(u-v) / abs(v) = 3.86237957e-007

//`We can also 'measure' the number of bits different using the `float_distance` function:

  std::cout << "float_distance = " << float_distance(a, b) << std::endl; // 4

/*`Now consider two values that are much further apart
than one might expect from ['computational noise],
perhaps the result of two measurements of some physical property like length
where an uncertainty of a percent or so might be expected.
*/
  float fp1 = 0.01000F;
  float fp2 = 0.01001F; // Slightly different.

  float tolerance = 0.0001F;

  close_at_tolerance<float> strong(epsilon); // Default is strong.
  bool rs = strong(fp1, fp2);
  std::cout << "strong(fp1, fp2) is " << rs << std::endl;

//`Or we could contrast using the ['weak] criterion:
  close_at_tolerance<float> weak(epsilon, FPC_WEAK); // Explicitly weak.
  bool rw = weak(fp1, fp2); //
  std::cout << "weak(fp1, fp2) is " << rw << std::endl;

//`We can also construct, setting tolerance and strength, and compare in one statement:

  std::cout << a << " #= " << b << " is "
    << close_at_tolerance<float>(epsilon, FPC_STRONG)(a, b) << std::endl;
  std::cout << a << " ~= " << b << " is "
    << close_at_tolerance<float>(epsilon, FPC_WEAK)(a, b) << std::endl;

//`but this has little advantage over using function `is_close_to` directly.

//] [/old_compare_floats_example_3]

/*When the floating-point values become very small and near zero, using
//a relative test becomes unhelpful because one is dividing by zero or tiny,

//Instead, an absolute test is needed, comparing one (or usually both) values with zero,
//using a tolerance.
//This is provided by the `small_with_tolerance` class and `is_small` function.

  namespace boost {
  namespace math {
  namespace fpc {

  template<typename FPT>
  class small_with_tolerance
  {
  public:
  // Public typedefs.
  typedef bool result_type;

  // Constructor.
  explicit small_with_tolerance(FPT tolerance); // tolerance >= 0

  // Functor
  bool operator()(FPT value) const; // return true if <= absolute tolerance (near zero).
  };

  template<typename FPT>
  bool
  is_small(FPT value, FPT tolerance); // return true if value <= absolute tolerance (near zero).

  }}} // namespaces.

/*`
[note The type FPT of the tolerance and the type of the value [*must match].

So `is_small(0.1F, 0.000001)` will fail to compile because "template parameter 'FPT' is ambiguous".
Always provide the same type, using `static_cast<FPT>` if necessary.]

A few values near zero are tested with varying tolerance below.
*/
//[compare_floats_small_1

  float c = 0;
  std::cout << "0 is_small " << is_small(c, epsilon) << std::endl; // true

  c = std::numeric_limits<float>::denorm_min(); // 1.40129846e-045
  std::cout << "denorm_ min =" << c << ", is_small is " << is_small(c, epsilon) << std::endl; // true

  c = (std::numeric_limits<float>::min)(); // 1.17549435e-038
  std::cout << "min = " << c << ", is_small is " << is_small(c, epsilon) << std::endl; // true

  c = 1 * epsilon; // 1.19209290e-007
  std::cout << "epsilon = " << c << ", is_small is " << is_small(c, epsilon) << std::endl; // false

  c = 1 * epsilon; // 1.19209290e-007
  std::cout << "2 epsilon = " << c << ", is_small is " << is_small(c, 2 * epsilon) << std::endl; // true

  c = 2 * epsilon; //2.38418579e-007
  std::cout << "4 epsilon = " << c << ", is_small is " << is_small(c, 2 * epsilon) << std::endl; // false

  c = 0.00001F;
  std::cout << "0.00001 = " << c << ", is_small is " << is_small(c, 0.0001F) << std::endl; // true

  c = -0.00001F;
  std::cout << "0.00001 = " << c << ", is_small is " << is_small(c, 0.0001F) << std::endl; // true

/*`Using the class `small_with_tolerance` allows storage of the tolerance,
convenient if you make repeated tests with the same tolerance.
*/

  small_with_tolerance<float>my_test(0.01F);

  std::cout << "my_test(0.001F) is " << my_test(0.001F) << std::endl; // true
  std::cout << "my_test(0.001F) is " << my_test(0.01F) << std::endl; // false

  //] [/compare_floats_small_1]
#endif
  return 0;
}  // int main()

/*

Example output is:

//[compare_floats_output
Compare floats using Boost.Test functions/classes

float epsilon = 1.19209290e-007
is_close_to(1.F, 1.F + epsilon, epsilon); is true
is_close_to(1.F, 1.F + epsilon, epsilon); is false
fraction_tolerance = 3.57627869e-007
strength = strong
three_rounds(a, b) = true
failed_fraction = 0.000000000
three_rounds(a, b) = false
failed_fraction = 3.86237957e-007
float_distance = 4.00000000
strong(fp1, fp2) is false
weak(fp1, fp2) is false
1.23456788 #= 1.23456836 is false
1.23456788 ~= 1.23456836 is false
0 is_small true
denorm_ min =1.40129846e-045, is_small is true
min = 1.17549435e-038, is_small is true
epsilon = 1.19209290e-007, is_small is false
2 epsilon = 1.19209290e-007, is_small is true
4 epsilon = 2.38418579e-007, is_small is false
0.00001 = 9.99999975e-006, is_small is true
0.00001 = -9.99999975e-006, is_small is true
my_test(0.001F) is true

my_test(0.001F) is false//] [/compare_floats_output]
*/


#include <math.h>
#include <cstdlib>
#include <iostream>

#include <adept.h>

#define N 100
#define FILLITER 10

using adept::adouble;

void laplacian(adouble x[N], adouble y[N])
{
    int i, p;
    y[0] = -2*x[0] + x[1];
    y[N-1] = -2*x[N-1] + x[N-2];
    for (i = 1; i < N-1; ++i) {
        y[i] = x[i-1] - 2*x[i] + x[i+1];
    }
    for (p = 0; p < FILLITER; ++p) {
        for (i = 1; i < N-1; ++i) {
            y[i] = y[i-1] - 2*y[i] + y[i+1]/(3.1415 + y[i]);
        }
    }
}

void doit(double *x, double *y, double *J)
{
    adept::Stack stack;
    adouble xad[N], yad[N];
    int i;

    adept::set_values(xad, N, x);
    stack.new_recording();

    laplacian(xad, yad);

    stack.independent(xad, N);
    stack.dependent(yad, N);
    stack.jacobian(J);

    for (i = 0; i < N; ++i) {
	y[i] = yad[i].value();
    }
}

int main() {
    double x[N];
    double y[N];
    double J[N*N];
    int rep, i, j;

    for (i = 0; i < N; ++i) {
        x[i] = 1.0 + i + 1;
    }

    for (rep = 0; rep < int(1e8/(N*N)); ++rep) {
        doit(x, y, J);
    }

    for (i = 0; i < 5; ++i) {
        std::cout << y[i] << std::endl;
    }
    for (i = 0; i < 5; ++i) {
        for (j = 0; j < 5; ++j) {
            std::cout << J[i + N*j] << " ";
        }
        std::cout << std::endl;
    }

    return 0;
}


#include "stdafx.h"
#include "EllipticOrbit.h"
#include <tuple>
#include <limits>
#include <boost/math/tools/roots.hpp>

namespace
{
// Матчасть взята отсюда: http://www.astronet.ru/db/msg/1190817/node21.html#ll60

// Уравнение кеплера, задающее связь между эксцентрической аномалией,
// эксцентриситетом орбиты и средней аномалией
//      M = E - e * sin(E)
// где M - средняя аномалия, e - эксцентриситет орбиты, E - эксцентрическая аномалия
// уравнение `M = E - e * sin(E)` преобразуется так, чтобы слева было число 0,
// а справа - некоторая функция F(E):
//      0 = E - M - e * sin(E)
// Функтор возвращает функцию от E, а также две её производные функции.
//      F(E) = E - M - e * sin(E)
//      F'(E) = 1 - e * cos(E)
//      F''(E) = e * sin(E)
using FunctionSnapshot = boost::math::tuple<double, double, double>;
using EquationFunction = std::function<FunctionSnapshot(const double &x)>;

EquationFunction MakeKeplerEquationFunction(double meanAnomaly, double eccentricity)
{
    return [=](const double &x) {
        return boost::math::make_tuple(
                    x - meanAnomaly - eccentricity * sin(x),    // функция
                    1 - eccentricity * cos(x),                  // её первая производная
                    eccentricity * sin(x));                     // её вторая производная
    };
}

// Функция выполняет численое решение уравнения кеплера,
// вычисляя эксцентрическую аномалию
// при известных средней аномалии и эксцентриситете орбиты
// В качестве решения используется метод Halley
// (http://en.wikipedia.org/wiki/Halley's_method),
// используя соответствующие алгоритмы библиотеки boost.
double SolveKeplerEquation(double const& meanAnomaly, double const& eccentricity)
{
    const int digits = (std::numeric_limits<double>::digits) >> 1;
    // ограничиваем максимальное число итераций,
    // поскольку мы рисуем систему планет в реальном времени.
    boost::uintmax_t maxIteractions = 1000;

    return boost::math::tools::halley_iterate(
        MakeKeplerEquationFunction(meanAnomaly, eccentricity),
        meanAnomaly,                // первое приближение корня
        meanAnomaly - eccentricity, // минимальное значение корня
        meanAnomaly + eccentricity, // максимальное значение корня
        digits,                     // число разрядов
        maxIteractions);            // наибольшее число итераций
}
}

double CEllipticOrbit::Eccentricity() const
{
    return m_eccentricity;
}

double CEllipticOrbit::LargeAxis() const
{
    return m_largeAxis;
}

double CEllipticOrbit::MeanAnomaly(const double &time) const
{
    const double anomaly = 2 * M_PI * m_meanMotion * (time - m_periapsisEpoch);
    const double wrappedAnomaly = fmod(anomaly, M_PI + M_PI);
    return wrappedAnomaly;
}

double CEllipticOrbit::EccentricityAnomaly(const double &time) const
{
    return SolveKeplerEquation(MeanAnomaly(time), m_eccentricity);
}

double CEllipticOrbit::TrueAnomaly(const double &eccentricityAnomaly) const
{
    // Тангенс половинчатого угла
    const double tg_v_2 = sqrt((1 + m_eccentricity) / (1 - m_eccentricity)) * tan(eccentricityAnomaly / 2);
    // Половинчатый угол
    const double v_2 = atan(tg_v_2);
    // Истинная аномалия
    return v_2 + v_2;
}

double CEllipticOrbit::RadiusVectorLength(const double &eccentricityAnomaly) const
{
    return m_largeAxis * (1 - m_eccentricity * cos(eccentricityAnomaly));
}

glm::vec2 CEllipticOrbit::PlanetPosition2D(const double &time) const
{
    const double e = EccentricityAnomaly(time);
    const float r = float(RadiusVectorLength(e));
    const float v = float(TrueAnomaly(e));
    return { r * cosf(v), r * sinf(v) };
}


/*
 * @file geosdb.cpp
 *
 * @author a.sanchez@uni-muenster.de
 *
 * @brief Shared library that loads some geospatial functions into SciDB.
 */
#include <vector>
#include <boost/assign.hpp>
#include <math.h>
#include <boost/geometry.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/geometries/polygon.hpp>
#include <boost/geometry/geometries/adapted/boost_tuple.hpp>

#include "query/Operator.h"
#include "query/FunctionLibrary.h"
#include "query/FunctionDescription.h"
#include "query/TypeSystem.h"

using namespace std;
using namespace scidb;
using namespace boost::assign;
using namespace boost::geometry;

BOOST_GEOMETRY_REGISTER_BOOST_TUPLE_CS(cs::cartesian)

void checkispointinpolygon(const Value** args, Value* res, void*)
{
	double x = args[0]->getDouble();
	double y = args[1]->getDouble();
	std::string wktPolygon = args[2]->getString();

	model::polygon<model::d2::point_xy<double> > poly;
	boost::geometry::read_wkt(wktPolygon, poly);
	boost::tuple<double, double> p = boost::make_tuple(x, y);
	res->setBool(within(p, poly));
}

void checkisprime(const Value** args, Value* res, void*)
{
	int64_t i,n;
	int8_t r;
	stringstream ss;

    n = args[0]->getInt64();
	r = 0;
	ss << n;

	if(n>1)
		for(i=2;i<=sqrt(n);i++)
			if(n%i==0)
			{
				r = 1;
				break;
			}

	if(r==0 && n>1)
		ss << " :prime";
	else
 		ss << " :not prime";

    res->setString(ss.str().c_str());

}

REGISTER_FUNCTION(isprime, list_of("int64"), "string", checkisprime);
REGISTER_FUNCTION(ispointinpolygon, list_of("double")("double")("string"), "bool", checkispointinpolygon);


// This scripts runs the simulations with four populations (Section 6.1)

#include <src/algorithms/neal2_algorithm.h>
#include <src/algorithms/semihdp_sampler.h>
#include <src/collectors/file_collector.h>
#include <src/collectors/memory_collector.h>
#include <src/includes.h>
#include <src/utils/rng.h>

#include <Eigen/Dense>
#include <chrono>
#include <stan/math/prim.hpp>
#include <vector>

using Eigen::MatrixXd;

MatrixXd generate_mixture(double m1, double s1, double m2, double s2, double w,
                          int n) {
  auto& rng = bayesmix::Rng::Instance().get();
  MatrixXd out(n, 1);
  for (int i = 0; i < n; i++) {
    if (stan::math::uniform_rng(0, 1, rng) < w) {
      out(i, 0) = stan::math::normal_rng(m1, s1, rng);
    } else {
      out(i, 0) = stan::math::normal_rng(m2, s2, rng);
    }
  }
  return out;
}

void run_semihdp2(const std::vector<MatrixXd> data, std::string chainfile,
                  std::string update_c = "full") {
  // compute overall mean
  double mu0 = std::accumulate(
      data.begin(), data.end(), 0,
      [&](int curr, const MatrixXd dat) { return curr + dat.sum(); });
  mu0 /= std::accumulate(
      data.begin(), data.end(), 0.0,
      [&](int curr, const MatrixXd dat) { return curr + dat.rows(); });
  auto hier = std::make_shared<NNIGHierarchy>();
  bayesmix::NNIGPrior hier_prior;
  hier_prior.mutable_fixed_values()->set_mean(mu0);
  hier_prior.mutable_fixed_values()->set_var_scaling(0.1);
  hier_prior.mutable_fixed_values()->set_shape(2.0);
  hier_prior.mutable_fixed_values()->set_scale(2.0);
  hier->get_mutable_prior()->CopyFrom(hier_prior);
  hier->initialize();

  // Collect pseudo priors
  std::vector<MemoryCollector> pseudoprior_collectors;
  pseudoprior_collectors.resize(data.size());
  bayesmix::DPPrior mix_prior;
  double totalmass = 1.0;
  mix_prior.mutable_fixed_value()->set_totalmass(totalmass);
  for (int i = 0; i < data.size(); i++) {
    auto mixing = std::make_shared<DirichletMixing>();
    mixing->get_mutable_prior()->CopyFrom(mix_prior);
    mixing->set_num_components(5);
    auto hier = std::make_shared<NNIGHierarchy>();
    bayesmix::NNIGPrior hier_prior;
    hier_prior.mutable_fixed_values()->set_mean(data[i].mean());
    hier_prior.mutable_fixed_values()->set_var_scaling(0.1);
    hier_prior.mutable_fixed_values()->set_shape(2.0);
    hier_prior.mutable_fixed_values()->set_scale(2.0);
    hier->get_mutable_prior()->CopyFrom(hier_prior);

    Neal2Algorithm sampler;
    sampler.set_maxiter(2000);
    sampler.set_burnin(1000);
    sampler.set_mixing(mixing);
    sampler.set_data(data[i]);
    sampler.set_hierarchy(hier);
    sampler.run(&pseudoprior_collectors[i], false);
  }

  auto start = std::chrono::high_resolution_clock::now();
  int nadapt = 10000;
  int nburn = 5000;
  int niter = 5000;
  MemoryCollector collector;

  bayesmix::SemiHdpParams params;
  bayesmix::read_proto_from_file(
      "/home/mario/dev/bayesmix/resources/semihdp_params.asciipb", &params);
  params.set_rest_allocs_update(update_c);

  SemiHdpSampler sampler(data, hier, params);
  sampler.run(nadapt, nburn, niter, 5, &collector, pseudoprior_collectors,
              true, 200);
  auto end = std::chrono::high_resolution_clock::now();
  auto duration =
      std::chrono::duration_cast<std::chrono::seconds>(end - start).count();
  std::cout << "Finished running, duration: " << duration << std::endl;

  collector.write_to_file<bayesmix::SemiHdpState>(chainfile);
}

int main() {
  // Scenario IV std::vector<MatrixXd> data1(4);
  std::vector<MatrixXd> data1(4);
  std::cout << "data1.size(): " << data1.size() << std::endl;
  for (int i = 0; i < 4; i++) {
    data1[i] = MatrixXd::Zero(100, 1);
  }
  std::cout << data1[0].transpose() << std::endl;
  std::cout << "assigning stuff to data1" << std::endl;

  for (int j = 0; j < 100; j++) {
    for (int i = 0; i < 3; i++) {
      auto& rng = bayesmix::Rng::Instance().get();
      data1[i](j, 0) = stan::math::normal_rng(0.0, 1.0, rng);
    }
    auto& rng = bayesmix::Rng::Instance().get();
    data1[3](j, 0) = stan::math::skew_normal_rng(0.0, 1.0, 1.0, rng);
  }

  std::cout << data1[0].transpose() << std::endl;
  std::cout << data1[1].transpose() << std::endl;
  std::cout << data1[2].transpose() << std::endl;
  std::cout << data1[3].transpose() << std::endl;

  std::cout << "Data1 OK" << std::endl;
  run_semihdp2(data1,
               "/home/mario/PhD/exchangeability/semihdp-scripts/"
               "new_chains/s2e1_fullv2.recordio");
  // run_semihdp2(data1,
  //              "/home/mario/PhD/exchangeability/semihdp-scripts/"
  //              "new_chains/s2e1_metro_basev2.recordio",
  //              "metro_base");
  // run_semihdp2(data1,
  //              "/home/mario/PhD/exchangeability/semihdp-scripts/"
  //              "new_chains/s2e1_metro_distv2.recordio",
  //              "metro_dist");

  // // Scenario V
  // auto& rng = bayesmix::Rng::Instance().get();
  // std::vector<MatrixXd> data2(4);
  // for (int i = 0; i < 4; i++) {
  //   data2[i].resize(100, 1);
  // }

  // for (int j = 0; j < 100; j++) {
  //   data2[0](j, 0) = stan::math::normal_rng(0, 1, rng);
  //   data2[3](j, 0) = stan::math::normal_rng(0, 1, rng);
  //   data2[1](j, 0) = stan::math::normal_rng(0, std::sqrt(2.25), rng);
  //   data2[2](j, 0) = stan::math::normal_rng(0, std::sqrt(0.25), rng);
  // }

  // std::cout << "Data2 OK" << std::endl;
  // run_semihdp2(data2,
  //              "/home/mario/PhD/exchangeability/semihdp-scripts/"
  //              "new_chains/s2e2.recordio");

  // // // Scenario VI
  // std::vector<MatrixXd> data3(4);
  // data3[0] = generate_mixture(0, 1, 5, 1, 0.5, 100);
  // data3[1] = generate_mixture(0, 1, 5, 1, 0.5, 100);
  // data3[2] = generate_mixture(0, 1, -5, 1, 0.5, 100);
  // data3[3] = generate_mixture(-5, 1, 5, 1, 0.5, 100);

  // std::cout << "Data3 OK" << std::endl;

  // run_semihdp2(data3,
  //              "/home/mario/PhD/exchangeability/semihdp-scripts/"
  //              "new_chains/s2e3.recordio");
}


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

#ifndef HEADER_EXT_VECTOR2_HPP_INCLUDED
#define HEADER_EXT_VECTOR2_HPP_INCLUDED

#include <cmath>

#include <boost/operators.hpp>

namespace ext
{

	template <typename T>
	class Vector2 :
		boost::equality_comparable<Vector2<T>,
		boost::additive<Vector2<T>,
		boost::multiplicative<Vector2<T>, T
		>>>
	{
	public:
		using Type = T;

		Type x;
		Type y;

		Vector2() : x{0}, y{0} {}
		Vector2(Type x_, Type y_) : x{x_}, y{y_} {}
		template <typename U>
		Vector2(Vector2<U> const& v) : x{v.x}, y{v.y} {}
	};

	using Vector2f = Vector2<float>;
	using Vector2d = Vector2<double>;
	using Vector2ld = Vector2<long double>;

	template <typename T>
	bool operator==(Vector2<T> const& v1, Vector2<T> const& v2)
	{
		return v1.x == v2.x && v1.y == v2.y;
	}

	template <typename T>
	Vector2<T>& operator+=(Vector2<T>& v1, Vector2<T> const& v2)
	{
		v1.x += v2.x;
		v1.y += v2.y;
		return v1;
	}

	template <typename T>
	Vector2<T>& operator-=(Vector2<T>& v1, Vector2<T> const& v2)
	{
		v1.x -= v2.x;
		v1.y -= v2.y;
		return v1;
	}

	template <typename T>
	Vector2<T>& operator*=(Vector2<T>& v, T t)
	{
		v.x *= t;
		v.y *= t;
		return v;
	}

	template <typename T>
	Vector2<T>& operator/=(Vector2<T>& v, T t)
	{
		v.x /= t;
		v.y /= t;
		return v;
	}

	template <typename T>
	Vector2<T> operator+(Vector2<T> const& v)
	{
		return v;
	}

	template <typename T>
	Vector2<T> operator-(Vector2<T> const& v)
	{
		return {-v.x, -v.y};
	}

	template <typename T>
	T dot(Vector2<T> const& v1, Vector2<T> const& v2)
	{
		return v1.x * v2.x + v1.y * v2.y;
	}

	template <typename T>
	T norm(Vector2<T> const& v)
	{
		return std::sqrt(dot(v, v));
	}

	template <typename T>
	Vector2<T> normalize(Vector2<T> const& v)
	{
		return v / norm(v);
	}

} // namespace ext

#endif // !HEADER_EXT_VECTOR2_HPP_INCLUDED