#ifndef STAN_MATH_PRIM_SCAL_PROB_EXPONENTIAL_LPDF_HPP
#define STAN_MATH_PRIM_SCAL_PROB_EXPONENTIAL_LPDF_HPP

#include <stan/math/prim/scal/meta/is_constant_struct.hpp>
#include <stan/math/prim/scal/meta/operands_and_partials.hpp>
#include <stan/math/prim/scal/err/check_consistent_sizes.hpp>
#include <stan/math/prim/scal/err/check_nonnegative.hpp>
#include <stan/math/prim/scal/err/check_not_nan.hpp>
#include <stan/math/prim/scal/err/check_positive_finite.hpp>
#include <stan/math/prim/scal/fun/value_of.hpp>
#include <stan/math/prim/scal/meta/length.hpp>
#include <stan/math/prim/scal/meta/scalar_seq_view.hpp>
#include <stan/math/prim/scal/meta/VectorBuilder.hpp>
#include <stan/math/prim/scal/meta/partials_return_type.hpp>
#include <stan/math/prim/scal/meta/return_type.hpp>
#include <stan/math/prim/scal/fun/constants.hpp>
#include <stan/math/prim/scal/meta/include_summand.hpp>
#include <boost/random/exponential_distribution.hpp>
#include <boost/random/variate_generator.hpp>
#include <cmath>

namespace stan {
  namespace math {

    /**
     * The log of an exponential density for y with the specified
     * inverse scale parameter.
     * Inverse scale parameter must be greater than 0.
     * y must be greater than or equal to 0.
     *
     \f{eqnarray*}{
     y
     &\sim&
     \mbox{\sf{Expon}}(\beta) \\
     \log (p (y \, |\, \beta) )
     &=&
     \log \left( \beta \exp^{-\beta y} \right) \\
     &=&
     \log (\beta) - \beta y \\
     & &
     \mathrm{where} \; y > 0
     \f}
     *
     * @param y A scalar variable.
     * @param beta Inverse scale parameter.
     * @throw std::domain_error if beta is not greater than 0.
     * @throw std::domain_error if y is not greater than or equal to 0.
     * @tparam T_y Type of scalar.
     * @tparam T_inv_scale Type of inverse scale.
     */
    template <bool propto, typename T_y, typename T_inv_scale>
    typename return_type<T_y, T_inv_scale>::type
    exponential_lpdf(const T_y& y, const T_inv_scale& beta) {
      static const char* function("exponential_lpdf");
      typedef typename stan::partials_return_type<T_y, T_inv_scale>::type
        T_partials_return;

      if (!(stan::length(y) && stan::length(beta)))
        return 0.0;

      using std::log;

      T_partials_return logp(0.0);
      check_nonnegative(function, "Random variable", y);
      check_positive_finite(function, "Inverse scale parameter", beta);
      check_consistent_sizes(function,
                             "Random variable", y,
                             "Inverse scale parameter", beta);

      scalar_seq_view<T_y> y_vec(y);
      scalar_seq_view<T_inv_scale> beta_vec(beta);
      size_t N = max_size(y, beta);

      VectorBuilder<include_summand<propto, T_inv_scale>::value,
                    T_partials_return, T_inv_scale> log_beta(length(beta));
      for (size_t i = 0; i < length(beta); i++)
        if (include_summand<propto, T_inv_scale>::value)
          log_beta[i] = log(value_of(beta_vec[i]));

      operands_and_partials<T_y, T_inv_scale>
        ops_partials(y, beta);

      for (size_t n = 0; n < N; n++) {
        const T_partials_return beta_dbl = value_of(beta_vec[n]);
        const T_partials_return y_dbl = value_of(y_vec[n]);
        if (include_summand<propto, T_inv_scale>::value)
          logp += log_beta[n];
        if (include_summand<propto, T_y, T_inv_scale>::value)
          logp -= beta_dbl * y_dbl;

        if (!is_constant_struct<T_y>::value)
          ops_partials.edge1_.partials_[n] -= beta_dbl;
        if (!is_constant_struct<T_inv_scale>::value)
          ops_partials.edge2_.partials_[n] += 1 / beta_dbl - y_dbl;
      }
      return ops_partials.build(logp);
    }

    template <typename T_y, typename T_inv_scale>
    inline
    typename return_type<T_y, T_inv_scale>::type
    exponential_lpdf(const T_y& y, const T_inv_scale& beta) {
      return exponential_lpdf<false>(y, beta);
    }

  }
}
#endif


// Copyright Michael Drexl 2005, 2006.
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt or copy at
// http://boost.org/LICENSE_1_0.txt)

// Example use of the resource-constrained shortest paths algorithm.
#include <boost/config.hpp>

#ifdef BOOST_MSVC
#pragma warning(disable : 4267)
#endif

#include <boost/graph/adjacency_list.hpp>

#include <boost/graph/r_c_shortest_paths.hpp>
#include <iostream>

using namespace boost;

struct SPPRC_Example_Graph_Vert_Prop
{
    SPPRC_Example_Graph_Vert_Prop(int n = 0, int e = 0, int l = 0)
    : num(n), eat(e), lat(l)
    {
    }
    int num;
    // earliest arrival time
    int eat;
    // latest arrival time
    int lat;
};

struct SPPRC_Example_Graph_Arc_Prop
{
    SPPRC_Example_Graph_Arc_Prop(int n = 0, int c = 0, int t = 0)
    : num(n), cost(c), time(t)
    {
    }
    int num;
    // traversal cost
    int cost;
    // traversal time
    int time;
};

typedef adjacency_list< vecS, vecS, directedS, SPPRC_Example_Graph_Vert_Prop,
    SPPRC_Example_Graph_Arc_Prop >
    SPPRC_Example_Graph;

// data structures for spp without resource constraints:
// ResourceContainer model
struct spp_no_rc_res_cont
{
    spp_no_rc_res_cont(int c = 0) : cost(c) {};
    spp_no_rc_res_cont& operator=(const spp_no_rc_res_cont& other)
    {
        if (this == &other)
            return *this;
        this->~spp_no_rc_res_cont();
        new (this) spp_no_rc_res_cont(other);
        return *this;
    }
    int cost;
};

bool operator==(
    const spp_no_rc_res_cont& res_cont_1, const spp_no_rc_res_cont& res_cont_2)
{
    return (res_cont_1.cost == res_cont_2.cost);
}

bool operator<(
    const spp_no_rc_res_cont& res_cont_1, const spp_no_rc_res_cont& res_cont_2)
{
    return (res_cont_1.cost < res_cont_2.cost);
}

// ResourceExtensionFunction model
class ref_no_res_cont
{
public:
    inline bool operator()(const SPPRC_Example_Graph& g,
        spp_no_rc_res_cont& new_cont, const spp_no_rc_res_cont& old_cont,
        graph_traits< SPPRC_Example_Graph >::edge_descriptor ed) const
    {
        new_cont.cost = old_cont.cost + g[ed].cost;
        return true;
    }
};

// DominanceFunction model
class dominance_no_res_cont
{
public:
    inline bool operator()(const spp_no_rc_res_cont& res_cont_1,
        const spp_no_rc_res_cont& res_cont_2) const
    {
        // must be "<=" here!!!
        // must NOT be "<"!!!
        return res_cont_1.cost <= res_cont_2.cost;
        // this is not a contradiction to the documentation
        // the documentation says:
        // "A label $l_1$ dominates a label $l_2$ if and only if both are
        // resident at the same vertex, and if, for each resource, the resource
        // consumption of $l_1$ is less than or equal to the resource
        // consumption of $l_2$, and if there is at least one resource where
        // $l_1$ has a lower resource consumption than $l_2$." one can think of
        // a new label with a resource consumption equal to that of an old label
        // as being dominated by that old label, because the new one will have a
        // higher number and is created at a later point in time, so one can
        // implicitly use the number or the creation time as a resource for
        // tie-breaking
    }
};
// end data structures for spp without resource constraints:

// data structures for shortest path problem with time windows (spptw)
// ResourceContainer model
struct spp_spptw_res_cont
{
    spp_spptw_res_cont(int c = 0, int t = 0) : cost(c), time(t) {}
    spp_spptw_res_cont& operator=(const spp_spptw_res_cont& other)
    {
        if (this == &other)
            return *this;
        this->~spp_spptw_res_cont();
        new (this) spp_spptw_res_cont(other);
        return *this;
    }
    int cost;
    int time;
};

bool operator==(
    const spp_spptw_res_cont& res_cont_1, const spp_spptw_res_cont& res_cont_2)
{
    return (res_cont_1.cost == res_cont_2.cost
        && res_cont_1.time == res_cont_2.time);
}

bool operator<(
    const spp_spptw_res_cont& res_cont_1, const spp_spptw_res_cont& res_cont_2)
{
    if (res_cont_1.cost > res_cont_2.cost)
        return false;
    if (res_cont_1.cost == res_cont_2.cost)
        return res_cont_1.time < res_cont_2.time;
    return true;
}

// ResourceExtensionFunction model
class ref_spptw
{
public:
    inline bool operator()(const SPPRC_Example_Graph& g,
        spp_spptw_res_cont& new_cont, const spp_spptw_res_cont& old_cont,
        graph_traits< SPPRC_Example_Graph >::edge_descriptor ed) const
    {
        const SPPRC_Example_Graph_Arc_Prop& arc_prop = get(edge_bundle, g)[ed];
        const SPPRC_Example_Graph_Vert_Prop& vert_prop
            = get(vertex_bundle, g)[target(ed, g)];
        new_cont.cost = old_cont.cost + arc_prop.cost;
        int& i_time = new_cont.time;
        i_time = old_cont.time + arc_prop.time;
        i_time < vert_prop.eat ? i_time = vert_prop.eat : 0;
        return i_time <= vert_prop.lat ? true : false;
    }
};

// DominanceFunction model
class dominance_spptw
{
public:
    inline bool operator()(const spp_spptw_res_cont& res_cont_1,
        const spp_spptw_res_cont& res_cont_2) const
    {
        // must be "<=" here!!!
        // must NOT be "<"!!!
        return res_cont_1.cost <= res_cont_2.cost
            && res_cont_1.time <= res_cont_2.time;
        // this is not a contradiction to the documentation
        // the documentation says:
        // "A label $l_1$ dominates a label $l_2$ if and only if both are
        // resident at the same vertex, and if, for each resource, the resource
        // consumption of $l_1$ is less than or equal to the resource
        // consumption of $l_2$, and if there is at least one resource where
        // $l_1$ has a lower resource consumption than $l_2$." one can think of
        // a new label with a resource consumption equal to that of an old label
        // as being dominated by that old label, because the new one will have a
        // higher number and is created at a later point in time, so one can
        // implicitly use the number or the creation time as a resource for
        // tie-breaking
    }
};
// end data structures for shortest path problem with time windows (spptw)

// example graph structure and cost from
// http://www.boost.org/libs/graph/example/dijkstra-example.cpp
enum nodes
{
    A,
    B,
    C,
    D,
    E
};
char name[] = "ABCDE";

int main()
{
    SPPRC_Example_Graph g;

    add_vertex(SPPRC_Example_Graph_Vert_Prop(A, 0, 0), g);
    add_vertex(SPPRC_Example_Graph_Vert_Prop(B, 5, 20), g);
    add_vertex(SPPRC_Example_Graph_Vert_Prop(C, 6, 10), g);
    add_vertex(SPPRC_Example_Graph_Vert_Prop(D, 3, 12), g);
    add_vertex(SPPRC_Example_Graph_Vert_Prop(E, 0, 100), g);

    add_edge(A, C, SPPRC_Example_Graph_Arc_Prop(0, 1, 5), g);
    add_edge(B, B, SPPRC_Example_Graph_Arc_Prop(1, 2, 5), g);
    add_edge(B, D, SPPRC_Example_Graph_Arc_Prop(2, 1, 2), g);
    add_edge(B, E, SPPRC_Example_Graph_Arc_Prop(3, 2, 7), g);
    add_edge(C, B, SPPRC_Example_Graph_Arc_Prop(4, 7, 3), g);
    add_edge(C, D, SPPRC_Example_Graph_Arc_Prop(5, 3, 8), g);
    add_edge(D, E, SPPRC_Example_Graph_Arc_Prop(6, 1, 3), g);
    add_edge(E, A, SPPRC_Example_Graph_Arc_Prop(7, 1, 5), g);
    add_edge(E, B, SPPRC_Example_Graph_Arc_Prop(8, 1, 4), g);

    // the unique shortest path from A to E in the dijkstra-example.cpp is
    // A -> C -> D -> E
    // its length is 5
    // the following code also yields this result

    // with the above time windows, this path is infeasible
    // now, there are two shortest paths that are also feasible with respect to
    // the vertex time windows:
    // A -> C -> B -> D -> E and
    // A -> C -> B -> E
    // however, the latter has a longer total travel time and is therefore not
    // pareto-optimal, i.e., it is dominated by the former path
    // therefore, the code below returns only the former path

    // spp without resource constraints
    graph_traits< SPPRC_Example_Graph >::vertex_descriptor s = A;
    graph_traits< SPPRC_Example_Graph >::vertex_descriptor t = E;

    std::vector<
        std::vector< graph_traits< SPPRC_Example_Graph >::edge_descriptor > >
        opt_solutions;
    std::vector< spp_no_rc_res_cont > pareto_opt_rcs_no_rc;

    r_c_shortest_paths(g, get(&SPPRC_Example_Graph_Vert_Prop::num, g),
        get(&SPPRC_Example_Graph_Arc_Prop::num, g), s, t, opt_solutions,
        pareto_opt_rcs_no_rc, spp_no_rc_res_cont(0), ref_no_res_cont(),
        dominance_no_res_cont(),
        std::allocator< r_c_shortest_paths_label< SPPRC_Example_Graph,
            spp_no_rc_res_cont > >(),
        default_r_c_shortest_paths_visitor());

    std::cout << "SPP without resource constraints:" << std::endl;
    std::cout << "Number of optimal solutions: ";
    std::cout << static_cast< int >(opt_solutions.size()) << std::endl;
    for (int i = 0; i < static_cast< int >(opt_solutions.size()); ++i)
    {
        std::cout << "The " << i << "th shortest path from A to E is: ";
        std::cout << std::endl;
        for (int j = static_cast< int >(opt_solutions[i].size()) - 1; j >= 0;
             --j)
            std::cout << name[source(opt_solutions[i][j], g)] << std::endl;
        std::cout << "E" << std::endl;
        std::cout << "Length: " << pareto_opt_rcs_no_rc[i].cost << std::endl;
    }
    std::cout << std::endl;

    // spptw
    std::vector<
        std::vector< graph_traits< SPPRC_Example_Graph >::edge_descriptor > >
        opt_solutions_spptw;
    std::vector< spp_spptw_res_cont > pareto_opt_rcs_spptw;

    r_c_shortest_paths(g, get(&SPPRC_Example_Graph_Vert_Prop::num, g),
        get(&SPPRC_Example_Graph_Arc_Prop::num, g), s, t, opt_solutions_spptw,
        pareto_opt_rcs_spptw, spp_spptw_res_cont(0, 0), ref_spptw(),
        dominance_spptw(),
        std::allocator< r_c_shortest_paths_label< SPPRC_Example_Graph,
            spp_spptw_res_cont > >(),
        default_r_c_shortest_paths_visitor());

    std::cout << "SPP with time windows:" << std::endl;
    std::cout << "Number of optimal solutions: ";
    std::cout << static_cast< int >(opt_solutions.size()) << std::endl;
    for (int i = 0; i < static_cast< int >(opt_solutions.size()); ++i)
    {
        std::cout << "The " << i << "th shortest path from A to E is: ";
        std::cout << std::endl;
        for (int j = static_cast< int >(opt_solutions_spptw[i].size()) - 1;
             j >= 0; --j)
            std::cout << name[source(opt_solutions_spptw[i][j], g)]
                      << std::endl;
        std::cout << "E" << std::endl;
        std::cout << "Length: " << pareto_opt_rcs_spptw[i].cost << std::endl;
        std::cout << "Time: " << pareto_opt_rcs_spptw[i].time << std::endl;
    }

    // utility function check_r_c_path example
    std::cout << std::endl;
    bool b_is_a_path_at_all = false;
    bool b_feasible = false;
    bool b_correctly_extended = false;
    spp_spptw_res_cont actual_final_resource_levels(0, 0);
    graph_traits< SPPRC_Example_Graph >::edge_descriptor ed_last_extended_arc;
    check_r_c_path(g, opt_solutions_spptw[0], spp_spptw_res_cont(0, 0), true,
        pareto_opt_rcs_spptw[0], actual_final_resource_levels, ref_spptw(),
        b_is_a_path_at_all, b_feasible, b_correctly_extended,
        ed_last_extended_arc);
    if (!b_is_a_path_at_all)
        std::cout << "Not a path." << std::endl;
    if (!b_feasible)
        std::cout << "Not a feasible path." << std::endl;
    if (!b_correctly_extended)
        std::cout << "Not correctly extended." << std::endl;
    if (b_is_a_path_at_all && b_feasible && b_correctly_extended)
    {
        std::cout << "Actual final resource levels:" << std::endl;
        std::cout << "Length: " << actual_final_resource_levels.cost
                  << std::endl;
        std::cout << "Time: " << actual_final_resource_levels.time << std::endl;
        std::cout << "OK." << std::endl;
    }

    return 0;
}


#include <boost/mp11/mpl.hpp>
#include <tuple>
#include <type_traits>

template <int I>
using int_ = std::integral_constant<int, I>;

int main()
{
    using v1 = boost::mp11::mp_list_c<int, 5, 2, 3, 1, 4>;
    using index1 = boost::mp11::mp_find<v1, int_<3>>;
    using size1 = boost::mp11::mp_size<v1>;
    using in_v1 = boost::mp11::mp_less<index1, size1>;
    constexpr int r1{ boost::mp11::mp_if<in_v1, index1, int_<-1>>::value };
    static_assert(r1 == 2);

    using v2 = std::tuple<int_<5>, int_<2>, int_<3>>;
    using index2 = boost::mp11::mp_find<v2, int_<6>>;
    using size2 = boost::mp11::mp_size<v2>;
    using in_v2 = boost::mp11::mp_less<index2, size2>;
    constexpr int r2{ boost::mp11::mp_if<in_v2, index2, int_<-1>>::value };
    static_assert(r2 == -1);
}


#include <frovedis.hpp>
#include <frovedis/ml/clustering/shrink_kmeans.hpp>

#include <boost/program_options.hpp>

using namespace boost;
using namespace frovedis;
using namespace std;

void do_kmeans(const string& input, bool dense, const string& output, int k,
               int num_iteration, double eps, bool binary) {
  if(binary) {
    if(dense) {
      time_spent t(DEBUG);
      auto mat = make_rowmajor_matrix_loadbinary<double>(input);
      t.show("load matrix: ");
      auto r = kmeans(mat, k, num_iteration, eps);
      t.show("kmeans time: ");
      r.transpose().savebinary(output);
      t.show("save centroid time: ");
    } else {
      time_spent t(DEBUG);
      auto mat = make_crs_matrix_loadbinary<double>(input);
      t.show("load matrix: ");
      auto r = kmeans(std::move(mat), k, num_iteration, eps);
      t.show("kmeans time: ");
      r.transpose().savebinary(output);
      t.show("save centroid time: ");
    }
  } else {
    if(dense) {
      time_spent t(DEBUG);
      auto mat = make_rowmajor_matrix_load<double>(input);
      t.show("load matrix: ");
      auto r = kmeans(mat, k, num_iteration, eps);
      t.show("kmeans time: ");
      r.transpose().save(output);
      t.show("save model time: ");
    } else {
      time_spent t(DEBUG);
      auto mat = make_crs_matrix_load<double>(input);
      t.show("load matrix: ");
      auto r = kmeans(std::move(mat), k, num_iteration, eps);
      t.show("kmeans time: ");
      r.transpose().save(output);
      t.show("save model time: ");
    }
  }
}

void do_assign(const string& input, bool dense, const string& input_centroid,
               const string& output, bool binary) {
  if(binary) {
    if(dense) {
      time_spent t(DEBUG);
      auto mat = make_rowmajor_matrix_local_loadbinary<double>(input);
      t.show("load matrix: ");
      auto c = make_rowmajor_matrix_local_loadbinary<double>(input_centroid);
      t.show("load centroid: ");
      auto ct = c.transpose();
      auto r = kmeans_assign_cluster(mat, ct);
      make_dvector_scatter(r).savebinary(output);
    } else {
      time_spent t(DEBUG);
      auto mat = make_crs_matrix_local_loadbinary<double>(input);
      t.show("load matrix: ");
      auto c = make_rowmajor_matrix_local_loadbinary<double>(input_centroid);
      t.show("load centroid: ");
      auto ct = c.transpose();
      auto r = kmeans_assign_cluster(mat, ct);
      make_dvector_scatter(r).savebinary(output);
    }
  } else {
    if(dense) {
      time_spent t(DEBUG);
      auto mat = make_rowmajor_matrix_local_load<double>(input);
      t.show("load matrix: ");
      auto c = make_rowmajor_matrix_local_load<double>(input_centroid);
      t.show("load centroid: ");
      auto ct = c.transpose();
      auto r = kmeans_assign_cluster(mat, ct);
      make_dvector_scatter(r).saveline(output);
    } else {
      time_spent t(DEBUG);
      auto mat = make_crs_matrix_local_load<double>(input);
      t.show("load matrix: ");
      auto c = make_rowmajor_matrix_local_load<double>(input_centroid);
      t.show("load centroid: ");
      auto ct = c.transpose();
      auto r = kmeans_assign_cluster(mat, ct);
      make_dvector_scatter(r).saveline(output);
    }
  }
}

int main(int argc, char* argv[]) {
  use_frovedis use(argc, argv);

  using namespace boost::program_options;

  options_description opt("option");
  opt.add_options()
    ("help,h", "print help")
    ("assign,a", "assign data to cluster mode")
    ("input,i", value<string>(), "input matrix")
    ("centroid,c", value<string>(), "input centroid for assignment")
    ("output,o", value<string>(), "output centroids or cluster")
    ("k,k", value<int>(), "number of clusters")
    ("num-iteration,n", value<int>(), "number of max iteration")
    ("eps,e", value<double>(), "epsilon to stop the iteration")
    ("sparse,s", "use sparse matrix [default]")
    ("dense,d", "use dense matrix")
    ("verbose", "set loglevel to DEBUG")
    ("verbose2", "set loglevel to TRACE")
    ("binary,b", "use binary input/output");

  variables_map argmap;
  store(command_line_parser(argc,argv).options(opt).allow_unregistered().
        run(), argmap);
  notify(argmap);

  string input, output, input_centroid;
  int num_iteration = 100;
  int k;
  double eps = 0.01;
  bool assign = false;
  bool dense = false;
  bool binary = false;

  if(argmap.count("help")){
    cerr << opt << endl;
    exit(1);
  }

  if(argmap.count("input")){
    input = argmap["input"].as<string>();
  } else {
    cerr << "input is not specified" << endl;
    cerr << opt << endl;
    exit(1);
  }

  if(argmap.count("output")){
    output = argmap["output"].as<string>();
  } else {
    cerr << "output is not specified" << endl;
    cerr << opt << endl;
    exit(1);
  }

  if(argmap.count("assign")){
    assign = true;
  }

  if(argmap.count("centroid")){
    input_centroid = argmap["centroid"].as<string>();
  } else {
    if(assign == true) {
      cerr << "output is not specified" << endl;
      cerr << opt << endl;
      exit(1);
    }
  }

  if(argmap.count("k")){
    k = argmap["k"].as<int>();
  } else {
    if(assign == false) {
      cerr << "number of cluster is not specified" << endl;
      cerr << opt << endl;
      exit(1);
    }
  }

  if(argmap.count("num-iteration")){
    num_iteration = argmap["num-iteration"].as<int>();
  }

  if(argmap.count("epsilon")){
    eps = argmap["epsilon"].as<double>();
  }

  if(argmap.count("sparse")){
    dense = false;
  }

  if(argmap.count("dense")){
    dense = true;
  }

  if(argmap.count("binary")){
    binary = true;
  }

  if(argmap.count("verbose")){
    set_loglevel(DEBUG);
  }

  if(argmap.count("verbose2")){
    set_loglevel(TRACE);
  }

  if(assign) do_assign(input, dense, input_centroid, output, binary);
  else do_kmeans(input, dense, output, k, num_iteration, eps, binary);
}


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

#include "core/volumetric/VolumetricOptimization.h"

float count_dist(float *r) {
    float dist = 0;
    for (int i = 0; i < 6891; i++)
        dist += r[i] * r[i];
    return sqrt(dist);
}

void surfelwarp::VolumetricOptimization::Solve(
        DeviceArrayView<float4> &live_vertex,
        cudaStream_t stream
) {
    CheckPoints(live_vertex, stream);
    for (auto i = 0; i < Constants::kNumGaussNewtonIterations; i++) {
        float r[6891], j[6891 * 72];
        ComputeJacobian(live_vertex, j, stream);
        ComputeResidual(live_vertex, r, stream);

        Eigen::MatrixXf J = Eigen::Map<Eigen::Matrix<float, 6891, 72>>(j);
        Eigen::VectorXf R(Eigen::Map<Eigen::VectorXf>(r, 6891));
        Eigen::MatrixXf Jt = J.transpose();
        Eigen::MatrixXf JtJ = Jt * J; // A
        Eigen::VectorXf b = -1 * Jt * R;

        Eigen::LeastSquaresConjugateGradient<Eigen::MatrixXf> lscg;
        lscg.compute(JtJ);
        auto x = lscg.solve(b);

        std::vector<float> v;
        for (int p = 0; p < 72; p++)
            v.push_back(0.5 * x(p));
        m_smpl_handler->AddTheta(v);

        ComputeResidual(live_vertex, r, stream);
        float dist1 = count_dist(r);

        m_smpl_handler->SubTheta(v);
        m_smpl_handler->SubTheta(v);

        ComputeResidual(live_vertex, r, stream);
        float dist2 = count_dist(r);
        if (dist2 > dist1) {
            m_smpl_handler->AddTheta(v);
            m_smpl_handler->AddTheta(v);
        }
    }
}


#include <NTL/mat_GF2.h>
#include <NTL/matrix.h>
#include <NTL/GF2.h>
#include <NTL/version.h>

using namespace NTL;
using namespace std;

int main(int argc, char * argv[])
{
    Mat<GF2> mat;
    mat.SetDims(2, 2);
    for (int i = 0; i < 2; i++) {
        for (int j = 0; j < 2; j++) {
            cout << "mat.put(i, j, 1)" << endl;
            mat.put(i, j, 1);
            cout << "mat.get(i, j) = " << mat.get(i, j) << endl;
            cout << "mat.put(i, j, 0)" << endl;
            mat.put(i, j, 0);
            cout << "mat.get(i, j) = " << mat.get(i, j) << endl;
        }
    }
    mat.put(0,0,1);
    mat.put(0,1,1);
    mat.put(1,0,1);
    mat.put(1,1,1);
    cout << mat << endl;
}


#include <iostream>
#include <vector>
#include <thread>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/search/search.h>
#include <pcl/search/kdtree.h>
#include <pcl/features/normal_3d.h>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/filters/passthrough.h>
#include <pcl/segmentation/region_growing.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/segmentation/sac_segmentation.h>
//#include <pcl/ModelCoefficients.h>
//#include <pcl/sample_consensus/method_types.h>
//#include <pcl/sample_consensus/model_types.h>

#include <ros/ros.h>
#include <geometry_msgs/PointStamped.h>
#include <sensor_msgs/PointCloud2.h>
#include <visualization_msgs/Marker.h>
#include <visualization_msgs/MarkerArray.h>
#include <Eigen/Core>
#include <Eigen/Dense>
#include <dynamic_reconfigure/server.h>
#include "region_growing_segmentation/TutorialsConfig.h"
#define PI 3.14
pcl::PointXYZ click_point;
bool new_point = false;
Eigen::Vector3f marker_normal(0, 1, 0);
float bias_x, bias_y, bias_z = 0;
float bias_s_x, bias_s_z = 0;
float bias_roll = 0, bias_pitch = 0, bias_yaw = 0;
float bias_q_x, bias_q_y, bias_q_z, bias_q_w = 0;
//pcl::visualization::CloudViewer viewer("Cluster viewer");
visualization_msgs::MarkerArray marker_array;
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
void callback(region_growing_segmentation::TutorialsConfig &config, uint32_t level) {
  ROS_INFO("Reconfigure Request: %f %f",
           config.position_x, config.position_y
  );
  bias_roll = config.roll * PI / 180;
  bias_pitch = config.pitch * PI / 180;
  bias_yaw = config.yaw * PI / 180;
  Eigen::Quaternionf quaternionf;
  Eigen::Vector3f euler_angle(bias_yaw, bias_pitch, bias_roll);
  quaternionf = Eigen::AngleAxisf(euler_angle[0], Eigen::Vector3f::UnitZ()) *
      Eigen::AngleAxisf(euler_angle[1], Eigen::Vector3f::UnitY()) *
      Eigen::AngleAxisf(euler_angle[2], Eigen::Vector3f::UnitX());

  bias_x = config.position_x;
  bias_y = config.position_y;
  bias_z = config.position_z;
  bias_q_x = quaternionf.x();
  bias_q_y = quaternionf.y();
  bias_q_z = quaternionf.z();
//  bias_q_w = quaternionf.w();
//  cout << "quaternionf x: " << quaternionf.x() << endl;
//  cout << "quaternionf y: " << quaternionf.y() << endl;
//  cout << "quaternionf z: " << quaternionf.z() << endl;
//  cout << "quaternionf w: " << quaternionf.w() << endl;

  bias_s_x = config.scale_x;
  bias_s_z = config.scale_z;
}

void pointCallback(const geometry_msgs::PointStampedPtr &msg) {
  std::cout << "new point" << std::endl;
  click_point.x = msg->point.x;
  click_point.y = msg->point.y;
  click_point.z = msg->point.z;
  new_point = true;
}

void cloudCallback(const sensor_msgs::PointCloud2ConstPtr &input) {
  //cout << "I RECEIVED INPUT !" << endl;
  //std::lock_guard<std::mutex> lock(cloud_lock_);
  pcl::fromROSMsg(*input, *cloud);
}
//void runViewer(){
//  while (!viewer.wasStopped()) {
//  }
//}
Eigen::Vector3f crossProduct(Eigen::Vector3f a, Eigen::Vector3f b) {
  Eigen::Vector3f c;

  c[0] = a[1] * b[2] - a[2] * b[1];
  c[1] = a[2] * b[0] - a[0] * b[2];
  c[2] = a[0] * b[1] - a[1] * b[0];

  return c;
}

float dotProduct(Eigen::Vector3f a, Eigen::Vector3f b) {
  float result;
  result = a[0] * b[0] + a[1] * b[1] + a[2] * b[2];
  return result;
}

float normalize(Eigen::Vector3f v) {
  float result;
  result = sqrt(v[0] * v[0] + v[1] * v[1] + v[2] * v[2]);
  return result;
}

Eigen::Matrix3f rotationMatrix(float angle, Eigen::Vector3f u) {
  float norm = normalize(u);
  Eigen::Matrix3f rotatinMatrix;

  u(0) = u(0) / norm;
  u(1) = u(1) / norm;
  u(2) = u(2) / norm;

  rotatinMatrix(0, 0) = cos(angle) + u(0) * u(0) * (1 - cos(angle));
  rotatinMatrix(0, 1) = u(0) * u(1) * (1 - cos(angle)) - u(2) * sin(angle);
  rotatinMatrix(0, 2) = u(1) * sin(angle) + u(0) * u(2) * (1 - cos(angle));

  rotatinMatrix(1, 0) = u(2) * sin(angle) + u(0) * u(1) * (1 - cos(angle));
  rotatinMatrix(1, 1) = cos(angle) + u(1) * u(1) * (1 - cos(angle));
  rotatinMatrix(1, 2) = -u(0) * sin(angle) + u(1) * u(2) * (1 - cos(angle));

  rotatinMatrix(2, 0) = -u(1) * sin(angle) + u(0) * u(2) * (1 - cos(angle));
  rotatinMatrix(2, 1) = u(0) * sin(angle) + u(1) * u(2) * (1 - cos(angle));
  rotatinMatrix(2, 2) = cos(angle) + u(2) * u(2) * (1 - cos(angle));

  return rotatinMatrix;
}
Eigen::Matrix3f calculationNormalRotation(Eigen::Vector3f plane_normal) {
  Eigen::Vector3f rotation_axis;
  float rotation_angle;
  Eigen::Matrix3f rotation_matrix;
  rotation_axis = crossProduct(marker_normal, plane_normal);
  rotation_angle = acos(dotProduct(marker_normal, plane_normal) / normalize(marker_normal) / normalize(plane_normal));
  rotation_matrix = rotationMatrix(rotation_angle, rotation_axis);
  return rotation_matrix;
}

visualization_msgs::MarkerArray drawMarker(Eigen::Vector3f plane_normal, pcl::PointXYZ min_point) {
  Eigen::Matrix3f rotation_matrix = calculationNormalRotation(plane_normal);
  Eigen::Quaternionf q(rotation_matrix);
  visualization_msgs::Marker marker;
  marker.header.frame_id = "livox_frame";
  marker.header.stamp = ros::Time();
  marker.ns = "/";
  marker.id = 0;
  marker.type = visualization_msgs::Marker::CUBE;
  marker.action = visualization_msgs::Marker::ADD;
  marker.pose.position.x = min_point.x + bias_x;
  marker.pose.position.y = min_point.y + bias_y;
  marker.pose.position.z = min_point.z + bias_z;
  marker.pose.orientation.x = q.x() + bias_q_x;
  marker.pose.orientation.y = q.y() + bias_q_y;
  marker.pose.orientation.z = q.z() + bias_q_z;
  marker.pose.orientation.w = q.w() + bias_q_w;
  //std::cout << "quaternion x y z :" << q.x() << " " << q.y() << " " << q.z() << std::endl;
  marker.scale.x = 10 + bias_s_x;
  marker.scale.y = 0.1;
  marker.scale.z = 10 + bias_s_z;
  marker.color.a = 1.0; // Don't forget to set the alpha!
  marker.color.r = 0.0;
  marker.color.g = 1.0;
  marker.color.b = 0.0;
  marker_array.markers.push_back(marker);
  return marker_array;
}

pcl::PointXYZ minDisPoint(pcl::PointCloud<pcl::PointXYZ> result_point) {
  int min_dis = 5000;
  int temp_dis = 0;
  int point_index = 0;
  for (int i = 0; i < result_point.points.size(); i++) {
    temp_dis = abs(0.099276 * result_point.points[i].x - 0.994441 * result_point.points[i].y
                       + 0.03509592 * result_point.points[i].z + 14.3107);
    if (temp_dis < min_dis) {
      min_dis = temp_dis;
      point_index = i;
    }
  }
  std::cout << "min distance: " << min_dis << std::endl;
  return result_point.points[point_index];
}

int
main(int argc, char **argv) {
  ros::init(argc, argv, "region_growing_segmention");

  dynamic_reconfigure::Server<region_growing_segmentation::TutorialsConfig> server;
  dynamic_reconfigure::Server<region_growing_segmentation::TutorialsConfig>::CallbackType f;

  f = boost::bind(&callback, _1, _2);
  server.setCallback(f);

  ros::NodeHandle nh;
  ros::Subscriber point_sub = nh.subscribe("/clicked_point", 1, pointCallback);
  ros::Publisher seg_cloud_pub = nh.advertise<sensor_msgs::PointCloud2>("/seg_pointcloud", 1);
  ros::Publisher cloud_pub = nh.advertise<sensor_msgs::PointCloud2>("/pointcloud", 1);
  ros::Publisher vis_pub = nh.advertise<visualization_msgs::MarkerArray>("visualization_marker", 0);
  ros::Subscriber cloud_sub = nh.subscribe("/trans_pointcloud",1,cloudCallback);
//  if (pcl::io::loadPCDFile<pcl::PointXYZ>("/home/hjx/Documents/sub_map.pcd", *cloud) == -1) {
//    std::cout << "Cloud reading failed." << std::endl;
//    return (-1);
//  }

  pcl::PointCloud<pcl::PointXYZ>::Ptr downsampled(new pcl::PointCloud<pcl::PointXYZ>());
  pcl::VoxelGrid<pcl::PointXYZ> voxelgrid;
  voxelgrid.setLeafSize(0.1f, 0.1f, 0.1f);
//  voxelgrid.setInputCloud(cloud);
//  voxelgrid.filter(*downsampled);
//  *cloud = *downsampled;

//  sensor_msgs::PointCloud2 out_pointcloud;
//  pcl::toROSMsg(*cloud, out_pointcloud);
//  out_pointcloud.header.frame_id = "livox_frame";
  //cloud_pub.publish(out_pointcloud);

  pcl::search::Search<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>);
  pcl::PointCloud<pcl::Normal>::Ptr normals(new pcl::PointCloud<pcl::Normal>);
  pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> normal_estimator;
  normal_estimator.setSearchMethod(tree);
//  normal_estimator.setInputCloud(cloud);
//  normal_estimator.setKSearch(50);
//  normal_estimator.compute(*normals);

//  pcl::IndicesPtr indices(new std::vector<int>);
//  pcl::PassThrough<pcl::PointXYZ> pass;
//  pass.setInputCloud(cloud);
//  pass.setFilterFieldName("z");
//  pass.setFilterLimits(0.0, 1.0);
//  pass.filter(*indices);

  pcl::RegionGrowing<pcl::PointXYZ, pcl::Normal> reg;
  reg.setMinClusterSize(50);
  reg.setMaxClusterSize(1000000);
  reg.setSearchMethod(tree);
  reg.setNumberOfNeighbours(30);
//  reg.setInputCloud(cloud);
//  //reg.setIndices (indices);
//  reg.setInputNormals(normals);
  reg.setSmoothnessThreshold(3.0 / 180.0 * M_PI);
  reg.setCurvatureThreshold(1.0);
  pcl::PointIndices point_cluster;

  std::vector<int> index;
  std::vector<float> sqr_distance;
  pcl::PointCloud<pcl::PointXYZRGB>::Ptr colored_cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
  pcl::PointCloud<pcl::PointXYZ> result_point;
  sensor_msgs::PointCloud2 seg_out_pointcloud;
  visualization_msgs::MarkerArray marker;
  pcl::PointXYZ min_point = click_point;
  Eigen::Vector3f plane_normal(0, 1, 0);
  ros::Rate sleep_rate(10);
  while (ros::ok()) {
    //cloud_pub.publish(out_pointcloud);
    if (new_point) {
      voxelgrid.setInputCloud(cloud);
      voxelgrid.filter(*downsampled);
      *cloud = *downsampled;

      normal_estimator.setInputCloud(cloud);
      normal_estimator.setKSearch(50);
      normal_estimator.compute(*normals);

      reg.setInputCloud(cloud);
      //reg.setIndices (indices);
      reg.setInputNormals(normals);

      tree->nearestKSearch(click_point, 1, index, sqr_distance);
      reg.getSegmentFromPoint(index[0], point_cluster);
      result_point.clear();
      for (std::vector<int>::iterator it = point_cluster.indices.begin(); it != point_cluster.indices.end(); it++) {
        result_point.points.push_back(cloud->points[*it]);
      }

      pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
      pcl::PointIndices::Ptr inliers(new pcl::PointIndices);

      pcl::SACSegmentation<pcl::PointXYZ> seg;

      seg.setOptimizeCoefficients(true);

      seg.setModelType(pcl::SACMODEL_PLANE);
      seg.setMethodType(pcl::SAC_RANSAC);
      seg.setDistanceThreshold(0.01);
      seg.setInputCloud(result_point.makeShared());
      seg.segment(*inliers, *coefficients);
      if (inliers->indices.size() == 0) {
        PCL_ERROR ("Could not estimate a planar model for the given dataset.");
        return (-1);
      }
      std::cerr << "Model coefficients: " << coefficients->values[0] << " "
                << coefficients->values[1] << " "
                << coefficients->values[2] << " "
                << coefficients->values[3] << std::endl;
      plane_normal << coefficients->values[0], coefficients->values[1], coefficients->values[2];
      min_point = minDisPoint(result_point);

//      std::vector<pcl::PointIndices> clusters;
//      reg.extract(clusters);
//
//      std::cout << "Number of clusters is equal to " << clusters.size() << std::endl;
//      std::cout << "First cluster has " << clusters[0].indices.size() << " points." << std::endl;
//      std::cout << "These are the indices of the points of the initial" <<
//                std::endl << "cloud that belong to the first cluster:" << std::endl;
//      int counter = 0;
//      while (counter < clusters[0].indices.size()) {
//        std::cout << clusters[0].indices[counter] << ", ";
//        counter++;
//        if (counter % 10 == 0)
//          std::cout << std::endl;
//      }
//      std::cout << std::endl;

//      pcl::PointCloud<pcl::PointXYZRGB>::Ptr colored_cloud = reg.getColoredCloud();
      //colored_cloud = reg.getColoredCloud();
      //pcl::io::savePCDFileASCII("colored_cloud.pcd", result_point);
      new_point = false;
    }
    marker = drawMarker(plane_normal, min_point);

    vis_pub.publish(marker);

//    pcl::visualization::CloudViewer viewer("Cluster viewer");
    if (!result_point.empty()) {
      pcl::toROSMsg(result_point, seg_out_pointcloud);
      seg_out_pointcloud.header.frame_id = "livox_frame";
      seg_cloud_pub.publish(seg_out_pointcloud);
    }
    ros::spinOnce();
    sleep_rate.sleep();

  }
//  while (!viewer.wasStopped()) {
//  }

  return (0);
}


# pragma once
//#define EIGEN_USE_MKL_ALL
#include"basis.hpp"
#include<cmath>

#include <Eigen/Sparse>
#ifdef MOM
using ValType= std::complex<double>;
#else
using ValType= double;
#endif
namespace Operators{
  using Many_Body::pi;
  using  Many_Body::GetLattice;
  using  Many_Body::Translate;
  using  Many_Body::Position;
  //   using  Many_Body::State();
  using Many_Body::RightId;
  using Many_Body::LeftId;

  using Mat= Eigen::SparseMatrix<ValType,Eigen::RowMajor>;

    template<typename T>
    size_t CheckSign(const T& state, size_t i, size_t j)
  {
    size_t m=0;
    if(i>j)
      {
  	for(size_t l=j+1; l<i; l++)
  	  {
  	    m+=size_t(state[l]);
  	  }

      }
    else
      	for(size_t l=i+1; l<j; l++)
  	  {
  	    m+=size_t(state[l]);
  	  }
    return m;
  }
    template<typename TotalBasis, typename State, typename Lattice>
  void Act(int i, int j, TotalBasis& totalBasis, State& tpState, Lattice state, Mat& op, double var)
  {
    if(state[i]==state[j])
      {

      }
    else{
      Lattice temp=state;
      //  std::cout<< "i,jx "<< i << ", "<< j << std::endl;
       state.flip(i);
       //setPartNr(j, temp[i]);
      state.flip(j);

      size_t signControl=CheckSign(temp, i, j);

		    auto it2 = totalBasis.find(state.GetId());

		     size_t newStateNr= Position(*it2);
		     //	         std::cout<< "old "<< temp << " at "<< Position(tpState)<< std::endl;

		     // std::cout<< "new "<< state <<" at  "<<newStateNr<<  std::endl;
	   	              if(signControl%2==0)
   	   	    	 {
	   		   op.coeffRef(newStateNr, Position(tpState))-= ValType{var};}
   	   	       else
   	   	    	 { op.coeffRef(newStateNr, Position(tpState))+= ValType{var};}
		    }

  }

  // carefull with changes, making one for cdag and one for ccdag
    template<typename T>
    size_t CheckSign2(const T& state, size_t i, size_t j)
  {
    size_t m=0;
    if(i>j)
      {
  	for(size_t l=j; l<i; l++)
  	  {
  	    m+=size_t(state[l]);
  	  }

      }
    else
      	for(size_t l=i; l<j; l++)
  	  {
  	    m+=size_t(state[l]);
  	  }
    return m;
  }

  size_t NextWithBC(size_t i, size_t sites, bool PB=true, size_t steps=1)
  {
    if(PB)
      {return (i+steps)%sites;}
    else{return (i+steps);}
  }
  size_t Length( size_t sites, bool PB=true)
  {
    if(PB)
      {return sites;}
    else{return sites-1;}
  }
    template<class TotalBasis>
  Mat CdagOperator(const TotalBasis& totalBasis, int i,   const bool& PB=true)
{

   using BasisIt= typename TotalBasis::BasisIt;

 using Lattice=typename TotalBasis::Lattice;
    size_t dim=totalBasis.dim;

    Mat op(dim, dim);
       op.setZero();
 for( auto& tpState :totalBasis)
	     {

	       BasisIt it2=totalBasis.find(Id(tpState));

	       Lattice state=GetLattice(*it2);
	       //	       std::cout << " stat "<< totalBasis.totalmaxPar << "\n";
	       //std::cout << " i "<< i<< "  has "<< state[i]<< std::endl;

	       if(state[i]==0 )
	         	 {

			   auto temp=state;
	         	   state.setPartNr(i, 1);
			   size_t signControl=CheckSign2(temp, i, 0);
			   // std::cout << "at i="<< i <<" changed to  "<< state << "\n"<< "with aign cont "<< signControl<< '\n';
	         	   it2= totalBasis.find(state.GetId());

	     		   size_t newStateNr= Position(*it2);
	      		              if(signControl%2==0)
   	      	    	 {
	      		   op.coeffRef(newStateNr, Position(tpState))+= ValType{1};}
   	      	       else
   	      	    	 { op.coeffRef(newStateNr, Position(tpState))-= ValType{1};}
	      	    }

	      }
      return op;

       }
  template<class TotalBasis>
  Mat COperator(const TotalBasis& totalBasis, int i,   const bool& PB=true)
{

   using BasisIt= typename TotalBasis::BasisIt;

 using Lattice=typename TotalBasis::Lattice;
    size_t dim=totalBasis.dim;

    Mat op(dim, dim);
       op.setZero();
 for( auto& tpState :totalBasis)
	     {

	        BasisIt it2=totalBasis.find(Id(tpState));

	       Lattice state=GetLattice(*it2);
	       // std::cout << " stat "<< state << "\n";
	       //std::cout << " i "<< i<< "  has "<< state[i]<< std::endl;

	       if(state[i]==1)
	         	 {

			   auto temp=state;
	         	   state.setPartNr(i, 0);
			   size_t signControl=CheckSign2(temp, i, 0);
			   // std::cout << "at i="<< i <<" changed to  "<< state << "\n"<< "with aign cont "<< signControl<< '\n';
	         	   it2= totalBasis.find(state.GetId());

	     		   size_t newStateNr= Position(*it2);
	      		              if(signControl%2==0)
   	      	    	 {
	      		   op.coeffRef(newStateNr, Position(tpState))+= ValType{1};}
   	      	       else
   	      	    	 { op.coeffRef(newStateNr, Position(tpState))-= ValType{1};}
	      	    }

	      }
      return op;

       }
  template<class TotalBasis>
  Mat NumberOperatorE(const TotalBasis& totalBasis, const double omega=1., bool PB=0, int start=0, int stop=0)
 {

    size_t dim=totalBasis.dim;
    size_t sites=totalBasis.sites;
    Mat op(dim, dim);
      op.setZero();
 stop = (stop!=0) ? stop : sites;

      for(const auto& tpState : totalBasis)
	{

	  for(size_t i=start; i<stop; i++)
	    {

  op.coeffRef(Position(tpState), Position(tpState))+=ValType(omega*totalBasis.particlesAt(Id(tpState), i));

	  }
	}
      return op;

       }
  template<class TotalBasis, class Functor>
  Mat EKinLongRangeOperator(const TotalBasis& totalBasis,Functor f, int lmx=1, double var=1. , bool PB=0 )
     {

using Lattice=typename TotalBasis::Lattice;

   size_t dim=totalBasis.dim;
    size_t sites=totalBasis.sites;
    Mat op(dim, dim);
      op.setZero();
    int  stop = Operators::Length( sites, PB);

   for( auto& tpState : totalBasis)
	     {
	       	    for(int i=0; i<stop; i++)
                 {
		   for(int j=1; j<=lmx; j++)
                 {
Lattice state_1=GetLattice(tpState);
		   Lattice state_2=GetLattice(tpState);
		   int j_1=(i+j)%sites;

	   	    		    if(state_1[i]==state_1[j_1])
   	       	     {
	  //

   	   	     }
   	   	    else{

		      Lattice temp=state_1;
		      state_1.switchPartNr(i, temp[j_1], j_1, temp[i]);

  size_t signControl=CheckSign(temp, i, j_1);

		    auto it2_1 = totalBasis.find(state_1.GetId());
	     		   size_t newStateNr_1= Position(*it2_1);

	   	               if(signControl%2==0)
   	   	    	  {
			    op.coeffRef(newStateNr_1, Position(tpState))-= ValType{var}*f(i,j_1);}

		   	       	   	       else
		   	    {
			      op.coeffRef(newStateNr_1, Position(tpState))+= ValType{var}*f(i,j_1);
}

		    }

		 }

   	  }
	     }
  return op;
  }
  template<class TotalBasis>
  Mat EKinOperator(const TotalBasis& totalBasis, double var=1. , bool PB=0, int start=0, int stop=0 )
     {

using Lattice=typename TotalBasis::Lattice;

   size_t dim=totalBasis.dim;
    size_t sites=totalBasis.sites;
    Mat op(dim, dim);
      op.setZero();
      stop = (stop!=0) ? stop : Operators::Length( sites, PB);

   for( auto& tpState : totalBasis)
	     {

	       	    for(size_t i=start; i<stop; i++)
                 {

		   Lattice state=GetLattice(tpState);
                     size_t j=Operators::NextWithBC(i, sites, PB);
	   	    		    if(state[i]==state[j])
   	       	     {
	  //

   	   	     }
   	   	    else{

		      Lattice temp=state;

		       state.setPartNr(j, temp[i]);
		      state.setPartNr(i, temp[j]);

	  	     size_t signControl=CheckSign(temp, i, j);

		    auto it2 = totalBasis.find(state.GetId());

		     size_t newStateNr= Position(*it2);

	   	              if(signControl%2==0)
   	   	    	 {
	   		   op.coeffRef(newStateNr, Position(tpState))-= ValType{var};}
   	   	       else
   	   	    	 { op.coeffRef(newStateNr, Position(tpState))+= ValType{var};}
		    }

	   	     }

   	  }

  return op;
  }

Mat EKinOperator(const Many_Body::OneElectronBasis& totalBasis, double var=1. , bool PB=0, int start=0, int stop=0 )
     {

       using Lattice=typename Many_Body::OneElectronBasis::Lattice;

   size_t dim=totalBasis.dim;
    size_t sites=totalBasis.sites;
    Mat op(dim, dim);
      op.setZero();
      stop = (stop!=0) ? stop : Operators::Length( sites, PB);

   for( auto& tpState : totalBasis)
   	     {

   	       	    for(size_t i=start; i<stop; i++)
                 {

   		   Lattice state=GetLattice(tpState);
                     size_t j=Operators::NextWithBC(i, sites, PB);
   	   	    		    if(state[i]==state[j])
   	       	     {
   	  //

   	   	     }
   	   	    else{

   		    Act(i, j, totalBasis, tpState, state, op, var);
   		    }

   	   	     }

   	  }

  return op;
  }

   template<class TotalBasis>
  Mat CurrOperator(const TotalBasis& totalBasis, double var=1. , bool PB=0, int start=0, int stop=0)
     {

using Lattice=typename TotalBasis::Lattice;

   size_t dim=totalBasis.dim;
    size_t sites=totalBasis.sites;
    Mat op(dim, dim);
      op.setZero();
      stop = (stop!=0) ? stop : Operators::Length( sites, PB);

   for( auto& tpState : totalBasis)
	     {

	       	    for(size_t i=start; i<stop; i++)
                 {

		   Lattice state=GetLattice(tpState);
                     size_t j=Operators::NextWithBC(i, sites, PB);
	   	    		    if(state[i]==state[j])
   	       	     {
	  //

   	   	     }
   	   	    else{

		      Lattice temp=state;

		       state.setPartNr(j, temp[i]);
		      state.setPartNr(i, temp[j]);

	  	     size_t signControl=CheckSign(temp, i, j);

		    auto it2 = totalBasis.find(state.GetId());

		    double otherSign=(state[j]==1)?+1:-1;

		     size_t newStateNr= Position(*it2);

	   	              if(signControl%2==0)
   	   	    	 {
	   		   op.coeffRef(newStateNr, Position(tpState))-= otherSign*ValType{var};}
   	   	       else
   	   	    	 { op.coeffRef(newStateNr, Position(tpState))+= otherSign*ValType{var};}
		    }

	   	     }

   	  }

  return op;
  }

  Mat CurrOperator(const  Many_Body::OneElectronBasis& totalBasis, double var=1. , bool PB=0, int start=0, int stop=0)
     {

using Lattice=typename  Many_Body::OneElectronBasis::Lattice;

   size_t dim=totalBasis.dim;
    size_t sites=totalBasis.sites;
    Mat op(dim, dim);
      op.setZero();
      stop = (stop!=0) ? stop : Operators::Length( sites, PB);

   for( auto& tpState : totalBasis)
	     {

	       	    for(size_t i=start; i<stop; i++)
                 {

		   Lattice state=GetLattice(tpState);
                     size_t j=Operators::NextWithBC(i, sites, PB);
	   	    		    if(state[i]==state[j])
   	       	     {
	  //

   	   	     }
   	   	    else{

		   Lattice temp=state;

		          state.flip(i);

			  state.flip(j);
	  	     size_t signControl=CheckSign(temp, i, j);

		    auto it2 = totalBasis.find(state.GetId());

		    double otherSign=(temp[j]==1)?+1:-1;

		     size_t newStateNr= Position(*it2);

	   	              if(signControl%2==0)
   	   	    	 {
	   		   op.coeffRef(newStateNr, Position(tpState))-= otherSign*ValType{var};}
   	   	       else
   	   	    	 { op.coeffRef(newStateNr, Position(tpState))+= otherSign*ValType{var};}
		    }

	   	     }

   	  }

  return op;

  }

    template<class TotalBasis>
    Mat totalHetOperator(const TotalBasis& totalBasis, double tint, double t0, double tl, double V, int Llead1,  int Llead2,  int Lchain )
     {
       bool PB=0;

using Lattice=typename TotalBasis::Lattice;

   size_t dim=totalBasis.dim;
    size_t sites=totalBasis.sites;
    Mat op(dim, dim);
      op.setZero();
 int L_x=Llead1+(Lchain+1)/2;
 double E=0;
 if(Lchain>1)
   {

   E=V/(Lchain+1);

   }
 std::cout<< "L x "<<L_x<< " E "<<E<<std::endl;
   for( auto& tpState : totalBasis)
	     {
const Lattice state=GetLattice(tpState);
	       	    for(size_t i=0; i<Llead1-1; i++)
                 {

		   if(state[i]==1)
		     {

	       op.coeffRef(Position(tpState), Position(tpState))-= ValType{V/2};

		     }
		   size_t j=Operators::NextWithBC(i, sites, PB);
		   //	   std::cout<< "i,j "<< i << ", "<< j << std::endl;
		     // first lead

		      Act(i, j, totalBasis, tpState, state, op, tl);

		 }
		     for(size_t i=Llead1+Lchain; i<Llead1+Lchain+Llead2-1; i++)
                 {
		   if(state[i]==1)
		     {
		       // std::cout<<" i "<<i+1<< " V "<< V <<std::endl;

		       op.coeffRef(Position(tpState), Position(tpState))+= ValType{V/2};

		     }

				   size_t j=Operators::NextWithBC(i, sites, PB);
		     // second lead

		     Act(i, j, totalBasis, tpState, state, op, tl);

	   	     }
		    	   if(state[Llead1+Llead2+Lchain-1]==1)
		     {

		       op.coeffRef(Position(tpState), Position(tpState))+= ValType{V/2};

		     }
			   if(state[Llead1-1]==1)
		     {

		       op.coeffRef(Position(tpState), Position(tpState))-= ValType{V/2};

		     }
		    // chain
		    	       	    for(size_t i=Llead1; i<Llead1+Lchain-1; i++)
                 {
		   Lattice state=GetLattice(tpState);

                     size_t j=Operators::NextWithBC(i, sites, PB);
		     // first lead
		     Act(i, j, totalBasis, tpState, state, op, t0);
		     	   if(state[i]==1)
		     {

		       		         op.coeffRef(Position(tpState), Position(tpState))+= (static_cast<double>((i+1))-L_x)*E;

		     }
	   	     }
				      	   if(state[Llead1+Lchain-1]==1)
		     {

		       	       op.coeffRef(Position(tpState), Position(tpState))+=(static_cast<double>(Llead1+Lchain)-L_x)*E;
	     }
				    Act(Llead1-1, Llead1, totalBasis, tpState, state, op, tint);
				    Act(Llead1+Lchain-1, Llead1+Lchain, totalBasis, tpState, state, op, tint);	  }

  return op;
  }
template<class TotalBasis>
Mat totCurrOperator(const TotalBasis& totalBasis, double var ,  int Llead1, int Llead2, int Lchain)
     {

       using Lattice=typename TotalBasis::Lattice;

   size_t dim=totalBasis.dim;
   size_t sites=totalBasis.sites;
    Mat op(dim, dim);
      op.setZero();
   for( auto& tpState : totalBasis)
	     {

	       int i=Llead1-1;

	       int j=i+1;

		   Lattice state=GetLattice(tpState);

	   	    		    if(state[i]==state[j])
   	       	     {

   	   	     }
   	   	    else{

		      Lattice temp=state;

		          state.flip(i);

			  state.flip(j);
	  	     size_t signControl=CheckSign(temp, i, j);

		    auto it2 = totalBasis.find(state.GetId());

		    double otherSign=(temp[j]==1)?+1:-1;

		     size_t newStateNr= Position(*it2);

	   	              if(signControl%2==0)
   	   	    	 {
	   		   op.coeffRef(newStateNr, Position(tpState))-= otherSign*ValType{var};}
   	   	       else
   	   	    	 { op.coeffRef(newStateNr, Position(tpState))+= otherSign*ValType{var};}
		    }
				    state=GetLattice(tpState);

	 i=Lchain+Llead1-1;

    j=i+1;

		if(state[i]==state[j])
   	       	     {
	  //

   	   	     }
   	   	    else{

		      Lattice temp=state;
		    state.flip(i);
       //setPartNr(j, temp[i]);
      state.flip(j);

	  	     size_t signControl=CheckSign(temp, i, j);

		    auto it2 = totalBasis.find(state.GetId());

		    double otherSign=(temp[j]==1)?+1:-1;

		     size_t newStateNr= Position(*it2);

	   	              if(signControl%2==0)
   	   	    	 {
	   		   op.coeffRef(newStateNr, Position(tpState))-= otherSign*ValType{var};}
   	   	       else
   	   	    	 { op.coeffRef(newStateNr, Position(tpState))+= otherSign*ValType{var};}
		    }

   	  }

   op*=0.5;

  return op;
  }
}


/**
 * \ file fmath.cpp
 */

#include <cmath>

#include <ATK/Utility/fmath.h>

#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_NO_MAIN
#include <boost/test/unit_test.hpp>

BOOST_AUTO_TEST_CASE(fmath_exp_float_test)
{
  for (int i = 0; i < 10; ++i)
  {
    BOOST_REQUIRE_CLOSE(std::exp(float(i)), fmath::exp(float(i)), 0.0001);
  }
}

BOOST_AUTO_TEST_CASE(fmath_exp_double_test)
{
  for (int i = 0; i < 100; ++i)
  {
    BOOST_REQUIRE_CLOSE(std::exp(double(i)), fmath::exp(double(i)), 0.0001);
  }
}

BOOST_AUTO_TEST_CASE(fmath_log_float_test)
{
  for (int i = 1; i < 100; ++i)
  {
    BOOST_REQUIRE_CLOSE(std::log(float(i)), fmath::log(float(i)), 0.0001);
  }
}

BOOST_AUTO_TEST_CASE(fmath_log_double_test)
{
  for (int i = 1; i < 100; ++i)
  {
    BOOST_REQUIRE_CLOSE(std::log(double(i)), fmath::log(double(i)), 0.0001);
  }
}

BOOST_AUTO_TEST_CASE(fmath_log10_float_test)
{
  for (int i = 1; i < 100; ++i)
  {
    BOOST_REQUIRE_CLOSE(std::log10(float(i)), fmath::log10(float(i)), 0.0001);
  }
}

BOOST_AUTO_TEST_CASE(fmath_log10_double_test)
{
  for (int i = 1; i < 100; ++i)
  {
    BOOST_REQUIRE_CLOSE(std::log10(double(i)), fmath::log10(double(i)), 0.0001);
  }
}

BOOST_AUTO_TEST_CASE(fmath_pow_float_test)
{
  for (int i = 1; i < 10; ++i)
  {
    BOOST_REQUIRE_CLOSE(std::pow(10, float(i)), fmath::pow(10, float(i)), 0.001);
  }
}

BOOST_AUTO_TEST_CASE(fmath_pow_double_test)
{
  for (int i = 1; i < 100; ++i)
  {
    BOOST_REQUIRE_CLOSE(std::pow(10, double(i)), fmath::pow(10, double(i)), 0.0001);
  }
}


/**
 * File: testing.cpp
 * Date: Apr 16 2021
 * Author: Open Risk  (www.openriskmanagement.com)
 *
 */

#include "stats.hpp"
#include <armadillo>
#include "random_var.h"

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

    int SampleSize = 100;
    int DataType = 1;
    RandomVar myR(SampleSize, DataType);

    cout << "Testing" << endl;
    for (int i = 0; i < myR.size(); i++) {
        double rval = stats::rt(30);
        myR.setS(i, rval);
    }

    myR.Print();

}


#include <boost/geometry/formulas/thomas_inverse.hpp>


/*
    BSD 3-Clause License

    Copyright (c) 2018, Roboy
            All rights reserved.

    Redistribution and use in source and binary forms, with or without
            modification, are permitted provided that the following conditions are met:

    * Redistributions of source code must retain the above copyright notice, this
    list of conditions and the following disclaimer.

    * Redistributions in binary form must reproduce the above copyright notice,
    this list of conditions and the following disclaimer in the documentation
    and/or other materials provided with the distribution.

    * Neither the name of the copyright holder nor the names of its
    contributors may be used to endorse or promote products derived from
    this software without specific prior written permission.

    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
    AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
            IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
    DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
            FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
            DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
            SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
            CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
    OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
    OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

    author: Simon Trendel ( simon.trendel@tum.de ), 2018
    description: helper class for handling triangulation from lighthouse angles
*/

#pragma once

// Eigen
#include <Eigen/Core>
#include <Eigen/Dense>
#include "darkroom/Sensor.hpp"

struct LighthouseCalibration{
    double phase = 0;
    double curve = 0;
    double tilt = 0;
    double gibmag = 0;
    double gibphase = 0;
    void reset(){ phase = 0; curve = 0; tilt = 0; gibmag = 0; gibphase = 0; }
};

// offset from center of laser rotation
#define AXIS_OFFSET 0.015

using namespace Eigen;

class Triangulation{
public:
    /**
   * get the 3D minimum distance between 2 lines
   * following http://geomalgorithms.com/a07-_distance.html#Distance-between-Lines
   * @param pos0 origin line0
   * @param dir1 direction line0
   * @param pos1 origin line1
   * @param dir2 direction line1
   * @param tri0 point on line0 closest to line1
   * @param tri1 point on line1 closest to line0
   * @return distance between the lines
   */
    double dist3D_Line_to_Line( Vector3d &pos0, Vector3d &dir1,
                                Vector3d &pos1, Vector3d &dir2,
                                Vector3d &tri0, Vector3d &tri1);

/**
    * This function triangulates the position of a sensor using the horizontal and vertical angles from two ligthouses
    * @param angles0 vertical/horizontal angles form first lighthouse
    * @param angles1 vertical/horizontal angles form second lighthouse
    * @param RT_0 pose matrix of first lighthouse
    * @param RT_1 pose matrix of second lighthouses
    * @param triangulated_position the triangulated position
    * @param ray0 ligthhouse ray
    * @param ray1 ligthhouse ray
    */
    double triangulateFromLighthouseAngles(Vector2d &angles0, Vector2d &angles1, Matrix4d &RT_0, Matrix4d &RT_1,
                                           Vector3d &triangulated_position, Vector3d &ray0, Vector3d &ray1);

    double triangulateFromRays(Vector3d &ray0, Vector3d &ray1, Matrix4d &RT_0, Matrix4d &RT_1, Vector3d &triangulated_position);

    void rayFromLighthouseAngles(Vector2d &angles, Vector3d &ray, int lighthouse);
    void rayFromLighthouseAngles(double elevation, double azimuth, Vector3d &ray, int lighthouse);

    // for each motor and each lighthouse
    LighthouseCalibration calibration[2][2];
};


#include <algorithm>

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

#include "StakeStats.h"
#include "bignum.h"
#include "types.h"

StakeStats::StakeStats(void)
{
    m_ProbabilitiesNotToMint = 1;
}

void StakeStats::PushChancesToMint(CBigNum bnChancesToMint)
{
    CBigNum bnMaxValue = CBigNum(~uint256(0));

    if (bnChancesToMint > bnMaxValue)
        bnChancesToMint = bnMaxValue;

    uint64_t precision = 1000000000000000000;

    CBigNum bnExtendedChancesToMint = bnChancesToMint * precision;
    CBigNum bnExtendedProbabilitiesToMint = bnExtendedChancesToMint / bnMaxValue;

    double probabilitiesToMint = static_cast<double>(bnExtendedProbabilitiesToMint.getuint64()) / precision;
    double probabilitiesNotToMint = 1 - probabilitiesToMint;

    m_ProbabilitiesNotToMint *= probabilitiesNotToMint;

    /* Just add a '/' before the '/*' to uncomment this whole block
    //
    printf("---\n");
    printf("bnChancesToMint               : %-100s\n", bnChancesToMint.ToString().c_str());
    printf("bnMaxValue                    : %-100s\n", bnMaxValue.ToString().c_str());
    printf("bnExtendedChancesToMint       : %-100s\n", bnExtendedChancesToMint.ToString().c_str());
    printf("bnExtendedProbabilitiesToMint : %-100s\n", bnExtendedProbabilitiesToMint.ToString().c_str());
    printf("extendedProbabilitiesToMint   : %.17g\n", static_cast<double>(bnExtendedProbabilitiesToMint.getuint64()));
    printf("probabilitiesToMint           : %.17g\n", probabilitiesToMint);
    printf("probabilitiesNotToMint        : %.17g\n", probabilitiesNotToMint);
    printf("---\n");
    //*/
}

timestamp_t StakeStats::GetEstimatedStakeTime(double expectedChancesToMint)
{
    double expectedChancesNotToMint = std::max(0., 1. - expectedChancesToMint);
    double expectedEventCount = log(expectedChancesNotToMint) / log(m_ProbabilitiesNotToMint);

    if (boost::math::isinf(expectedEventCount))
        return -1;

    return static_cast<timestamp_t>(expectedEventCount);
}


#include "drake/solvers/fbstab/components/dense_linear_solver.h"

#include <cmath>

#include <Eigen/Dense>

#include "drake/solvers/fbstab/components/dense_data.h"
#include "drake/solvers/fbstab/components/dense_residual.h"
#include "drake/solvers/fbstab/components/dense_variable.h"

namespace drake {
namespace solvers {
namespace fbstab {
namespace {
// Solves the system A*A' x = b in place
// where A is lower triangular and invertible.
void CholeskySolve(const Eigen::MatrixXd& A, Eigen::VectorXd* b) {
  A.triangularView<Eigen::Lower>().solveInPlace(*b);
  A.triangularView<Eigen::Lower>().transpose().solveInPlace(*b);
}
}  //  namespace

DenseLinearSolver::DenseLinearSolver(int nz, int nv) {
  if (nz <= 0 || nv <= 0) {
    throw std::runtime_error(
        "In DenseLinearSolver::DenseLinearSolver: inputs must be positive.");
  }
  nz_ = nz;
  nv_ = nv;

  K_.resize(nz_, nz_);
  r1_.resize(nz_);
  r2_.resize(nv_);
  Gamma_.resize(nv_);
  mus_.resize(nv_);
  gamma_.resize(nv_);
  B_.resize(nv_, nz_);
}

void DenseLinearSolver::SetAlpha(double alpha) { alpha_ = alpha; }

bool DenseLinearSolver::Initialize(const DenseVariable& x,
                                   const DenseVariable& xbar, double sigma) {
  const DenseData* const data = x.data();
  if (xbar.data() != data) {
    throw std::runtime_error(
        "In DenseLinearSolver::Factor: x and xbar have mismatched problem "
        "data.");
  }
  if (xbar.nz_ != x.nz_ || xbar.nv_ != x.nv_) {
    throw std::runtime_error(
        "In DenseLinearSolver::Factor: inputs must be the same size");
  }
  if (xbar.nz_ != nz_ || xbar.nv_ != nv_) {
    throw std::runtime_error(
        "In DenseLinearSolver::Factor: inputs must match object size.");
  }
  if (sigma <= 0) {
    throw std::runtime_error(
        "In DenseLinearSolver::Factor: sigma must be positive.");
  }
  const Eigen::MatrixXd& H = data->H();
  const Eigen::MatrixXd& A = data->A();

  K_ = H + sigma * Eigen::MatrixXd::Identity(nz_, nz_);

  // K <- K + A'*diag(Gamma(x))*A
  Eigen::Vector2d pfb_gradient;
  for (int i = 0; i < nv_; i++) {
    const double ys = x.y()(i) + sigma * (x.v()(i) - xbar.v()(i));
    pfb_gradient = PFBGradient(ys, x.v()(i));
    gamma_(i) = pfb_gradient(0);
    mus_(i) = pfb_gradient(1) + sigma * pfb_gradient(0);
    Gamma_(i) = gamma_(i) / mus_(i);
  }
  // B is used to avoid temporaries
  B_.noalias() = Gamma_.asDiagonal() * A;
  K_.noalias() += A.transpose() * B_;

  // Factor K = LL' in place
  Eigen::LLT<Eigen::Ref<Eigen::MatrixXd> > L(K_);

  Eigen::ComputationInfo status = L.info();
  if (status != Eigen::Success) {
    return false;
  } else {
    return true;
  }
}

bool DenseLinearSolver::Solve(const DenseResidual& r, DenseVariable* x) const {
  if (x == nullptr) {
    throw std::runtime_error("In DenseLinearSolver::Solve: x cannot be null.");
  }
  if (r.nz_ != x->nz_ || r.nv_ != x->nv_) {
    throw std::runtime_error(
        "In DenseLinearSolver::Solve residual and variable objects must be the "
        "same size");
  }
  if (x->nz_ != nz_ || x->nv_ != nv_) {
    throw std::runtime_error(
        "In DenseLinearSolver::Factor: inputs must match object size.");
  }
  const DenseData* const data = x->data();
  const Eigen::MatrixXd& A = data->A();
  const Eigen::VectorXd& b = data->b();

  // This method solves the system:
  // KK'z = rz - A'*diag(1/mus)*rv
  // diag(mus) v = rv + diag(gamma)*A*z
  // Where K has been precomputed by the factor routine.
  // See (28) and (29) in https://arxiv.org/pdf/1901.04046.pdf

  // Compute rz - A'*(rv./mus) and store it in r1_.
  r2_ = r.v_.cwiseQuotient(mus_);
  r1_.noalias() = r.z_ - A.transpose() * r2_;

  // Solve KK'*z = rz - A'*(rv./mus)
  // where K = chol(H + sigma*I + A'*Gamma*A)
  // is assumed to have been computed during the factor phase.
  x->z() = r1_;
  CholeskySolve(K_, x->z_);

  // Compute v = diag(1/mus) * (rv + diag(gamma)*A*z)
  // written so as to avoid temporary creation
  r2_.noalias() = A * x->z();
  r2_.noalias() = gamma_.asDiagonal() * r2_;
  r2_.noalias() += r.v_;

  // v = r2./mus
  x->v() = r2_.cwiseQuotient(mus_);

  // y = b - Az
  x->y() = b - A * x->z();

  return true;
}

Eigen::Vector2d DenseLinearSolver::PFBGradient(double a, double b) const {
  const double r = sqrt(a * a + b * b);
  const double d = 1.0 / sqrt(2.0);

  Eigen::Vector2d v;
  if (r < zero_tolerance_) {
    v(0) = alpha_ * (1.0 - d);
    v(1) = alpha_ * (1.0 - d);

  } else if ((a > 0) && (b > 0)) {
    v(0) = alpha_ * (1.0 - a / r) + (1.0 - alpha_) * b;
    v(1) = alpha_ * (1.0 - b / r) + (1.0 - alpha_) * a;

  } else {
    v(0) = alpha_ * (1.0 - a / r);
    v(1) = alpha_ * (1.0 - b / r);
  }

  return v;
}

}  // namespace fbstab
}  // namespace solvers
}  // namespace drake


#include <iostream>
#include <opencv2/core/core.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include <opencv2/video/tracking.hpp>
#include <opencv2/opencv.hpp>

#include <Eigen/Core>
#include <Eigen/Geometry>
#include <g2o/core/base_vertex.h>
#include <g2o/core/base_unary_edge.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/optimization_algorithm_levenberg.h>
#include <g2o/solvers/csparse/linear_solver_csparse.h>
#include <g2o/types/sba/types_six_dof_expmap.h>
#include <chrono>

using namespace std;
using namespace cv;

void find_feature_matches (
    const Mat& img_1, const Mat& img_2,
    std::vector<KeyPoint>& keypoints_1,
    std::vector<KeyPoint>& keypoints_2,
    std::vector< DMatch >& matches );

Point2d pixel2cam ( const Point2d& p, const Mat& K )
{
    return Point2d
            (
                    ( p.x - K.at<double> ( 0,2 ) ) / K.at<double> ( 0,0 ),
                    ( p.y - K.at<double> ( 1,2 ) ) / K.at<double> ( 1,1 )
            );
}

int main ( int argc, char** argv )
{
    if ( argc != 5 )
    {
        cout<<" usage: pose_estimation_3d2d img1 img2 depth1 depth2"<<endl;
        return 1;
    }
    //-- 读取图像
    Mat img_1 = imread ( argv[1], CV_LOAD_IMAGE_COLOR );
    Mat img_2 = imread ( argv[2], CV_LOAD_IMAGE_COLOR );

    vector<KeyPoint> keypoints_1, keypoints_2;
    vector<DMatch> matches;
    find_feature_matches ( img_1, img_2, keypoints_1, keypoints_2, matches );

    cout<<"一共找到了"<<matches.size() <<"组匹配点"<<endl;

    // 建立3D点
    Mat d1 = imread ( argv[3], CV_LOAD_IMAGE_UNCHANGED );       // 深度图为16位无符号数，单通道图像
    Mat K = ( Mat_<double> ( 3,3 ) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1 );
    vector<Point3f> pts_3d;
    vector<Point2f> pts_2d;
    for ( DMatch m:matches )
    {
        //计算3D坐标
        ushort d = d1.ptr<unsigned short> (int ( keypoints_1[m.queryIdx].pt.y )) [ int ( keypoints_1[m.queryIdx].pt.x ) ];

        if ( d == 0 )   // bad depth
            continue;
        float dd = d/5000.0;
        //调用计数坐标函数
        Point2d p1 = pixel2cam ( keypoints_1[m.queryIdx].pt, K );

        pts_3d.push_back ( Point3f ( p1.x*dd, p1.y*dd, dd ) );
        pts_2d.push_back ( keypoints_2[m.trainIdx].pt );
    }

    cout<<"3d-2d pairs: "<<pts_3d.size() <<endl;

    Mat r, t;

    solvePnP ( pts_3d, pts_2d, K, Mat(), r, t, false ); // 调用OpenCV 的 PnP 求解，可选择EPNP，DLS等方法
    //solvePnPRansac ( pts_3d, pts_2d, K, Mat(), r, t, false );
    Mat R;
    //旋转向量到旋转矩阵
    cv::Rodrigues ( r, R ); // r为旋转向量形式，用Rodrigues公式转换为矩阵

    cout<<"R="<<endl<<R<<endl;
    cout<<"t="<<endl<<t<<endl;

    vector<cv::Point2f> next_keypoints;
    vector<cv::Point3f> prev_keypoints_3d;
    vector<cv::Point2f> prev_keypoints;
   // list< cv::Point2f > keypoints;      // 因为要删除跟踪失败的点，使用list
    vector<cv::KeyPoint> kps;
    cv::Ptr<cv::FastFeatureDetector> detector = cv::FastFeatureDetector::create();
    detector->detect( img_1, kps );
    for ( auto kp:kps )
    {
        prev_keypoints.push_back( kp.pt ); //将坐标放入keypoints链表中
    }

//    for ( auto kp:keypoints )
//    {
//        prev_keypoints.push_back(kp); //prev_keypoints 赋值为keypoints
//    }
    vector<unsigned char> status;
    vector<float> error;

    chrono::steady_clock::time_point t1 = chrono::steady_clock::now();

    cv::calcOpticalFlowPyrLK( img_1, img_2, prev_keypoints, next_keypoints, status, error );

    chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
    chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>>( t2-t1 );
    cout<<"LK Flow use time："<<time_used.count()<<" seconds."<<endl;

    cout<<"prev:"<<prev_keypoints.size() <<"  next:"<<next_keypoints.size()<<endl;

    vector<cv::KeyPoint> n_Keypoints;
    KeyPoint::convert(next_keypoints, n_Keypoints, 1, 1, 0, -1);

    Ptr<DescriptorMatcher> matcher  = DescriptorMatcher::create ( "BruteForce-Hamming" );
    vector<DMatch> match;
    // BFMatcher matcher ( NORM_HAMMING );
    matcher->match ( kps, n_Keypoints, match );

    // 把跟丢的点删掉，keypoint 放当前帧

//    int i=0;
//    for ( auto iter=keypoints.begin(); iter!=keypoints.end(); i++)
//    {
//        if ( status[i] == 0 ) //表示状态
//        {
//            iter = keypoints.erase(iter);
//            continue;
//        }
//        *iter = next_keypoints[i];
//        iter++;
//    }
//    for ( Point2f m:prev_keypoints )
//    {
//        //计算3D坐标
//        ushort d = d1.ptr<unsigned short> (int (m.y)) [ int (m.x) ];
//        if ( d == 0 )   // bad depth
//            continue;
//        float dd = d/5000.0;
//        //调用计数坐标函数
//        prev_keypoints_3d.push_back ( Point3f ( m.x*dd, m.y*dd, dd ) );
//    }
//
//
//    Mat r2, t22;
//
//    solvePnP ( prev_keypoints_3d, next_keypoints, K, Mat(), r2, t22, false ); // 调用OpenCV 的 PnP 求解，可选择EPNP，DLS等方法
//    //solvePnPRansac ( pts_3d, pts_2d, K, Mat(), r, t, false );
//    Mat R2;
//    //旋转向量到旋转矩阵
//    cv::Rodrigues ( r2, R2 ); // r为旋转向量形式，用Rodrigues公式转换为矩阵
//
//    cout<<"R="<<endl<<R2<<endl;
//    cout<<"t="<<endl<<t22<<endl;
    return 0;

}

void find_feature_matches ( const Mat& img_1, const Mat& img_2,
                            std::vector<KeyPoint>& keypoints_1,
                            std::vector<KeyPoint>& keypoints_2,
                            std::vector< DMatch >& matches )
{
    //-- 初始化
    Mat descriptors_1, descriptors_2;
    // used in OpenCV3
    Ptr<FeatureDetector> detector = ORB::create();
    Ptr<DescriptorExtractor> descriptor = ORB::create();
    // use this if you are in OpenCV2
    // Ptr<FeatureDetector> detector = FeatureDetector::create ( "ORB" );
    // Ptr<DescriptorExtractor> descriptor = DescriptorExtractor::create ( "ORB" );
    Ptr<DescriptorMatcher> matcher  = DescriptorMatcher::create ( "BruteForce-Hamming" );
    //-- 第一步:检测 Oriented FAST 角点位置
    detector->detect ( img_1,keypoints_1 );
    detector->detect ( img_2,keypoints_2 );

    //-- 第二步:根据角点位置计算 BRIEF 描述子
    descriptor->compute ( img_1, keypoints_1, descriptors_1 );
    descriptor->compute ( img_2, keypoints_2, descriptors_2 );

    //-- 第三步:对两幅图像中的BRIEF描述子进行匹配，使用 Hamming 距离
    vector<DMatch> match;
    // BFMatcher matcher ( NORM_HAMMING );
    matcher->match ( descriptors_1, descriptors_2, match );

    //-- 第四步:匹配点对筛选
    double min_dist=10000, max_dist=0;

    //找出所有匹配之间的最小距离和最大距离, 即是最相似的和最不相似的两组点之间的距离
    for ( int i = 0; i < descriptors_1.rows; i++ )
    {
        double dist = match[i].distance;
        if ( dist < min_dist ) min_dist = dist;
        if ( dist > max_dist ) max_dist = dist;
    }

    printf ( "-- Max dist : %f \n", max_dist );
    printf ( "-- Min dist : %f \n", min_dist );

    //当描述子之间的距离大于两倍的最小距离时,即认为匹配有误.但有时候最小距离会非常小,设置一个经验值30作为下限.
    for ( int i = 0; i < descriptors_1.rows; i++ )
    {
        if ( match[i].distance <= max ( 2*min_dist, 30.0 ) )
        {
            matches.push_back ( match[i] );
        }
    }
}


#include <boost/test/unit_test.hpp>
#include "ray.h"
#include "aabb.h"

BOOST_AUTO_TEST_SUITE(aabb)

BOOST_AUTO_TEST_CASE(trace_through_corners)
{
	math::aabb<3> b(math::vec<3>(0, 0, 0), 20.0f);
	math::ray<3> r(math::vec<3>(-20, -20, -20), math::vec<3>(40, 40, 40));

	math::scalar t0, t1;

	b.trace(r, &t0, &t1);

	BOOST_REQUIRE (math::abs(t0 - 0.25f) < math::EPSILON);
	BOOST_REQUIRE (math::abs(t1 - 0.75f) < math::EPSILON);
}

BOOST_AUTO_TEST_SUITE_END()


#include <vector>
#include <tuple>
#include <iostream>
#include <fstream>
#include <cmath>
#include <string>
#include <random>
#include <nlopt.hpp>
#include <Eigen/Dense>
#include <boost/math/special_functions/binomial.hpp>

#define MAXBUFSIZE  ((int) 1e6)

unsigned int function_calls =0;
unsigned long int mcmc_steps = 10000;

std::vector< std::pair<int,std::vector<double> > > read_prior(const char*
	filename)
	{
		std::ifstream infile;
		infile.open(filename);

		std::vector< std::pair<int,std::vector<double> > > acc;

		while(!infile.eof())
		{
			std::string line;
			std::getline(infile,line);

			std::stringstream stream(line);
			int size_household;
			stream >> size_household;

			std::vector<double> priors;
			double probab;
			while(stream >> probab)
			{
				priors.push_back(probab);
			}
			acc.push_back(std::make_pair(size_household,priors));
		}
		return acc;
	}

int find_maximum(const std::vector< std::pair<int,
	std::vector<double> > > &priors)
{
	int curr_max = 0;
	for(auto household : priors)
	{
		int temp = household.first;
		if(temp > curr_max) curr_max = temp;
	}
	return curr_max;
}

Eigen::MatrixXd proposal(const std::vector< std::pair<int,
	std::vector<double> > > &priors,int max_n,std::mt19937 &rng)
	{
		Eigen::MatrixXd final_sizes = Eigen::MatrixXd::Zero(max_n+1,max_n);

		for(auto household : priors)
		{
			int number_sick=0;

			for(double ind_prob : household.second) // We go through all members
				// of the household and assign them an individual status
			{
				std::bernoulli_distribution sick(ind_prob);
				if(sick(rng))
				{
					number_sick++;
				}
			}

		final_sizes(number_sick,household.first-1)++;
		}
		return final_sizes;
	}

Eigen::MatrixXd readMatrix(const char *filename)
    {
    int cols = 0, rows = 0;
    double buff[MAXBUFSIZE];

    // Read numbers from file into buffer.
    std::ifstream infile;
    infile.open(filename);
    while (! infile.eof())
        {
        std::string line;
        std::getline(infile, line);

        int temp_cols = 0;
        std::stringstream stream(line);
        while(! stream.eof())
            stream >> buff[cols*rows+temp_cols++];

        if (temp_cols == 0)
            continue;

        if (cols == 0)
            cols = temp_cols;

        rows++;
        }

    infile.close();

    // rows--;

    // Populate matrix with numbers.
    Eigen::MatrixXd result(rows,cols);
    for (int i = 0; i < rows; i++)
        for (int j = 0; j < cols; j++)
            result(i,j) = buff[ cols*i+j ];

    return result;
    }

double Phi(const double &x, const double &alpha, const double &beta, const
	double &k, const int n)
{
	double mean = beta/pow(n,alpha);
	return pow(k/(k+x*mean),k);
}

/* Returns a (n+1)*(n+1) matrix such that F(i,j) = F_i^{n,j}. Columns correspond
to different values of s0, rows are the m values. As such, F is upper
triangular, and F(i,j) for i>j is undefined. */

std::pair<Eigen::MatrixXd,Eigen::MatrixXd> solve_triangular(const
	std::vector<double> &v, const int n)
	{
		Eigen::MatrixXd result1(n+1,n+1);
		Eigen::MatrixXd result2(n+1,n+1);
		for (int s0=0; s0 <= n; ++s0)
		{
			Eigen::MatrixXd A1(s0+1,s0+1);
			Eigen::MatrixXd A2(s0+1,s0+1);
			Eigen::VectorXd B(s0+1);
			for(int i=0; i<=s0; ++i)
			{
				B(i) = boost::math::binomial_coefficient<double>(s0,i);
				for (int j = 0; j <= i; ++j)
				{
					A1(i,j) = boost::math::binomial_coefficient<double>
					(s0-j,i-j)
					/(pow(Phi(s0-i,v[6],v[5],v[4],n),j)*(pow(v[0],s0-i)));
					A2(i,j) = boost::math::binomial_coefficient<double>
					(s0-j,i-j)
					/(pow(Phi(s0-i,v[6],v[5],v[4],n),j)*(pow(v[3],s0-i)));
				}
			}
			Eigen::VectorXd sol1 = A1.triangularView<Eigen::Lower>().solve(B);
			sol1.conservativeResize(n+1);
			result1.col(s0)=sol1;
			Eigen::VectorXd sol2 = A2.triangularView<Eigen::Lower>().solve(B);
			sol2.conservativeResize(n+1);
			result2.col(s0)=sol2;
		}

		for (int i = 0; i <= n; ++i)
		{
			for (int j = 0; j < i; ++j)
			{
				result1(i,j)=1;
				result2(i,j)=0;
			}
		}
 		return std::pair<Eigen::MatrixXd,Eigen::MatrixXd> {result1, result2};
	}

/* Returns the value of T_{(m,n)}, the expected frequency of households of size
n with m infected individuals. Arguments are a tuple (n,m), the running
parameter vector and the precomputed matrix containing the F_m^{n,s0}. */
double expected_prob(const std::tuple<int,int> &sizes, const
	std::vector<double> &parameters, const
	std::pair<Eigen::MatrixXd,Eigen::MatrixXd> &distributions)
	{
	const double n = std::get<0>(sizes); // n \ge 1
	const double m = std::get<1>(sizes); // 0 \le m \le s0

	double pasx = parameters[1];
	double ppr = parameters[2];

	Eigen::MatrixXd dis_current = distributions.first;
	Eigen::MatrixXd dis_prior = distributions.second;

	double sum=0.;

	for(int t=0; t<=(n-m); ++t)
	{
		for(int r=0; r<=(n-m-t); ++r)
		{
			for (int l = 0; l<= (n-m-r-t); ++l)
			{
				double prob1 = boost::math::binomial_coefficient<double>
				(m+t,t)*pow(pasx,t)*pow(1-pasx,m);
				double prob2 = boost::math::binomial_coefficient<double>
				(n,r)*pow(ppr,r)*pow(1-ppr,n-r);
				if(m+t>n-r-l) std::cout << "n=" << n << " m=" << m << " t=" << t
					<< " r=" << r << " l=" << l << std::endl;
				double expected_prob1 = dis_current(m+t,n-r-l);
				double expected_prob2 = dis_prior(l,n-r);
				sum += prob1*prob2*expected_prob1*expected_prob2;
			}
		}
	}
	return sum;
}

double Dev(const std::vector<double> &v, std::vector<double> &grad, void*
	my_func_data)
	{
		/* Contains the observed final sizes k_{(m,n)}. Be careful :
		households(i,j) contains k_{(i,j+1)}. Also, households(i,j)=0
		if i>j. */
		const Eigen::MatrixXd *households =
			static_cast<Eigen::MatrixXd *>(my_func_data);

		int max_n = (*households).cols();

		std::vector< std::pair<Eigen::MatrixXd,Eigen::MatrixXd > >
		distribution_matrices;

		/*	Computes the expected final size distributions F(n; s0,m) for 1
		\le n \le n_max and m\le s0 \le n. Output is a vector whose (n+1)-th
		element contains the matrix of the F(n;s0,m) for a fixed n. */

		for (int n = 1; n <= max_n; ++n)
		{
			std::pair<Eigen::MatrixXd,Eigen::MatrixXd> temp = solve_triangular
			(v,n);
			distribution_matrices.push_back(temp);
		}

		double sum = 0.;

		auto colSums = (*households).colwise().sum();

		for (int n = 1; n <= max_n; ++n)
		{
			double col_sum = colSums(n-1);
			for(int m=0; m<=n; ++m)
			{
				double observed = (*households)(m,n-1);
				if(observed==0.) continue;
				std::tuple<int,int> obs_tuple {n,m};
				double expected = expected_prob(obs_tuple, v,
					distribution_matrices[n-1]);
				sum += observed*(std::log10(observed/col_sum)-
					std::log10(expected));
			}
		}
		return sum;
	}

int main(int argc, char const *argv[])
{
	const auto priors = read_prior("test_prior.txt");

	const int max_n = find_maximum(priors);

	std::vector<Eigen::MatrixXd> households; /* Contiendra les valeurs de
	tailles finales */

	nlopt::opt opt(nlopt::LN_BOBYQA, 7);

	void* f_data;

	opt.set_min_objective(Dev,f_data); // Définit Dev comme fonction à minimiser

	/* Les paramètres sont dans l'ordre (cf Fraser et al. 2011) :

	- La proba d'échappement Q
	- La proba d'infection asymptomatique et infectieuse p_asx
	- La proba d'infection asymptomatique et non-infectieuse p_r
	- La proba d'échappement à une infection antérieure Q_prior
	- Hétérogénéité des infectivités k
	- Facteur de taille du domicile beta
	- Exposant de la taille du domicile alpha

	*/

	std::vector<double> lb { 0.,0.,0.,0.,0.,0.,0. };
	std::vector<double> ub { 1.,1.,1.,1.,5.,5.,5. };
	opt.set_lower_bounds(lb);
	opt.set_upper_bounds(ub);

	std::vector<std::string> var_names
		{"Q","p_asx","p_pr","Q_prior","k","beta","alpha"};
	std::vector<double> init_guess { 0.5,0.5,0.5,0.5,1.,1.,1. } ;

	Eigen::MatrixXd test_matrix = readMatrix("sim_matrix.txt");

	std::vector<std::vector<double> > variables;

	// Eigen::MatrixXd prop = proposal(priors,max_n,rd);
	// std::cout << prop << std::endl;
	void* my_func_data = static_cast<void *>(&test_matrix);
	opt.set_min_objective(Dev,my_func_data);
	double minf =0.;
	try
	{
		nlopt::result result = opt.optimize(init_guess,minf);
	}	catch(const nlopt::roundoff_limited& e)
	{
		for(int i=0; i<7; ++i)
		{
			std::cout << var_names[i] << " : " << init_guess[i] << std::endl;
		}
		std::cout << "Vraisemblance : " << minf << std::endl;
	}

	// std::random_device rd;
	// std::mt19937 rng(rd());

	// for (unsigned long int i = 0; i < mcmc_steps; ++i)
	// {
	// 	Eigen::MatrixXd prop = proposal(priors,max_n,rng);
	// 	std::vector<double> init_guess { 0.5,0.5,0.5,0.5,1.,1.,1. } ;
	// 	void* my_func_data = static_cast<void *>(&prop);
	// 	opt.set_min_objective(Dev,my_func_data);
	// 	opt.set_maxeval(10000);
	// 	double minf = 0.;
	// 	try
	// 	{
	// 		nlopt::result result = opt.optimize(init_guess,minf);
	// 	}	catch(const nlopt::roundoff_limited& e)
	// 	{
	// 		if(isnan(minf))
	// 		{
	// 			std::cout << prop << std::endl;
	// 			break;
	// 		}
	// 		init_guess.push_back(minf);
	// 		variables.push_back(init_guess);
	// 	}
	// }

	// std::ofstream outfile("mcmc_output.csv");
 //    std::ostream_iterator<double> output_iterator(outfile, " ");
 //    for(auto iteration : variables)
 //    {
	// 	std::copy(iteration.begin(),iteration.end(),output_iterator);
	// 	outfile << "\n";
 //    }

	return 0;
}


/*
 * eval_basic.cpp
 * Date: 2013-05-10
 * Author: Karsten Ahnert (karsten.ahnert@gmx.de)
 */

#define FUSION_MAX_VECTOR_SIZE 20

#include "parser.hpp"
#include "generate_data.hpp"

#include <gpcxx/eval/static_eval.hpp>
#include <gpcxx/generate/basic_generate_strategy.hpp>
#include <gpcxx/generate/uniform_symbol.hpp>
#include <gpcxx/io/simple.hpp>
#include <gpcxx/tree/basic_tree.hpp>
#include <gpcxx/app/timer.hpp>
#include <gpcxx/stat/node_statistics.hpp>

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

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

using namespace std;
namespace fusion = boost::fusion;

typedef double value_type;
typedef std::vector< value_type > vector_type;
typedef std::array< double , 3 > context_type;
typedef char symbol_type;
typedef std::mt19937 rng_type ;

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

inline value_type my_log( value_type v )
{
    return ( std::abs( v ) < 1.0e-20 ) ? 0.0 : std::log( std::abs( v ) );
}

struct my_div
{
    template< typename T >
    inline T operator()( T const& t1 , T const &t2 ) const
    {
        if( t2 == 0.0 ) return 1.0;
        else return t1 / t2;
    }
};

double objective_function( double x1 , double x2 , double x3 )
{
    return  1.0 / ( 1.0 + pow( x1 , -4.0 ) ) + 1.0 / ( 1.0 + pow( x2 , -4.0 ) ) + 1.0 / ( 1.0 + pow( x3 , -4.0 ) );
}

struct eval_cursor1
{
    template< typename Cursor >
    static inline value_type eval_cursor( Cursor const& c , context_type const &context )
    {
        switch( *c )
        {
            case 'x' : return context[0]; break;
            case 'y' : return context[1]; break;
            case 'z' : return context[2]; break;
            case 'e' : return exp( eval_cursor( c.children(0) , context ) ); break;
            case 'l' : return my_log( eval_cursor( c.children(0) , context ) ); break;
            case 's' : return sin( eval_cursor( c.children(0) , context ) ); break;
            case 'c' : return cos( eval_cursor( c.children(0) , context ) ); break;
            case '+' : return eval_cursor( c.children(0) , context ) + eval_cursor( c.children(1) , context ); break;
            case '-' : return eval_cursor( c.children(0) , context ) - eval_cursor( c.children(1) , context ); break;
            case '*' : return eval_cursor( c.children(0) , context ) * eval_cursor( c.children(1) , context ); break;
            case '/' : return my_div()( eval_cursor( c.children(0) , context ) , eval_cursor( c.children(1) , context ) ); break;
        }
        return value_type( 0.0 );
    }

    template< typename Tree >
    inline value_type operator()( Tree const &t , context_type const &context ) const
    {
        return eval_cursor1::eval_cursor( t.root() , context );
    }
};

struct eval_cursor2
{
    template< typename Cursor >
    inline static value_type eval_cursor( Cursor const &c , context_type const &context )
    {
        if( c.size() == 0 )
        {
            switch( *c )
            {
                case 'x' : return context[0]; break;
                case 'y' : return context[1]; break;
                case 'z' : return context[2]; break;
            }
        }
        else if( c.size() == 1 )
        {
            switch( *c )
            {
                case 'e' : return exp( eval_cursor( c.children(0) , context ) ); break;
                case 'l' : return my_log( eval_cursor( c.children(0) , context ) ); break;
                case 's' : return sin( eval_cursor( c.children(0) , context ) ); break;
                case 'c' : return cos( eval_cursor( c.children(0) , context ) ); break;
            }
        }
        else if( c.size() == 2 )
        {
            switch( *c )
            {
                case '+' : return eval_cursor( c.children(0) , context ) + eval_cursor( c.children(1) , context ); break;
                case '-' : return eval_cursor( c.children(0) , context ) - eval_cursor( c.children(1) , context ); break;
                case '*' : return eval_cursor( c.children(0) , context ) * eval_cursor( c.children(1) , context ); break;
                case '/' : return eval_cursor( c.children(0) , context ) / eval_cursor( c.children(1) , context ); break;
            }
        }

    }

    template< typename Tree >
    inline value_type operator()( Tree const &t , context_type const &context ) const
    {
        return eval_cursor( t.root() , context );
    }
};

template< typename Cursor >
struct eval_cursor3
{
    typedef Cursor cursor_type;
    typedef eval_cursor3< cursor_type > evaluator;

    static inline double eval_x( cursor_type const &c , context_type const& context ) { return context[0]; }
    static inline double eval_y( cursor_type const &c , context_type const& context ) { return context[0]; }
    static inline double eval_z( cursor_type const &c , context_type const& context ) { return context[0]; }
    static inline double eval_sin( cursor_type const &c , context_type const& context ) { return sin( eval_cursor( c.children(0) , context ) ); }
    static inline double eval_cos( cursor_type const &c , context_type const& context ) { return cos( eval_cursor( c.children(0) , context ) ); }
    static inline double eval_exp( cursor_type const &c , context_type const& context ) { return exp( eval_cursor( c.children(0) , context ) ); }
    static inline double eval_log( cursor_type const &c , context_type const& context ) { return my_log( eval_cursor( c.children(0) , context ) ); }
    static inline double eval_plus( cursor_type const &c , context_type const& context )
    { return  eval_cursor( c.children(0) , context ) + eval_cursor( c.children(1) , context ); }
    static inline double eval_minus( cursor_type const &c , context_type const& context )
    { return eval_cursor( c.children(0) , context ) - eval_cursor( c.children(1) , context ); }
    static inline double eval_multiplies( cursor_type const &c , context_type const& context )
    { return eval_cursor( c.children(0) , context ) * eval_cursor( c.children(1) , context ); }
    static inline double eval_divides( cursor_type const &c , context_type const& context )
    { return eval_cursor( c.children(0) , context ) / eval_cursor( c.children(1) , context ); }

    typedef double( *func_type )( cursor_type const& , context_type const& );
    typedef std::array< func_type , 128 > lookup_table_type;
    static lookup_table_type const& get_table( void )
    {
        static lookup_table_type tbl;
        std::fill( tbl.begin() , tbl.end() , nullptr );
        tbl[ size_t( 'x' ) ] = &evaluator::eval_x;
        tbl[ size_t( 'y' ) ] = &evaluator::eval_y;
        tbl[ size_t( 'z' ) ] = &evaluator::eval_z;
        tbl[ size_t( 's' ) ] = &evaluator::eval_sin;
        tbl[ size_t( 'c' ) ] = &evaluator::eval_cos;
        tbl[ size_t( 'e' ) ] = &evaluator::eval_exp;
        tbl[ size_t( 'l' ) ] = &evaluator::eval_log;
        tbl[ size_t( '+' ) ] = &evaluator::eval_plus;
        tbl[ size_t( '-' ) ] = &evaluator::eval_minus;
        tbl[ size_t( '*' ) ] = &evaluator::eval_multiplies;
        tbl[ size_t( '/' ) ] = &evaluator::eval_divides;
        return tbl;
    }

    static double eval_cursor( cursor_type const &cursor , context_type const& c )
    {
        lookup_table_type const& tbl = get_table();
        func_type f = tbl[ size_t( *cursor ) ];
        return (*f)( cursor , c );
    }

    template< typename Tree >
    double operator()( Tree const& t , context_type const & c ) const
    {
        return eval_cursor( t.root() , c );
    }
};

auto eval_cursor4 = gpcxx::make_static_eval< value_type , symbol_type , context_type >(
    fusion::make_vector(
        fusion::make_vector( 'x' , []( context_type const& t ) { return t[0]; } )
        , fusion::make_vector( 'y' , []( context_type const& t ) { return t[1]; } )
        , fusion::make_vector( 'z' , []( context_type const& t ) { return t[2]; } )
        ) ,
    fusion::make_vector(
        fusion::make_vector( 's' , []( double v ) -> double { return std::sin( v ); } )
        , fusion::make_vector( 'c' , []( double v ) -> double { return std::cos( v ); } )
        , fusion::make_vector( 'e' , []( double v ) -> double { return std::exp( v ); } )
        , fusion::make_vector( 'l' , []( double v ) -> double { return my_log( v ); } )
        ) ,
    fusion::make_vector(
        fusion::make_vector( '+' , std::plus< double >() )
        , fusion::make_vector( '-' , std::minus< double >() )
        , fusion::make_vector( '*' , std::multiplies< double >() )
        , fusion::make_vector( '/' , std::divides< double >() )
        ) );

/// \return time for evaluation of tree, result sum
template< typename Evaluator , typename Trees >
std::tuple< double , double > run_test( Evaluator const &eval , Trees const &trees , const vector_type &x1 , const vector_type &x2 , const vector_type &x3 , std::vector< double > &fitness )
{
    std::tuple< double , double > res;

    size_t number_of_datapoints = x1.size();
    size_t number_of_trees = trees.size();
    std::vector< vector_type > y( number_of_trees );
    for( size_t t=0 ; t<number_of_trees ; ++t ) y[t] = vector_type( number_of_datapoints );

    //
    // EVALUATION
    //
    gpcxx::timer timer;
    for( size_t t=0 ; t<number_of_trees ; ++t )
    {
        for( size_t i=0 ; i<number_of_datapoints ; ++i )
        {
            context_type c { { x1[i] , x2[i] , x3[i] } };
            y[t][i] = eval( trees[t] , c );
        }
    }
    std::get< 0 >( res ) = timer.seconds();

    double sum = 0.0;

    for( size_t t=0 ; t<number_of_trees ; ++t )
    {
        double chi2 = 0.0;
        for( size_t i=0 ; i<number_of_datapoints ; ++i )
        {
            sum += y[t][i];
            double diff = y[t][i] - objective_function( x1[i] , x2[i] , x3[i] );
            chi2 += std::abs( diff );
        }
        chi2 /= double( x1.size() );
        fitness.push_back( 1.0 / ( 1.0 + chi2 ) );
    }
    std::get< 1 >( res ) = sum;

    return res;
}

template< typename Tree , typename Evaluator >
void run_tree_type( std::string const &name , Evaluator const& eval , vector_type const &x1 , vector_type const &x2 , vector_type const &x3 , std::string const & filename )
{
    std::vector< Tree > trees;

    // read trees
    ifstream fin( filename );
    std::string line;
    while( std::getline( fin , line ) )
    {
        Tree tree;
        trees.push_back( tree );
        parser::tree_transformator< Tree > trafo( trees.back() , trees.back().root() );
        parser::parse_tree( line , trafo );
    }

    std::vector< double > fitness;

    cout.precision( 14 );
    cout << "Starting test " << name << endl;
    auto times = run_test( eval , trees , x1 , x2 , x3 , fitness );
    cout << tab << "Finished!" << endl;
    cout << tab << "Evaluation time " << std::get< 0 >( times ) << endl;
    cout << tab << "Result sum " << std::get< 1 >( times ) << endl << endl;

    std::ofstream fout( std::string( "eval_" ) + name + ".dat" );
    fout.precision( 14 );
    for( size_t i=0 ; i<fitness.size() ; ++i )
    {
        gpcxx::node_statistics stat = gpcxx::calc_node_statistics_tree( trees[i] );

        fout << i;
        fout << tab << fitness[i];
        fout << tab << trees[i].root().height();
        fout << tab << stat.num_nodes;
        fout << tab << gpcxx::simple( trees[i] );
        fout << "\n";
    }
}

int main( int argc , char *argv[] )
{
    if( argc != 2 )
    {
        cerr << "usage : " << argv[0] << " infile" << endl;
        return -1;
    }

    // generate test data
    vector_type x1, x2 , x3;
    // generate_test_data( x1 , x2 , x3 , -5.0 , 5.0 + 0.1 , 1.0 );
    generate_test_data( x1 , x2 , x3 , -5.0 , 5.0 + 0.1 , 0.4 );

    // run test for several tree tests
    run_tree_type< gpcxx::basic_tree< char > >( "basic_tree_eval1" , eval_cursor1() , x1 , x2 , x3 , argv[1] );
//     run_tree_type< gpcxx::basic_tree< char > >( "basic_tree_eval2" , eval_cursor2() , x1 , x2 , x3 , argv[1] );
//     run_tree_type< gpcxx::basic_tree< char > >( "basic_tree_eval3" , eval_cursor3< gpcxx::basic_tree< char >::const_cursor >() , x1 , x2 , x3 , argv[1] );
//     run_tree_type< gpcxx::basic_tree< char > >( "basic_tree_eval4" , eval_cursor4 , x1 , x2 , x3 , argv[1] );

    return 0;
}