// Copyright 2004 The Trustees of Indiana University.

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

//  Authors: Douglas Gregor
//           Andrew Lumsdaine
#ifndef _ALG_CTX_BWTC_H
#define _ALG_CTX_BWTC_H

#include <stack>
#include <vector>
#include <boost/graph/overloading.hpp>
#include <boost/graph/dijkstra_shortest_paths.hpp>
#include <boost/graph/dijkstra_shortest_paths_no_color_map.hpp>
#include <boost/graph/breadth_first_search.hpp>
#include <boost/graph/relax.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/tuple/tuple.hpp>
#include <boost/type_traits/is_convertible.hpp>
#include <boost/type_traits/is_same.hpp>
#include <boost/mpl/if.hpp>
#include <boost/property_map/property_map.hpp>
#include <boost/graph/named_function_params.hpp>
#include <algorithm>

#include <functional>

using namespace boost;

namespace nctx {

namespace detail { namespace graph {

  /**
   * @brief Dijkstra visitor keeping track of vertices, shortest paths and updates due to decisions on constraints.
   *
   * This visitor is based on the Boost implementation. The corresponding documentation states:
   *
   * Customized visitor passed to Dijkstra's algorithm by Brandes'
   * betweenness centrality algorithm. This visitor is responsible for
   * keeping track of the order in which vertices are discovered, the
   * predecessors on the shortest path(s) to a vertex, and the number
   * of shortest paths.
   *
   * In the extension of nctx, this visitor is also responsible for keeping track of the decisions resulting from the decision function. If the function prohibits traversal to a descending node, the visitor rolls back distance updates by Dijkstra's algorithm.
   */
  template<typename Graph, typename WeightMap, typename IncomingMap,
           typename DistanceMap, typename PathCountMap>
  struct brandes_dijkstra_visitor : public bfs_visitor<>
  {
    typedef typename graph_traits<Graph>::vertex_descriptor vertex_descriptor;
    typedef typename graph_traits<Graph>::edge_descriptor edge_descriptor;
    typedef typename std::function<bool(typename graph_traits<Graph>::vertex_descriptor, typename graph_traits<Graph>::vertex_descriptor, typename graph_traits<Graph>::vertex_descriptor)> decision_function;

    brandes_dijkstra_visitor(std::stack<vertex_descriptor>& ordered_vertices,
                             WeightMap weight,
                             IncomingMap incoming,
                             DistanceMap distance,
                             PathCountMap path_count_constraint,
                             PathCountMap path_count_unconstraint,
                             decision_function &betw_decision_fct,
                             vertex_descriptor s)
      : ordered_vertices(ordered_vertices), weight(weight),
        incoming(incoming), distance(distance),
        path_count_constraint(path_count_constraint),
        path_count_unconstraint(path_count_unconstraint), betw_decision_fct(betw_decision_fct),
        s(s)
    { }

    /**
     * Whenever an edge e = (v, w) is relaxed, the incoming edge list
     * for w is set to {(v, w)} and the shortest path count of w is set to
     * the number of paths that reach {v}.
     */
    void edge_relaxed(edge_descriptor e, const Graph& g)
    {
      vertex_descriptor v = source(e, g), w = target(e, g);
      incoming[w].clear();
      incoming[w].push_back(e);
      put(path_count_unconstraint, w, get(path_count_unconstraint, v));
      if(betw_decision_fct(s,v,w)){
        put(path_count_constraint, w, get(path_count_constraint, v));
      }
    }

    /**
     * If an edge e = (v, w) was not relaxed, it may still be the case
     * that we've found more equally-short paths, so include {(v, w)} in the
     * incoming edges of w and add all of the shortest paths to v to the
     * shortest path count of w.
     */
    void edge_not_relaxed(edge_descriptor e, const Graph& g)
    {
      typedef typename property_traits<WeightMap>::value_type weight_type;
      typedef typename property_traits<DistanceMap>::value_type distance_type;
      vertex_descriptor v = source(e, g), w = target(e, g);

      distance_type d_v = get(distance, v), d_w = get(distance, w);
      weight_type w_e = get(weight, e);
      closed_plus<distance_type> combine;
      if (d_w == combine(d_v, w_e)) {
        put(path_count_unconstraint, w, get(path_count_unconstraint, w) + get(path_count_unconstraint, v));
        incoming[w].push_back(e);
        if(betw_decision_fct(s,v,w)){
          put(path_count_constraint, w, get(path_count_constraint, w) + get(path_count_constraint, v));
        }
      }

    }

    /// Keep track of vertices as they are reached
    void examine_vertex(vertex_descriptor w, const Graph&)
    {
      ordered_vertices.push(w);
    }

  private:
    std::stack<vertex_descriptor>& ordered_vertices;
    WeightMap weight;
    IncomingMap incoming;
    DistanceMap distance;
    PathCountMap path_count_constraint;
    PathCountMap path_count_unconstraint;
    decision_function &betw_decision_fct;
    vertex_descriptor s;
  };

  /**
   * Function object that calls Dijkstra's shortest paths algorithm
   * using the Dijkstra visitor for the Brandes betweenness centrality
   * algorithm.
   */
  template<typename WeightMap, typename Graph, typename BetweennessDecisionFunction>
  struct brandes_dijkstra_shortest_paths
  {
    typedef typename graph_traits<Graph>::vertex_descriptor vertex_descriptor;
    //~ typedef typename BetweennessDecisionFunction = std::function<bool(vertex_descriptor, vertex_descriptor, vertex_descriptor)>;
    brandes_dijkstra_shortest_paths(WeightMap weight_map,
               BetweennessDecisionFunction &betw_decision_fct)
      : weight_map(weight_map), betw_decision_fct(betw_decision_fct) { }

    template<typename IncomingMap, typename DistanceMap,
             typename PathCountMap, typename VertexIndexMap>
    void
    operator()(Graph& g,
               vertex_descriptor s,
               std::stack<vertex_descriptor>& ov,
               IncomingMap incoming,
               DistanceMap distance,
               PathCountMap path_count_constraint,
               PathCountMap path_count_unconstraint,
               VertexIndexMap vertex_index)
    {
      typedef brandes_dijkstra_visitor<Graph, WeightMap, IncomingMap,
                                       DistanceMap, PathCountMap> visitor_type;
      visitor_type visitor(ov, weight_map, incoming, distance,
        path_count_constraint,
        path_count_unconstraint, betw_decision_fct, s);

      boost::dijkstra_shortest_paths(g, s,
                              boost::weight_map(weight_map)
                              .vertex_index_map(vertex_index)
                              .distance_map(distance)
                              .visitor(visitor));
    }

  private:
    BetweennessDecisionFunction betw_decision_fct;
    WeightMap weight_map;
  };

  // When the edge centrality map is a dummy property map, no
  // initialization is needed.
  template<typename Iter>
  inline void
  init_centrality_map(std::pair<Iter, Iter>, dummy_property_map) { }

  // When we have a real edge centrality map, initialize all of the
  // centralities to zero.
  template<typename Iter, typename Centrality>
  void
  init_centrality_map(std::pair<Iter, Iter> keys, Centrality centrality_map)
  {
    typedef typename property_traits<Centrality>::value_type
      centrality_type;
    while (keys.first != keys.second) {
      put(centrality_map, *keys.first, centrality_type(0));
      ++keys.first;
    }
  }

  // When the edge centrality map is a dummy property map, no update
  // is performed.
  template<typename Key, typename T>
  inline void
  update_centrality(dummy_property_map, const Key&, const T&) { }

  // When we have a real edge centrality map, add the value to the map
  template<typename CentralityMap, typename Key, typename T>
  inline void
  update_centrality(CentralityMap centrality_map, Key k, const T& x)
  { put(centrality_map, k, get(centrality_map, k) + x); }

  template<typename Iter>
  inline void
  divide_centrality_by_two(std::pair<Iter, Iter>, dummy_property_map) {}

  template<typename Iter, typename CentralityMap>
  inline void
  divide_centrality_by_two(std::pair<Iter, Iter> keys,
                           CentralityMap centrality_map)
  {
    typename property_traits<CentralityMap>::value_type two(2);
    while (keys.first != keys.second) {
      put(centrality_map, *keys.first, get(centrality_map, *keys.first) / two);
      ++keys.first;
    }
  }

  template<typename Graph, typename CentralityMap, typename EdgeCentralityMap,
           typename IncomingMap, typename DistanceMap,
           typename DependencyMap, typename PathCountMap,
           typename VertexIndexMap, typename ShortestPaths>
  void
  brandes_betweenness_centrality_impl(const Graph& g,
                                      CentralityMap centrality,     // C_B
                                      EdgeCentralityMap edge_centrality_map,
                                      IncomingMap incoming, // P
                                      DistanceMap distance,         // d
                                      DependencyMap dependency,     // delta
                                      PathCountMap path_count_constraint,      // sigma
                                      PathCountMap path_count_unconstraint,      // sigma
                                      VertexIndexMap vertex_index,
                                      ShortestPaths shortest_paths)
  {
    typedef typename graph_traits<Graph>::vertex_iterator vertex_iterator;
    typedef typename graph_traits<Graph>::vertex_descriptor vertex_descriptor;
    //~ const BglGraph<T>& g = *(gc.get_graph());
    // Initialize centrality
    init_centrality_map(vertices(g), centrality);
    init_centrality_map(edges(g), edge_centrality_map);

    std::stack<vertex_descriptor> ordered_vertices;
    vertex_iterator s, s_end;
    for (boost::tie(s, s_end) = vertices(g); s != s_end; ++s) {
      // Initialize for this iteration
      vertex_iterator w, w_end;
      for (boost::tie(w, w_end) = vertices(g); w != w_end; ++w) {
        incoming[*w].clear();
        put(path_count_constraint, *w, 0);
        put(path_count_unconstraint, *w, 0);
        put(dependency, *w, 0);
      }
      put(path_count_constraint, *s, 1);
      put(path_count_unconstraint, *s, 1);

      // Execute the shortest paths algorithm. This will be either
      // Dijkstra's algorithm or a customized breadth-first search,
      // depending on whether the graph is weighted or unweighted.
      shortest_paths(g, *s, ordered_vertices, incoming, distance,
                     path_count_constraint,
                     path_count_unconstraint, vertex_index);

      while (!ordered_vertices.empty()) {
        vertex_descriptor w = ordered_vertices.top();
        ordered_vertices.pop();

        typedef typename property_traits<IncomingMap>::value_type
          incoming_type;
        typedef typename incoming_type::iterator incoming_iterator;
        typedef typename property_traits<DependencyMap>::value_type
          dependency_type;

        for (incoming_iterator vw = incoming[w].begin();
             vw != incoming[w].end(); ++vw) {
          vertex_descriptor v = source(*vw, g);
          //~ dependency_type factor_raw = dependency_type(get(path_count_unconstraint, v))
            //~ / dependency_type(get(path_count_unconstraint, w));

          if(dependency_type(get(path_count_constraint, w)) > dependency_type(0)){
            bool ctx_fac = (dependency_type(get(path_count_constraint, v)) != dependency_type(get(path_count_unconstraint, v)))
              && (dependency_type(get(path_count_constraint, w)) != dependency_type(get(path_count_unconstraint, w)));

            //~ dependency_type factor =
              //~ ((dependency_type(get(path_count_unconstraint, v)) + dependency_type(get(path_count_constraint, v))) / dependency_type(2))
              //~ / ((dependency_type(get(path_count_unconstraint, w)) + dependency_type(get(path_count_constraint, w))) / dependency_type(2));
            dependency_type factor = (ctx_fac) ?
              (dependency_type(get(path_count_constraint, v)) / dependency_type(get(path_count_constraint, w)))
              * (dependency_type(get(path_count_unconstraint, v)) / dependency_type(get(path_count_unconstraint, w)))
              :
              dependency_type(get(path_count_unconstraint, v)) / dependency_type(get(path_count_unconstraint, w));
            //~ if(factor_raw == dependency_type(0)){
              //~ std::cout << *s << " | " << v << " -> " << w <<  ": true: " << factor_raw << " \t fac: " << factor << std::endl;
              //~ std::cout << "\t  constraint v: \t" << dependency_type(get(path_count_constraint, v)) << std::endl
                        //~ << "\tunconstraint v: \t" << dependency_type(get(path_count_unconstraint, v)) << std::endl
                        //~ << "\t  constraint w: \t" << dependency_type(get(path_count_constraint, w)) << std::endl
                        //~ << "\tunconstraint w: \t" << dependency_type(get(path_count_unconstraint, w)) << std::endl;
            //~ }
            //~ if(dependency_type(get(path_count_constraint, v)) != dependency_type(get(path_count_unconstraint, v)))
              //~ std::cout << *s << " | " << v << " -> " << w <<  ": true: " << factor_raw << " \t fac: " << factor << std::endl
                        //~ << "\t  constraint v: \t" << dependency_type(get(path_count_constraint, v)) << std::endl
                        //~ << "\tunconstraint v: \t" << dependency_type(get(path_count_unconstraint, v)) << std::endl;
            factor *= (dependency_type(1) + get(dependency, w));
            put(dependency, v, get(dependency, v) + factor);
            update_centrality(edge_centrality_map, *vw, factor);
          }
        }

        if (w != *s) {
          update_centrality(centrality, w, get(dependency, w));
        }
      }
    }

    typedef typename graph_traits<Graph>::directed_category directed_category;
    const bool is_undirected =
      is_convertible<directed_category*, undirected_tag*>::value;
    if (is_undirected) {
      divide_centrality_by_two(vertices(g), centrality);
      divide_centrality_by_two(edges(g), edge_centrality_map);
    }
  }

} } // end namespace detail::graph

template<typename Graph, typename CentralityMap, typename EdgeCentralityMap,
         typename IncomingMap, typename DistanceMap,
         typename DependencyMap, typename PathCountMap,
         typename VertexIndexMap, typename WeightMap,
         typename BetweennessDecisionFunction>
void
brandes_betweenness_centrality(const Graph& g,
                               CentralityMap centrality,     // C_B
                               EdgeCentralityMap edge_centrality_map,
                               IncomingMap incoming, // P
                               DistanceMap distance,         // d
                               DependencyMap dependency,     // delta
                               PathCountMap path_count_constraint,      // sigma
                               PathCountMap path_count_unconstraint,      // sigma
                               VertexIndexMap vertex_index,
                               WeightMap weight_map,
                               BetweennessDecisionFunction &betw_decision_fct
                               BOOST_GRAPH_ENABLE_IF_MODELS_PARM(Graph,vertex_list_graph_tag))
{

  detail::graph::brandes_dijkstra_shortest_paths<WeightMap, const Graph, BetweennessDecisionFunction>
    shortest_paths(weight_map,betw_decision_fct);

  detail::graph::brandes_betweenness_centrality_impl(g, centrality,
                                                     edge_centrality_map,
                                                     incoming, distance,
                                                     dependency, path_count_constraint,
                                                     path_count_unconstraint,
                                                     vertex_index,
                                                     shortest_paths);
}

namespace detail { namespace graph {
  template<typename Graph, typename CentralityMap, typename EdgeCentralityMap,
           typename WeightMap, typename VertexIndexMap,
           typename BetweennessDecisionFunction>
  void
  brandes_betweenness_centrality_dispatch2(const Graph& g,
                                           CentralityMap centrality,
                                           EdgeCentralityMap edge_centrality_map,
                                           WeightMap weight_map,
                                           VertexIndexMap vertex_index,
                                           BetweennessDecisionFunction &betw_decision_fct)
  {
    typedef typename graph_traits<Graph>::degree_size_type degree_size_type;
    typedef typename graph_traits<Graph>::edge_descriptor edge_descriptor;
    typedef typename mpl::if_c<(is_same<CentralityMap,
                                        dummy_property_map>::value),
                                         EdgeCentralityMap,
                               CentralityMap>::type a_centrality_map;
    typedef typename property_traits<a_centrality_map>::value_type
      centrality_type;

    typename graph_traits<Graph>::vertices_size_type V = num_vertices(g);

    std::vector<std::vector<edge_descriptor> > incoming(V);
    std::vector<centrality_type> distance(V);
    std::vector<centrality_type> dependency(V);
    std::vector<degree_size_type> path_count_constraint(V);
    std::vector<degree_size_type> path_count_unconstraint(V);

    brandes_betweenness_centrality(
      g, centrality, edge_centrality_map,
      make_iterator_property_map(incoming.begin(), vertex_index),
      make_iterator_property_map(distance.begin(), vertex_index),
      make_iterator_property_map(dependency.begin(), vertex_index),
      make_iterator_property_map(path_count_constraint.begin(), vertex_index),
      make_iterator_property_map(path_count_unconstraint.begin(), vertex_index),
      vertex_index,
      weight_map,
      betw_decision_fct);
  }

  template<typename Graph, typename CentralityMap, typename EdgeCentralityMap,
           typename VertexIndexMap>
  void
  brandes_betweenness_centrality_dispatch2(const Graph& g,
                                           CentralityMap centrality,
                                           EdgeCentralityMap edge_centrality_map,
                                           VertexIndexMap vertex_index)
  {
    typedef typename graph_traits<Graph>::degree_size_type degree_size_type;
    typedef typename graph_traits<Graph>::edge_descriptor edge_descriptor;
    typedef typename mpl::if_c<(is_same<CentralityMap,
                                        dummy_property_map>::value),
                                         EdgeCentralityMap,
                               CentralityMap>::type a_centrality_map;
    typedef typename property_traits<a_centrality_map>::value_type
      centrality_type;

    typename graph_traits<Graph>::vertices_size_type V = num_vertices(g);

    std::vector<std::vector<edge_descriptor> > incoming(V);
    std::vector<centrality_type> distance(V);
    std::vector<centrality_type> dependency(V);
    std::vector<degree_size_type> path_count(V);

    brandes_betweenness_centrality(
      g, centrality, edge_centrality_map,
      make_iterator_property_map(incoming.begin(), vertex_index),
      make_iterator_property_map(distance.begin(), vertex_index),
      make_iterator_property_map(dependency.begin(), vertex_index),
      make_iterator_property_map(path_count.begin(), vertex_index),
      vertex_index);
  }

  template<typename Graph, typename WeightMap, typename BetweennessDecisionFunction>
  struct brandes_betweenness_centrality_dispatch1
  {
    template<typename CentralityMap,
             typename EdgeCentralityMap, typename VertexIndexMap>
    static void
    run(const Graph& g, CentralityMap centrality,
        EdgeCentralityMap edge_centrality_map, VertexIndexMap vertex_index,
        WeightMap weight_map,
        BetweennessDecisionFunction &betw_decision_fct)
    {
      brandes_betweenness_centrality_dispatch2(g, centrality, edge_centrality_map,
                                               weight_map, vertex_index,
                                               betw_decision_fct);
    }
  };

  template <typename T>
  struct is_bgl_named_params {
    BOOST_STATIC_CONSTANT(bool, value = false);
  };

  template <typename Param, typename Tag, typename Rest>
  struct is_bgl_named_params<bgl_named_params<Param, Tag, Rest> > {
    BOOST_STATIC_CONSTANT(bool, value = true);
  };

} } // end namespace detail::graph

// DocString: betweenness_ctx
/**
 * @brief Betweenness centrality with dynamic contextual constraints.
 *
 * Using this function allows obtaining betweenness centrality under dynamic contextual constraints. Enforcement of constraints is the task of the given user-defined function.
 *
 * The function enforcing contextual constraints is evaluated at each node during shortest path traversal. The function needs to evaluate to True or False allowing an edge to be visited or not. As parameters, the current state of the betweenness calculation is passed to the function, i.e. the starting node for which a centrality value is being calculated, the current node, and the descending node in question. If the function returns False, the descending node is not being visited.
 *
 * The three nodes are passed as indices allowing for access of (external) attribute and other associated information.
 *
 * Note that the decision function is evaluated more than once during path traversal. That means, there should not happen any resource-intense computation inside this function. Also, it does not allow to keep track of the status of calculation, e.g. by calculating the visited edges or something similar.
 *
 * If the decision function simply returns True all the time, this function results in the unaltered betweenness centrality values.
 *
 * Obtaining betweenness centrality is based on Brandes' efficient algorithm. At the current stage, the implementation allows for single-core execution only.
 *
 * @param g The graph object
 * @param betw_decision_fct A function enforcing constraints. The signature of the function is ``(vertex index, vertex index, vertex index) -> Bool``.
 *
 */
template<typename Graph, typename Param, typename Tag, typename Rest, typename DecisionFunction>
void
brandes_betweenness_centrality_ctx(const Graph& g,
                               const bgl_named_params<Param,Tag,Rest>& params,
                               DecisionFunction &betw_decision_fct
                               BOOST_GRAPH_ENABLE_IF_MODELS_PARM(Graph,vertex_list_graph_tag))
{
  typedef bgl_named_params<Param,Tag,Rest> named_params;
  typedef typename get_param_type<edge_weight_t, named_params>::type ew;

  detail::graph::brandes_betweenness_centrality_dispatch1<Graph, ew, DecisionFunction>::run(
    g,
    choose_param(get_param(params, vertex_centrality),
                 dummy_property_map()),
    choose_param(get_param(params, edge_centrality),
                 dummy_property_map()),
    choose_param(get_param(params, vertex_index),
                 dummy_property_map()),
    get_param(params, edge_weight),
    betw_decision_fct);

}

// disable_if is required to work around problem with MSVC 7.1 (it seems to not
// get partial ordering getween this overload and the previous one correct)
//~ template<typename Graph, typename CentralityMap>
//~ typename disable_if<detail::graph::is_bgl_named_params<CentralityMap>,
                    //~ void>::type
//~ brandes_betweenness_centrality(const Graph& g, CentralityMap centrality
                               //~ BOOST_GRAPH_ENABLE_IF_MODELS_PARM(Graph,vertex_list_graph_tag))
//~ {
  //~ detail::graph::brandes_betweenness_centrality_dispatch2(
    //~ g, centrality, dummy_property_map(), get(vertex_index, g)); //TODO INDIZES
//~ }

//~ template<typename Graph, typename CentralityMap, typename EdgeCentralityMap>
//~ void
//~ brandes_betweenness_centrality(const Graph& g, CentralityMap centrality,
                               //~ EdgeCentralityMap edge_centrality_map
                               //~ BOOST_GRAPH_ENABLE_IF_MODELS_PARM(Graph,vertex_list_graph_tag))
//~ {
  //~ detail::graph::brandes_betweenness_centrality_dispatch2(
    //~ g, centrality, edge_centrality_map, get(vertex_index, g)); //TODO INDIZES
//~ }

/**
 * Converts "absolute" betweenness centrality (as computed by the
 * brandes_betweenness_centrality algorithm) in the centrality map
 * into "relative" centrality. The result is placed back into the
 * given centrality map.
 */
template<typename Graph, typename CentralityMap>
void
relative_betweenness_centrality(const Graph& g, CentralityMap centrality)
{
  typedef typename graph_traits<Graph>::vertex_iterator vertex_iterator;
  typedef typename property_traits<CentralityMap>::value_type centrality_type;

  typename graph_traits<Graph>::vertices_size_type n = num_vertices(g);
  centrality_type factor = centrality_type(2)/centrality_type(n*n - 3*n + 2);
  vertex_iterator v, v_end;
  for (boost::tie(v, v_end) = vertices(g); v != v_end; ++v) {
    put(centrality, *v, factor * get(centrality, *v));
  }
}

} // end namespace boost

#endif // BOOST_GRAPH_BRANDES_BETWEENNESS_CENTRALITY_HPP


#include "teca_cartesian_mesh_coordinate_transform.h"

#include "teca_cartesian_mesh.h"
#include "teca_array_collection.h"
#include "teca_variant_array.h"
#include "teca_metadata.h"
#include "teca_coordinate_util.h"

#include <algorithm>
#include <iostream>

#if defined(TECA_HAS_BOOST)
#include <boost/program_options.hpp>
#endif

//#define TECA_DEBUG

struct teca_cartesian_mesh_coordinate_transform::internals_t
{
    internals_t() : bounds_in{0.0}, bounds_out{0.0} {}

    void clear();

    /** transform the input such that it covers x_out_0 to x_out_1 while
     * maintaining relative relationships between points
     */
    template <typename coord_t>
    static
    void transform_axis(coord_t *x_out, const coord_t *x_in, size_t nx,
        coord_t x_out_0, coord_t x_out_1);

    /// check that the requested bounds are valid for a given input
    static
    int validate_target_bounds(char ax_name,
        const const_p_teca_variant_array &ax_in, const double *tgt_bounds);

    /** allocate buffers and apply the transform. the transform is skipped
     * if the rquested bounds are invalid (higher bounds is less than lower bound)
     */
    static
    void transform_axes(p_teca_variant_array &ax_out,
        const const_p_teca_variant_array &ax_in, const double *tgt_bounds);

    double bounds_in[6];
    double bounds_out[6];

    teca_metadata coordinates_in;
    teca_metadata coordinates_out;

    std::string x_axis_variable_out;
    std::string y_axis_variable_out;
    std::string z_axis_variable_out;

    teca_metadata x_axis_attributes_out;
    teca_metadata y_axis_attributes_out;
    teca_metadata z_axis_attributes_out;
};

// --------------------------------------------------------------------------
void teca_cartesian_mesh_coordinate_transform::internals_t::clear()
{
    for (int i = 0; i < 6; ++i)
        bounds_in[i] = 0.0;

    for (int i = 0; i < 6; ++i)
        bounds_out[i] = 0.0;

    this->coordinates_in.clear();
    this->coordinates_out.clear();

    this->x_axis_variable_out = "";
    this->y_axis_variable_out = "";
    this->z_axis_variable_out = "";

    this->x_axis_attributes_out.clear();
    this->y_axis_attributes_out.clear();
    this->z_axis_attributes_out.clear();
}

// --------------------------------------------------------------------------
template <typename coord_t>
void teca_cartesian_mesh_coordinate_transform::internals_t::transform_axis(
    coord_t *x_out, const coord_t *x_in, size_t nx, coord_t x_out_0, coord_t x_out_1)
{
    // transform the input onto [0, 1] and then
    // shift and scale to the target range [x_out_0, x_out_1]
    coord_t x_in_min = x_in[0];
    coord_t x_in_max = x_in[nx - 1];
    coord_t delta_x_in = x_in_max - x_in_min;
    coord_t delta_x_out = x_out_1 - x_out_0;
    coord_t scale_fac = nx < 2 ? coord_t(0) : delta_x_out / delta_x_in;

    for (size_t i = 0; i < nx; ++i)
        x_out[i] = (x_in[i] - x_in_min) * scale_fac + x_out_0;
}

// --------------------------------------------------------------------------
int teca_cartesian_mesh_coordinate_transform::internals_t::validate_target_bounds(
    char ax_name, const const_p_teca_variant_array &ax_in, const double *tgt_bounds)
{
    size_t ax_size = ax_in->size();
    bool tgt_bounds_valid = !(tgt_bounds[1] < tgt_bounds[0]);

    // check that the bounds specified in each axis direction are different
    // skip this check when the bounds are invlaid, or the input axis has
    // only 1 value
    if (tgt_bounds_valid && (ax_size > 1) &&
        teca_coordinate_util::equal(tgt_bounds[0], tgt_bounds[1]))
    {
        TECA_ERROR("Invlaid " << ax_name << "-axis target bounds specified ["
            << tgt_bounds[0] << ", " << tgt_bounds[1]
            << "] the values are equal but there are " << ax_size << " values.")

        return -1;
    }

    return 0;
}

// --------------------------------------------------------------------------
void teca_cartesian_mesh_coordinate_transform::internals_t::transform_axes(
    p_teca_variant_array &ax_out, const const_p_teca_variant_array &ax_in,
    const double *tgt_bounds)
{
    TEMPLATE_DISPATCH(const teca_variant_array_impl,
        ax_in.get(),

        if(!(tgt_bounds[1] < tgt_bounds[0]))
        {
            // allocate output
            size_t ax_size = ax_in->size();

            ax_out = ax_in->new_instance(ax_size);

            NT *p_ax_out =
                dynamic_cast<teca_variant_array_impl<NT>*>(ax_out.get())->get();

            // transform the axis
            const NT *p_ax_in =
                dynamic_cast<const TT*>(ax_in.get())->get();

            teca_cartesian_mesh_coordinate_transform::internals_t::transform_axis(
                p_ax_out, p_ax_in, ax_size, NT(tgt_bounds[0]), NT(tgt_bounds[1]));
        }
        else
        {
            // the requested bounds are invalid, pass the current axis through
            ax_out = ax_in->new_copy();
        }
        )
}

// --------------------------------------------------------------------------
teca_cartesian_mesh_coordinate_transform::teca_cartesian_mesh_coordinate_transform()
    : target_bounds({1.0,0.0, 1.0,0.0, 1.0,0.0})
{
    this->set_number_of_input_connections(1);
    this->set_number_of_output_ports(1);

    this->internals = new teca_cartesian_mesh_coordinate_transform::internals_t;
}

// --------------------------------------------------------------------------
teca_cartesian_mesh_coordinate_transform::~teca_cartesian_mesh_coordinate_transform()
{
    delete this->internals;
}

// --------------------------------------------------------------------------
void teca_cartesian_mesh_coordinate_transform::set_modified()
{
    this->internals->clear();
    this->teca_algorithm::set_modified();
}

// --------------------------------------------------------------------------
int teca_cartesian_mesh_coordinate_transform::set_target_bounds(const teca_metadata &md)
{
    teca_metadata coords;
    if (md.get("coordinates", coords))
    {
        TECA_ERROR("Failed to set target_bounds metadata is missing"
            " coordinates key")
        return -1;
    }

    // set target bounds to pass through
    this->target_bounds[0] = 1;
    this->target_bounds[1] = 0;
    this->target_bounds[2] = 1;
    this->target_bounds[3] = 0;
    this->target_bounds[4] = 1;
    this->target_bounds[5] = 0;

    // set the bounds from the coordinate axes that are found
    const_p_teca_variant_array x = coords.get("x");
    if (x)
    {
        unsigned long nx = x->size();
        x->get(0, this->target_bounds[0]);
        x->get(nx - 1, this->target_bounds[1]);
    }

    const_p_teca_variant_array y = coords.get("y");
    if (y)
    {
        unsigned long ny = y->size();
        y->get(0, this->target_bounds[2]);
        y->get(ny - 1, this->target_bounds[3]);
    }

    const_p_teca_variant_array z = coords.get("z");
    if (z)
    {
        unsigned long nz = z->size();
        z->get(0, this->target_bounds[4]);
        z->get(nz - 1, this->target_bounds[5]);
    }

    if (!x && !y && !z)
    {
        TECA_ERROR("failed to set target_bounds cooridinate metadata"
            " is missing x, y, and z coordinate arrays")
        return -1;
    }

    return 0;
}

#if defined(TECA_HAS_BOOST)
// --------------------------------------------------------------------------
void teca_cartesian_mesh_coordinate_transform::get_properties_description(
    const std::string &prefix, options_description &global_opts)
{
    options_description opts("Options for "
        + (prefix.empty()?"teca_cartesian_mesh_coordinate_transform":prefix));

    opts.add_options()
        TECA_POPTS_MULTI_GET(std::vector<double>, prefix, target_bounds,
            "6 double precision values that define the output coordinate axis"
            " bounds, specified in the following order : [x0 x1 y0 y1 z0 z1]."
            " The Cartesian mesh is transformed such that its coordinatres span"
            " the specified target bounds while maintaining relative spacing of"
            " original input coordinate points. Pass [1, 0] for each axis that"
            " should not be transformed.")
        TECA_POPTS_GET(std::string, prefix, x_axis_variable,
            "Set the name of variable that has x axis coordinates. If not"
            " provided, the name passed through unchanged.")
        TECA_POPTS_GET(std::string, prefix, y_axis_variable,
            "Set the name of variable that has y axis coordinates. If not"
            " provided, the name is passed through unchanged.")
        TECA_POPTS_GET(std::string, prefix, z_axis_variable,
            "Set the name of variable that has z axis coordinates. If not"
            " provided, the name is passed through unchanged.")
        TECA_POPTS_GET(std::string, prefix, x_axis_units,
            "Set the units of the x-axis coordinates. If not provided the"
            " units are passed through unchanged.")
        TECA_POPTS_GET(std::string, prefix, y_axis_units,
            "Set the units of the y-axis coordinates. If not provided the"
            " units are passed through unchanged.")
        TECA_POPTS_GET(std::string, prefix, z_axis_units,
            "Set the units of the z-axis coordinates. If not provided the"
            " units are passed through unchanged.")
        ;

    this->teca_algorithm::get_properties_description(prefix, opts);

    global_opts.add(opts);
}

// --------------------------------------------------------------------------
void teca_cartesian_mesh_coordinate_transform::set_properties(
    const std::string &prefix, variables_map &opts)
{
    this->teca_algorithm::set_properties(prefix, opts);

    TECA_POPTS_SET(opts, std::vector<double>, prefix, target_bounds)
    TECA_POPTS_SET(opts, std::string, prefix, x_axis_variable)
    TECA_POPTS_SET(opts, std::string, prefix, y_axis_variable)
    TECA_POPTS_SET(opts, std::string, prefix, z_axis_variable)
    TECA_POPTS_SET(opts, std::string, prefix, x_axis_units)
    TECA_POPTS_SET(opts, std::string, prefix, y_axis_units)
    TECA_POPTS_SET(opts, std::string, prefix, z_axis_units)
}
#endif

// --------------------------------------------------------------------------
teca_metadata teca_cartesian_mesh_coordinate_transform::get_output_metadata(
    unsigned int port, const std::vector<teca_metadata> &input_md)
{
#ifdef TECA_DEBUG
    std::cerr << teca_parallel_id()
        << "teca_cartesian_mesh_coordinate_transform::get_output_metadata"
        << std::endl;
#endif
    (void)port;

    if (this->internals->coordinates_out)
    {
        // update coordinates and bounds
        teca_metadata md_out = input_md[0];
        md_out.set("coordinates", this->internals->coordinates_out);
        md_out.set("bounds", this->internals->bounds_out);

        // update the attributes
        teca_metadata atts_out;
        md_out.get("attributes", atts_out);

        atts_out.set(this->internals->x_axis_variable_out, this->internals->x_axis_attributes_out);
        atts_out.set(this->internals->y_axis_variable_out, this->internals->y_axis_attributes_out);
        atts_out.set(this->internals->z_axis_variable_out, this->internals->z_axis_attributes_out);

        md_out.set("attributes", atts_out);

        return md_out;
    }

    // check that bounds for each cooridnate axes are specified
    if (this->target_bounds.size() != 6)
    {
        TECA_FATAL_ERROR("Invalid target_bounds. " << this->target_bounds.size()
            << "  were specified while 6 are needed.")
        return teca_metadata();
    }

    // copy the metadata
    teca_metadata out_md(input_md[0]);

    // get the input coordinate axes
    const_p_teca_variant_array x_in;
    const_p_teca_variant_array y_in;
    const_p_teca_variant_array z_in;

    if (out_md.get("coordinates", this->internals->coordinates_in)
        || !(x_in = this->internals->coordinates_in.get("x"))
        || !(y_in = this->internals->coordinates_in.get("y"))
        || !(z_in = this->internals->coordinates_in.get("z")))
    {
        TECA_FATAL_ERROR("The input metadata has invalid coordinates")
        this->internals->clear();
        return teca_metadata();
    }

    // get the input bounds
    teca_coordinate_util::get_cartesian_mesh_bounds(x_in,
        y_in, z_in, this->internals->bounds_in);

    const double *tgt_bounds = this->target_bounds.data();

    // check the bounds request
    if (internals_t::validate_target_bounds('x', x_in, tgt_bounds    )
        || internals_t::validate_target_bounds('y', y_in, tgt_bounds + 2)
        || internals_t::validate_target_bounds('z', z_in, tgt_bounds + 4))
    {
        TECA_FATAL_ERROR("Invalid bounds requested")
        this->internals->clear();
        return teca_metadata();
    }

    // transform the coordinate axes
    p_teca_variant_array x_out;
    p_teca_variant_array y_out;
    p_teca_variant_array z_out;

    internals_t::transform_axes(x_out, x_in, tgt_bounds    );
    internals_t::transform_axes(y_out, y_in, tgt_bounds + 2);
    internals_t::transform_axes(z_out, z_in, tgt_bounds + 4);

    // set the new axes into the coordinates object
    this->internals->coordinates_out = this->internals->coordinates_in;

    this->internals->coordinates_out.set("x", x_out);
    this->internals->coordinates_out.set("y", y_out);
    this->internals->coordinates_out.set("z", z_out);

    // update the bounds in the output metadata
    teca_coordinate_util::get_cartesian_mesh_bounds(x_out,
        y_out, z_out, this->internals->bounds_out);

    out_md.set("bounds", this->internals->bounds_out, 6);

    // get the input coordinate axis names
    std::string x_axis_variable_in;
    std::string y_axis_variable_in;
    std::string z_axis_variable_in;
    if (this->internals->coordinates_in.get("x_variable", x_axis_variable_in)
        || this->internals->coordinates_in.get("y_variable", y_axis_variable_in)
        || this->internals->coordinates_in.get("z_variable", z_axis_variable_in))
    {
        TECA_FATAL_ERROR("Failed to get the coordinate axis variables")
        this->internals->clear();
        return teca_metadata();
    }

    // update the output axis variable names
    this->internals->x_axis_variable_out =
        this->x_axis_variable.empty() ? x_axis_variable_in : this->x_axis_variable;

    this->internals->y_axis_variable_out =
        this->y_axis_variable.empty() ? y_axis_variable_in : this->y_axis_variable;

    this->internals->z_axis_variable_out =
        this->z_axis_variable.empty() ? z_axis_variable_in : this->z_axis_variable;

    this->internals->coordinates_out.set("x_variable",
        this->internals->x_axis_variable_out);

    this->internals->coordinates_out.set("y_variable",
        this->internals->y_axis_variable_out);
    this->internals->coordinates_out.set("z_variable",
        this->internals->z_axis_variable_out);

    // pass the updated coordinate system
    out_md.set("coordinates", this->internals->coordinates_out);

    // get the input attributes
    teca_metadata atts;
    if (out_md.get("attributes", atts))
    {
        TECA_FATAL_ERROR("Failed to get the coordinate variables attributes")
        this->internals->clear();
        return teca_metadata();
    }

    // get coordinates attributes. some of these might be empty. that is OK.
    atts.get(x_axis_variable_in, this->internals->x_axis_attributes_out);
    atts.get(y_axis_variable_in, this->internals->y_axis_attributes_out);
    atts.get(z_axis_variable_in, this->internals->z_axis_attributes_out);

    // x axis attributes
    // update the description to note the transform
    if (!(tgt_bounds[1] < tgt_bounds[0]))
    {
        std::ostringstream oss;
        oss << x_axis_variable_in << " transformed from ["
            << this->internals->bounds_in[0] << ", " << this->internals->bounds_in[1]
            << "] to [" << this->internals->bounds_out[0] << ", "
            << this->internals->bounds_out[1] << "]";

        this->internals->x_axis_attributes_out.set("description", oss.str());
    }

    // update the units
    if (!this->x_axis_units.empty())
        this->internals->x_axis_attributes_out.set("units", this->x_axis_units);

    // remove the long name
    if (!this->x_axis_variable.empty())
        this->internals->x_axis_attributes_out.remove("long_name");

    // y axis attributes
    // update the description to note the transform
    if (!(tgt_bounds[3] < tgt_bounds[2]))
    {
        std::ostringstream oss;
        oss << y_axis_variable_in << " transformed from ["
            << this->internals->bounds_in[2] << ", " << this->internals->bounds_in[3]
            << "] to [" << this->internals->bounds_out[2] << ", "
            << this->internals->bounds_out[3] << "]";

        this->internals->y_axis_attributes_out.set("description", oss.str());
    }

    // update the units
    if (!this->y_axis_units.empty())
        this->internals->y_axis_attributes_out.set("units", this->y_axis_units);

    // remove the long name
    if (!this->y_axis_variable.empty())
        this->internals->y_axis_attributes_out.remove("long_name");

    // z axis attributes
    // update the description to note the transform
    if (!(tgt_bounds[5] < tgt_bounds[4]))
    {
        std::ostringstream oss;
        oss << z_axis_variable_in << " transformed from ["
            << this->internals->bounds_in[4] << ", " << this->internals->bounds_in[5]
            << "] to [" << this->internals->bounds_out[4] << ", "
            << this->internals->bounds_out[5] << "]";

        this->internals->z_axis_attributes_out.set(
            "description", oss.str());
    }

    // update the units
    if (!this->z_axis_units.empty())
        this->internals->z_axis_attributes_out.set("units", this->z_axis_units);

    // remove the long name
    if (!this->z_axis_variable.empty())
        this->internals->z_axis_attributes_out.remove("long_name");

    // set the output attributes
    atts.set(this->internals->x_axis_variable_out, this->internals->x_axis_attributes_out);
    atts.set(this->internals->y_axis_variable_out, this->internals->y_axis_attributes_out);
    atts.set(this->internals->z_axis_variable_out, this->internals->z_axis_attributes_out);

    // pass the updated attributes
    out_md.set("attributes", atts);

    return out_md;
}

// --------------------------------------------------------------------------
std::vector<teca_metadata>
teca_cartesian_mesh_coordinate_transform::get_upstream_request(
    unsigned int port, const std::vector<teca_metadata> &input_md,
    const teca_metadata &request)
{
    (void)port;
    (void)input_md;

    std::vector<teca_metadata> up_reqs;

    if (request.has("bounds"))
    {
        // get the requested bounds. these are specified in the output
        // coordinate system
        double bounds[6] = {0.0};
        request.get("bounds", bounds);

        // get the input coordinate axes
        const_p_teca_variant_array x_in;
        const_p_teca_variant_array y_in;
        const_p_teca_variant_array z_in;

        if (!(x_in = this->internals->coordinates_in.get("x"))
            || !(y_in = this->internals->coordinates_in.get("y"))
            || !(z_in = this->internals->coordinates_in.get("z")))
        {
            TECA_FATAL_ERROR("The input metadata has invalid coordinates")
            return up_reqs;
        }

        // get the transformed coordinate axes
        p_teca_variant_array x_out;
        p_teca_variant_array y_out;
        p_teca_variant_array z_out;

        if (!(x_out = this->internals->coordinates_out.get("x"))
            || !(y_out = this->internals->coordinates_out.get("y"))
            || !(z_out = this->internals->coordinates_out.get("z")))
        {
            TECA_FATAL_ERROR("The input metadata has invalid coordinates")
            return up_reqs;
        }

        // get the indices of bounding box. these can be used to invert the
        // transform.
        unsigned long extent[6] = {0ul};
        if (teca_coordinate_util::bounds_to_extent(bounds, x_out, y_out, z_out, extent))
        {
            TECA_FATAL_ERROR("The requested bounds " << bounds[0] << ", " << bounds[1]
                << ", " << bounds[2] << ", "  << bounds[3] << ", " << bounds[4]
                << ", " << bounds[5] << "] were not found in the transformed coordinates")
            return up_reqs;
        }

        // using the indices look up the bounding box in the input coordinate system
        double bounds_up[6] = {0.0};
        x_in->get(extent[0], bounds_up[0]);
        x_in->get(extent[1], bounds_up[1]);
        y_in->get(extent[2], bounds_up[2]);
        y_in->get(extent[3], bounds_up[3]);
        z_in->get(extent[4], bounds_up[4]);
        z_in->get(extent[5], bounds_up[5]);

        teca_metadata req(request);
        req.set("bounds", bounds_up, 6);

        up_reqs.push_back(req);
    }
    else
    {
        up_reqs.push_back(request);
    }

    return up_reqs;
}

// --------------------------------------------------------------------------
const_p_teca_dataset teca_cartesian_mesh_coordinate_transform::execute(
    unsigned int port, const std::vector<const_p_teca_dataset> &input_data,
    const teca_metadata &request)
{
#ifdef TECA_DEBUG
    std::cerr << teca_parallel_id()
        << "teca_cartesian_mesh_coordinate_transform::execute" << std::endl;
#endif
    (void)port;
    (void)request;

    p_teca_cartesian_mesh in_target
        = std::dynamic_pointer_cast<teca_cartesian_mesh>(
            std::const_pointer_cast<teca_dataset>(input_data[0]));

    if (!in_target)
    {
        TECA_FATAL_ERROR("invalid input dataset")
        return nullptr;
    }

    // get the transformed coordinate axes
    p_teca_variant_array x_out;
    p_teca_variant_array y_out;
    p_teca_variant_array z_out;

    if (!(x_out = this->internals->coordinates_out.get("x"))
        || !(y_out = this->internals->coordinates_out.get("y"))
        || !(z_out = this->internals->coordinates_out.get("z")))
    {
        TECA_FATAL_ERROR("The cached metadata has invalid coordinates")
        return nullptr;
    }

    // pass input through via shallow copy
    p_teca_cartesian_mesh target = teca_cartesian_mesh::New();
    target->shallow_copy(in_target);

    // get the subset
    unsigned long extent[6] = {0ul};
    in_target->get_extent(extent);

    unsigned long nx = extent[1] - extent[0] + 1;
    unsigned long ny = extent[3] - extent[2] + 1;
    unsigned long nz = extent[5] - extent[4] + 1;

    // get the input cooridnates
    p_teca_variant_array x_in = target->get_x_coordinates();
    p_teca_variant_array y_in = target->get_y_coordinates();
    p_teca_variant_array z_in = target->get_z_coordinates();

    // allocate the subset output coordinates
    p_teca_variant_array x_out_sub = x_in->new_instance(nx);
    p_teca_variant_array y_out_sub = y_in->new_instance(ny);
    p_teca_variant_array z_out_sub = z_in->new_instance(nz);

    // copy the subset
    TEMPLATE_DISPATCH(teca_variant_array_impl,
        x_out.get(),

        NT *p_x_out = dynamic_cast<TT*>(x_out.get())->get();
        NT *p_x_out_sub = dynamic_cast<TT*>(x_out_sub.get())->get();
        for (unsigned long i = 0; i < nx; ++i)
            p_x_out_sub[i] = p_x_out[extent[0] +i];

        NT *p_y_out = dynamic_cast<TT*>(y_out.get())->get();
        NT *p_y_out_sub = dynamic_cast<TT*>(y_out_sub.get())->get();
        for (unsigned long i = 0; i < ny; ++i)
            p_y_out_sub[i] = p_y_out[extent[0] +i];

        NT *p_z_out = dynamic_cast<TT*>(z_out.get())->get();
        NT *p_z_out_sub = dynamic_cast<TT*>(z_out_sub.get())->get();
        for (unsigned long i = 0; i < nz; ++i)
            p_z_out_sub[i] = p_z_out[extent[0] +i];
        )

    // update the mesh
    target->set_x_coordinates(this->internals->x_axis_variable_out, x_out_sub);
    target->set_y_coordinates(this->internals->y_axis_variable_out, y_out_sub);
    target->set_z_coordinates(this->internals->z_axis_variable_out, z_out_sub);

    teca_metadata atts_out;
    target->get_attributes(atts_out);

    atts_out.set(this->internals->x_axis_variable_out, this->internals->x_axis_attributes_out);
    atts_out.set(this->internals->y_axis_variable_out, this->internals->y_axis_attributes_out);
    atts_out.set(this->internals->z_axis_variable_out, this->internals->z_axis_attributes_out);

    target->set_attributes(atts_out);

    return target;
}


#include "solver/linear/gmres.h"

#include <deal.II/lac/petsc_full_matrix.h>
#include <deal.II/lac/petsc_vector.h>

#include "test_helpers/test_helper_functions.h"
#include "test_helpers/gmock_wrapper.h"

namespace  {

namespace solver = bart::solver;
namespace test_helpers = bart::test_helpers;

class SolverLinearGMRESTest : public ::testing::Test {
 protected:
  using FullMatrix = dealii::PETScWrappers::FullMatrix;
  using Vector = dealii::PETScWrappers::MPI::Vector;
  using Preconditioner = dealii::PETScWrappers::PreconditionerBase;
  using GMRES_Solver = solver::linear::GMRES;
  static constexpr int default_max_iterations_{ 100 };
  static constexpr double default_tolerance_{ 1e-10 };
};

TEST_F(SolverLinearGMRESTest, ConstructorDefaultValues) {
  GMRES_Solver solver;
  EXPECT_EQ(solver.max_iterations(), default_max_iterations_);
  EXPECT_EQ(solver.convergence_tolerance(), default_tolerance_);
  EXPECT_EQ(solver.solver_control().max_steps(), default_max_iterations_);
  EXPECT_EQ(solver.solver_control().tolerance(), default_tolerance_);
}

TEST_F(SolverLinearGMRESTest, ConstructorProvidedValues) {
  const int max_iterations{ test_helpers::RandomInt(100, 200) };
  const double tolerance { test_helpers::RandomDouble(1e-10, 1e-6) };

  GMRES_Solver solver(max_iterations, tolerance);
  EXPECT_EQ(solver.max_iterations(), max_iterations);
  EXPECT_EQ(solver.convergence_tolerance(), tolerance);
  EXPECT_EQ(solver.solver_control().max_steps(), max_iterations);
  EXPECT_EQ(solver.solver_control().tolerance(), tolerance);
}

TEST_F(SolverLinearGMRESTest, SolveTestNoPrecon) {

  std::vector<double> b{5,7,8};
  std::vector<double> x{-15, 8, 2};
  std::vector<std::vector<double>> A = {{1, 3, -2}, {3, 5, 6}, {2, 4, 3}};

  std::vector<unsigned int> indices{0,1,2};
  std::vector<double> zeroes(3,0);

  Vector petsc_b(MPI_COMM_WORLD, 3, 3);
  petsc_b.set(indices, b);
  petsc_b.compress(dealii::VectorOperation::insert);
  Vector petsc_x(MPI_COMM_WORLD, 3, 3);
  petsc_x.set(indices, zeroes);
  petsc_x.compress(dealii::VectorOperation::insert);

  FullMatrix petsc_A(3,3);

  for (int i = 0; i < 3; ++i) {
    for (int j = 0; j < 3; ++j) {
      petsc_A.set(i, j, A[i][j]);
    }
  }
  petsc_A.compress(dealii::VectorOperation::insert);

  dealii::PETScWrappers::PreconditionNone no_conditioner(petsc_A);

  GMRES_Solver solver(100, 1e-6);
  solver.Solve(&petsc_A, &petsc_x, &petsc_b, &no_conditioner);

  for (int i = 0; i < 3; ++i) {
    EXPECT_NEAR(petsc_x[i], x[i], 1e-6);
  }
}

} // namespace


///////////////////////////////////////////////////////////////////////////////
// distribution::model::models::likelihood::posterior.hpp                    //
//                                                                           //
//  Copyright 2009 Erwann Rogard. Distributed under the Boost                //
//  Software License, Version 1.0. (See accompanying file                    //
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)         //
///////////////////////////////////////////////////////////////////////////////
#ifndef BOOST_STATISTICS_DETAIL_DISTRIBUTION_MODEL_MODELS_POSTERIOR_POSTERIOR_HPP_ER_2009
#define BOOST_STATISTICS_DETAIL_DISTRIBUTION_MODEL_MODELS_POSTERIOR_POSTERIOR_HPP_ER_2009
#include <boost/statistics/detail/traits/add_cref.hpp>
#include <boost/statistics/detail/mpl/nested_type.hpp>
#include <boost/statistics/detail/distribution_common/meta/value.hpp>
#include <boost/statistics/detail/distribution/model/key/prior.hpp>

namespace boost{
namespace statistics{
namespace detail{
namespace distribution{
namespace model{

    template<typename Pr,typename L>
    class posterior : public L
    {

        typedef Pr prior_type;
        typedef typename
            statistics::detail::traits::add_cref<prior_type>::type cref_p_;

        public:

        typedef typename distribution::meta::value<L>::type value_type;

        posterior(){}

        posterior(cref_p_ p,const L& likelihood):p_(p),L(likelihood){}

        posterior(cref_p_ p,typename L::cref_m_ m,typename L::cref_d_ d)
         : L(m,d),p_(p){}

        template<typename Args>
        posterior(const Args& args) : L(args),p_(args[keyword::prior]){}

        posterior(const posterior& that)
            :   L(static_cast<const L&>(*this)),p_(that.p_){}

        cref_p_ prior()const{ return this->p_; }

        private:
        cref_p_ p_;

    };

}// model
}// distribution
}// detail
}// statistics
}// boost

#endif


#ifndef CONTINGENCY_TABLE_HH__
#define CONTINGENCY_TABLE_HH__

#include "LookupTable.hh"

#include <iosfwd>
#include <ostream>

#include <boost/math/distributions/chi_squared.hpp>

/**
  @class ContingencyTable
  @brief Models a (partially) filled contingency table

  Maintains a (partially) filled contingency table. Each table stores
  its corresponding distance threshold, making it possible to perform
  updates efficiently.
*/

class ContingencyTable
{
public:

  /** Creates an empty (invalid) contingency table */
  ContingencyTable();

  /**
    Creates a new contingency table that is empty. The distance
    threshold as well as the total number of instances of class
    1 are stored.

    The distance threshold is required for subsequent updates.

    The flag `withPseudocounts` changes the behaviour of the class. If set, the
    table will be created such that no cell can have a value of zero. This will
    ensure that the $p$-value is always valid.
  */

  ContingencyTable( unsigned n, unsigned n1, double threshold, bool withPseudocounts );

  /**
    Updates the contingency table by inserting a new element with
    a given distance and a given label. Exactly one entry will be
    changed by this procedure.
  */

  void insert( double distance, bool label );

  /**
    Checks whether two contingency tables are equal. Notice that this
    only involves checking that the values they hold are equal. There
    is no checking of the thresholds involved.
  */
  bool operator==( const ContingencyTable& other ) const noexcept;

  // Marginals ---------------------------------------------------------

  unsigned n()  const noexcept { return _as + _bs + _cs + _ds; }
  unsigned n1() const noexcept;
  unsigned n0() const noexcept;
  unsigned rs() const noexcept;
  unsigned qs() const noexcept;

  // $p$-values --------------------------------------------------------

  /**
    Calculates the $p$-value of a filled contingency table. This
    function does not work if the table is only partially filled
    or its values do not add up to the number of instances.
  */

  long double p() const;

  /**
    Calculates the minimum attainable $p$-value of a filled contingency
    table. This function does not work if the table is not fully filled
    or its values do not add up to the number of instances.
  */

  long double min_attainable_p() const;
  long double min_attainable_p( unsigned rs ) const;

  /**
    Calculates the minimum optimistic $p$-value of an incomplete table,
    i.e. the minimum $p$-value that can be attained if every remaining
    element is classified correctly.
  */

  long double min_optimistic_p() const;

  // Attributes --------------------------------------------------------

  unsigned as() const noexcept { return _as; }
  unsigned bs() const noexcept { return _bs; }
  unsigned cs() const noexcept { return _cs; }
  unsigned ds() const noexcept { return _ds; }

  double threshold() const noexcept { return _threshold; }

  /**
    Checks whether the table is *complete*, i.e. whether the marginals
    sum up to the expected values.
  */

  bool complete() const noexcept;

private:

  /**
    Calculates the Chi--squared test statistic of the given table
    and returns it. This is required for $p$-value calculation.
  */

  long double t() const;

  static boost::math::chi_squared_distribution<long double> _chi2;
  static LookupTable _lookupTable;

  unsigned _n;  //< Total number of items (fixed)
  unsigned _n1; //< Total number of items with class 1 (fixed)
  unsigned _n0; //< Total number of items with class 0 (fixed)

  // Entries in the actual contingency table. We follow common
  // terminology here, so that the table looks like this:
  //
  // ----------------
  // | as | bs | n1 |
  // |----|----|----|
  // | ds | cs | n0 |
  // |----|----|----|
  // | rs | qs | n  |
  // ----------------
  //
  // The marginals rs and qs are determined automatically upon
  // updating the table.
  unsigned _as;
  unsigned _bs;
  unsigned _cs;
  unsigned _ds;

  // Distance threshold for which this table was created. This value
  // decides where new entries will be added.
  double _threshold;
};

/**
  Output operator for a contingency table. The table will be reported as
  a list of numbers, i.e. `a, b, d, c`. This corresponds to the order of
  columns in the table.
*/

std::ostream& operator<<( std::ostream& o, const ContingencyTable& C );

#endif


#ifndef IMAGE_HPP
#define IMAGE_HPP

#include <cassert>
#include <iostream>
#include <fstream>
#include <vector>
#include <algorithm>
#include <limits>
#include <cmath>

#include <boost/random.hpp>

#include <graphlab.hpp>

#include <graphlab/macros_def.hpp>

/** A simple struct represent a gray scale image */
class image {
  size_t _rows, _cols;
  std::vector<double> data;
public:

  /** Create an empty image */
  image() : _rows(0), _cols(0), data(0,0) { }

  /** Create an image of a fixed size */
  image(size_t rows, size_t cols) :
    _rows(rows), _cols(cols), data(rows * cols, 0) { }

  inline void resize(size_t rows, size_t cols);

  /** Get the number of rows */
  inline size_t rows() const { return _rows; }

  /** Get the number of columns */
  inline size_t cols() const { return _cols; }

  /** get the number of pixels */
  inline size_t pixels() const { return _rows * _cols; }

  /** A function to read a pixel */
  inline double& pixel(size_t i, size_t j) { return data[vertid(i,j)]; }
  inline double pixel(size_t i, size_t j) const { return data[vertid(i,j)]; }

  /** Linear indexing */
  inline double& pixel(size_t i) { return data.at(i); }
  inline double pixel(size_t i) const { return data.at(i); }

  /** Get the vertex id of a pixel */
  inline size_t vertid(size_t i, size_t j) const;
  inline static size_t vertid(size_t rows, size_t cols, size_t i, size_t j) {
    assert(i < rows);
    assert(j < cols);
    return i * cols + j;
  }

  /** Get the pixel address from the vertex id */
  std::pair<size_t, size_t> loc(size_t vertex) const;

  /** A function to save the image to a file in pgm format */
  inline void save(const char* filename, bool autorescale = true, double min=0, double max=255) const;

  inline void save_vec(const char* filename) const {
    std::ofstream os(filename);
    assert(os.good());
    for(size_t i = 0; i < pixels(); ++i) {
      os << pixel(i) << "\n";
    }
    os.flush();
    os.close();
  }

  /** paint a beautiful sunset */
  void paint_sunset(size_t num_rings);

  /** Add random noise to the image */
  void corrupt(double sigma);

  inline double min() {
    return *std::min_element(data.begin(), data.end());
  }

  inline double max() {
    return *std::max_element(data.begin(), data.end());
  }

  inline void save(graphlab::oarchive &oarc) const {
    oarc << _rows;
    oarc << _cols;
    oarc << data;
  }

  inline void load(graphlab::iarchive &iarc) {
    iarc >> _rows;
    iarc >> _cols;
    iarc >> data;
  }

};

/** Generate a normally distributed random number N(mu, sigma^2) */
// std::pair<double, double> randn(double mu = 0, double sigma = 1 );

// IMPLEMENTATION =============================================================>

inline void image::resize(size_t rows, size_t cols) {
  _rows = rows;
  _cols = cols;
  data.resize(rows * cols, 0);
}

/** Get the vertex id of a pixel */
inline size_t image::vertid(size_t i, size_t j) const {
  assert(i < _rows);
  assert(j < _cols);
  return i * _cols + j;
}

// static size_t image::vertid(size_t rows, size_t cols, size_t i, size_t j)  {
//   assert(i < rows);
//   assert(j < cols);
//   return i * cols + j;
// }

/** Get the vertex id of a pixel */
inline std::pair<size_t, size_t> image::loc(size_t vertexid) const {
  assert(vertexid < _rows * _cols);
  return std::make_pair( vertexid / _cols, vertexid % _cols);
}

inline void image::save(const char* filename, bool autorescale, double min_, double max_) const {
  assert(_rows > 0 && _cols > 0);
  std::ofstream os(filename);
  os << "P2" << std::endl
     << _cols << " " << _rows << std::endl
     << 255 << std::endl;
  // Compute min and max pixel intensities
  double min = data[0]; double max = data[0];
  if (autorescale) {
    for(size_t i = 0; i < _rows * _cols; ++i) {
      min = std::min(min, data[i]);
      max = std::max(max, data[i]);
    }
  }
  else {
    min = min_;
    max = max_;
  }
  // Save the image (rescaled)
  for(size_t r = 0; r < _rows; ++r) {
    for(size_t c = 0; c < _cols; c++) {
      if(min != max) {
        int color =
          static_cast<int>(255.0 * (pixel(r,c) - min)/(max-min));
        if (color < 0) color = 0;
        if (color > 255) color = 255;
        os << color;
      } else { os << min; }
      if(c != _cols-1) os << "\t";
    }
    os << std::endl;
  }
  os.flush();
  os.close();
} // end of save

inline void image::paint_sunset(size_t num_rings) {
  const double center_r = rows() / 2.0;
  const double center_c = cols() / 2.0;
  const double max_radius = std::min(rows(), cols()) / 2.0;
  // Fill out the image
  for(size_t r = 0; r < rows(); ++r) {
    for(size_t c = 0; c < cols(); ++c) {
      double distance = sqrt((r-center_r)*(r-center_r) +
                             (c-center_c)*(c-center_c));
      // If on top of image
      if(r < rows() / 2) {
        // Compute ring of sunset
        size_t ring =
          static_cast<size_t>(std::floor(std::min(1.0, distance/max_radius)
                                         * (num_rings - 1) ) );
        pixel(r,c) = ring;
      } else {
        pixel(r,c) = 0;
      }
    }
  }
} // end of paint_beatiful_sunset

/** corrupt the image with gaussian noise */
inline void image::corrupt(double sigma) {
  //  boost::mt19937 rng;
  boost::lagged_fibonacci607 rng;
  boost::normal_distribution<double> noise_model(0, sigma);
  for(size_t i = 0; i < rows() * cols();  ) {
    // Corrupt two pixels at a time.
    pixel(i++) += noise_model(rng);
  }
} // end of corrupt_image

// /** generate a normally distributed iid pair */
// std::pair<double, double> randn(double mu , double sigma ) {
//   // Generate a N(0,1) from a Unif(0,1) using Box-Muller generator:
//   double u1 = static_cast<double>(rand()) / RAND_MAX;
//   double u2 = static_cast<double>(rand()) / RAND_MAX;
//   double coeff = std::sqrt(-2.0 * std::log(u1));
//   double n1 = coeff * std::cos(2.0 * M_PI * u2) ;
//   double n2 = coeff * std::sin(2.0 * M_PI * u2) ;
//   // Adjust for mean and variance
//   n1 = sigma * n1 + mu;
//   n2 = sigma * n2 + mu;
//   return std::make_pair(n1, n2);
// } // end of randn

#include <graphlab/macros_undef.hpp>

#endif


// Copyright (c) 2017 Franka Emika GmbH
// Use of this source code is governed by the Apache-2.0 license, see LICENSE
#include "examples_common.h"

#include <franka/exception.h>
#include <franka/robot.h>
#include <Eigen/Dense>
#include <algorithm>
#include <array>
#include <cmath>
#include <functional>
#include <iostream>
#include <string>
#include<cstdio>
#include <vector>
#include <array>
#include<sstream>

std::array<double, 7> split_by_space(std::string str) {
    std::stringstream input(str);
    std::array<double, 7> res{};
    std::string result;
    int ii = 0;
    while (input >> result || ii < 7) {
        res[ii++] = stod(result);
    }
    return res;
}

double randomm(int begin, int end){
    srand((int) time(0));//用系统时间作为随机种子
    int res=rand() % (end-begin+1)+ begin;
    double aa=(double)res/100;
    return aa;
}
Eigen::Matrix<double,7,1> randomm_matrix(int begin, int end){
    std::array<double,7>arr;
    for (size_t i = 0; i < 7; i++)
    arr[i]=randomm(begin,end);
    Eigen::Map<const Eigen::Matrix<double, 7, 1>>result(arr.data());
    return result;
}

std::vector<std::array<double, 7>> read_joint_position_from_file(const char *filename) {
    freopen(filename, "r", stdin);
    std:: vector<std::array<double, 7>> res;
    std::string line;
    while (getline(std::cin, line)) {
                 res.push_back(split_by_space(line));
    }
    return res;
}

void print_position(std::array<double, 16> initial_pose) {
  std::cout << "this is current position" << std::endl;
  for (int i = 0; i < 16; i++) {
    std::cout << initial_pose[i] << "  ";
    if ((i + 1) % 4 == 0)
      std::cout << std::endl;
  }
}

Eigen::MatrixXd pseudoinverse(Eigen::MatrixXd m) {
  // Eigen::Matrix<float,2,3> m;
  //  m<<0.68,0.597,-0.211, 0.823,0.566,-0.605;
  // m<<1,2,3,4,5,6;
  Eigen::JacobiSVD<Eigen::MatrixXd> svd = m.jacobiSvd(Eigen::ComputeFullU | Eigen::ComputeFullV);
  // Eigen::JacobiSVD<Eigen::MatrixXf> svd(m, Eigen::ComputeThinU | Eigen::ComputeThinV);
  const Eigen::MatrixXd singularValues = svd.singularValues();
  Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> singularValuesInv(m.cols(), m.rows());
  singularValuesInv.setZero();
  double pinvtoler = 1.e-6;  // choose your tolerance wisely
  for (unsigned int i = 0; i < singularValues.size(); ++i) {
    if (singularValues(i) > pinvtoler)
      singularValuesInv(i, i) = 1.0f / singularValues(i);
    else
      singularValuesInv(i, i) = 0.f;
  }
  Eigen::MatrixXd pinvmat = svd.matrixV() * singularValuesInv * svd.matrixU().transpose();
  return pinvmat;
}

void setDefaultBehavior(franka::Robot& robot) {
  robot.setCollisionBehavior(
      {{20.0, 20.0, 20.0, 20.0, 20.0, 20.0, 20.0}}, {{20.0, 20.0, 20.0, 20.0, 20.0, 20.0, 20.0}},
      {{10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0}}, {{10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0}},
      {{20.0, 20.0, 20.0, 20.0, 20.0, 20.0}}, {{20.0, 20.0, 20.0, 20.0, 20.0, 20.0}},
      {{10.0, 10.0, 10.0, 10.0, 10.0, 10.0}}, {{10.0, 10.0, 10.0, 10.0, 10.0, 10.0}});
  robot.setJointImpedance({{3000, 3000, 3000, 2500, 2500, 2000, 2000}});
  robot.setCartesianImpedance({{3000, 3000, 3000, 300, 300, 300}});
}

MotionGenerator::MotionGenerator(double speed_factor, const std::array<double, 7> q_goal)
    : q_goal_(q_goal.data()) {
  dq_max_ *= speed_factor;
  ddq_max_start_ *= speed_factor;
  ddq_max_goal_ *= speed_factor;
  dq_max_sync_.setZero();
  q_start_.setZero();
  delta_q_.setZero();
  t_1_sync_.setZero();
  t_2_sync_.setZero();
  t_f_sync_.setZero();
  q_1_.setZero();
}

bool MotionGenerator::calculateDesiredValues(double t, Vector7d* delta_q_d) const {
  Vector7i sign_delta_q;
  sign_delta_q << delta_q_.cwiseSign().cast<int>();
  Vector7d t_d = t_2_sync_ - t_1_sync_;
  Vector7d delta_t_2_sync = t_f_sync_ - t_2_sync_;
  std::array<bool, 7> joint_motion_finished{};

  for (size_t i = 0; i < 7; i++) {
    if (std::abs(delta_q_[i]) < kDeltaQMotionFinished) {
      (*delta_q_d)[i] = 0;
      joint_motion_finished[i] = true;
    } else {
      if (t < t_1_sync_[i]) {
        (*delta_q_d)[i] = -1.0 / std::pow(t_1_sync_[i], 3.0) * dq_max_sync_[i] * sign_delta_q[i] *
                          (0.5 * t - t_1_sync_[i]) * std::pow(t, 3.0);
      } else if (t >= t_1_sync_[i] && t < t_2_sync_[i]) {
        (*delta_q_d)[i] = q_1_[i] + (t - t_1_sync_[i]) * dq_max_sync_[i] * sign_delta_q[i];
      } else if (t >= t_2_sync_[i] && t < t_f_sync_[i]) {
        (*delta_q_d)[i] =
            delta_q_[i] + 0.5 *
                              (1.0 / std::pow(delta_t_2_sync[i], 3.0) *
                                   (t - t_1_sync_[i] - 2.0 * delta_t_2_sync[i] - t_d[i]) *
                                   std::pow((t - t_1_sync_[i] - t_d[i]), 3.0) +
                               (2.0 * t - 2.0 * t_1_sync_[i] - delta_t_2_sync[i] - 2.0 * t_d[i])) *
                              dq_max_sync_[i] * sign_delta_q[i];
      } else {
        (*delta_q_d)[i] = delta_q_[i];
        joint_motion_finished[i] = true;
      }
    }
  }
  return std::all_of(joint_motion_finished.cbegin(), joint_motion_finished.cend(),
                     [](bool x) { return x; });
}

void MotionGenerator::calculateSynchronizedValues() {
  Vector7d dq_max_reach(dq_max_);
  Vector7d t_f = Vector7d::Zero();
  Vector7d delta_t_2 = Vector7d::Zero();
  Vector7d t_1 = Vector7d::Zero();
  Vector7d delta_t_2_sync = Vector7d::Zero();
  Vector7i sign_delta_q;
  sign_delta_q << delta_q_.cwiseSign().cast<int>();

  for (size_t i = 0; i < 7; i++) {
    if (std::abs(delta_q_[i]) > kDeltaQMotionFinished) {
      if (std::abs(delta_q_[i]) < (3.0 / 4.0 * (std::pow(dq_max_[i], 2.0) / ddq_max_start_[i]) +
                                   3.0 / 4.0 * (std::pow(dq_max_[i], 2.0) / ddq_max_goal_[i]))) {
        dq_max_reach[i] = std::sqrt(4.0 / 3.0 * delta_q_[i] * sign_delta_q[i] *
                                    (ddq_max_start_[i] * ddq_max_goal_[i]) /
                                    (ddq_max_start_[i] + ddq_max_goal_[i]));
      }
      t_1[i] = 1.5 * dq_max_reach[i] / ddq_max_start_[i];
      delta_t_2[i] = 1.5 * dq_max_reach[i] / ddq_max_goal_[i];
      t_f[i] = t_1[i] / 2.0 + delta_t_2[i] / 2.0 + std::abs(delta_q_[i]) / dq_max_reach[i];
    }
  }
  double max_t_f = t_f.maxCoeff();
  for (size_t i = 0; i < 7; i++) {
    if (std::abs(delta_q_[i]) > kDeltaQMotionFinished) {
      double a = 1.5 / 2.0 * (ddq_max_goal_[i] + ddq_max_start_[i]);
      double b = -1.0 * max_t_f * ddq_max_goal_[i] * ddq_max_start_[i];
      double c = std::abs(delta_q_[i]) * ddq_max_goal_[i] * ddq_max_start_[i];
      double delta = b * b - 4.0 * a * c;
      if (delta < 0.0) {
        delta = 0.0;
      }
      dq_max_sync_[i] = (-1.0 * b - std::sqrt(delta)) / (2.0 * a);
      t_1_sync_[i] = 1.5 * dq_max_sync_[i] / ddq_max_start_[i];
      delta_t_2_sync[i] = 1.5 * dq_max_sync_[i] / ddq_max_goal_[i];
      t_f_sync_[i] =
          (t_1_sync_)[i] / 2.0 + delta_t_2_sync[i] / 2.0 + std::abs(delta_q_[i] / dq_max_sync_[i]);
      t_2_sync_[i] = (t_f_sync_)[i] - delta_t_2_sync[i];
      q_1_[i] = (dq_max_sync_)[i] * sign_delta_q[i] * (0.5 * (t_1_sync_)[i]);
    }
  }
}

franka::JointPositions MotionGenerator::operator()(const franka::RobotState& robot_state,
                                                   franka::Duration period) {
  time_ += period.toSec();

  if (time_ == 0.0) {
    q_start_ = Vector7d(robot_state.q_d.data());
    delta_q_ = q_goal_ - q_start_;
    calculateSynchronizedValues();
  }

  Vector7d delta_q_d;
  bool motion_finished = calculateDesiredValues(time_, &delta_q_d);

  std::array<double, 7> joint_positions;
  Eigen::VectorXd::Map(&joint_positions[0], 7) = (q_start_ + delta_q_d);
  franka::JointPositions output(joint_positions);
  output.motion_finished = motion_finished;
  return output;
}


// This file is part of Eigen, a lightweight C++ template library
// for linear algebra.
//
// Copyright (C) 2012-2016 Gael Guennebaud <gael.guennebaud@inria.fr>
//
// This Source Code Form is subject to the terms of the Mozilla
// Public License v. 2.0. If a copy of the MPL was not distributed
// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.

#define EIGEN_RUNTIME_NO_MALLOC
#include "main.h"
#include <limits>
#include <Eigen/Eigenvalues>
#include <Eigen/LU>

template<typename MatrixType> void generalized_eigensolver_real(const MatrixType& m)
{
  typedef typename MatrixType::Index Index;
  /* this test covers the following files:
     GeneralizedEigenSolver.h
  */
  Index rows = m.rows();
  Index cols = m.cols();

  typedef typename MatrixType::Scalar Scalar;
  typedef std::complex<Scalar> ComplexScalar;
  typedef Matrix<Scalar, MatrixType::RowsAtCompileTime, 1> VectorType;

  MatrixType a = MatrixType::Random(rows,cols);
  MatrixType b = MatrixType::Random(rows,cols);
  MatrixType a1 = MatrixType::Random(rows,cols);
  MatrixType b1 = MatrixType::Random(rows,cols);
  MatrixType spdA =  a.adjoint() * a + a1.adjoint() * a1;
  MatrixType spdB =  b.adjoint() * b + b1.adjoint() * b1;

  // lets compare to GeneralizedSelfAdjointEigenSolver
  {
    GeneralizedSelfAdjointEigenSolver<MatrixType> symmEig(spdA, spdB);
    GeneralizedEigenSolver<MatrixType> eig(spdA, spdB);

    VERIFY_IS_EQUAL(eig.eigenvalues().imag().cwiseAbs().maxCoeff(), 0);

    VectorType realEigenvalues = eig.eigenvalues().real();
    std::sort(realEigenvalues.data(), realEigenvalues.data()+realEigenvalues.size());
    VERIFY_IS_APPROX(realEigenvalues, symmEig.eigenvalues());

    // check eigenvectors
    typename GeneralizedEigenSolver<MatrixType>::EigenvectorsType D = eig.eigenvalues().asDiagonal();
    typename GeneralizedEigenSolver<MatrixType>::EigenvectorsType V = eig.eigenvectors();
    VERIFY_IS_APPROX(spdA*V, spdB*V*D);
  }

  // non symmetric case:
  {
    GeneralizedEigenSolver<MatrixType> eig(rows);
    // TODO enable full-prealocation of required memory, this probably requires an in-place mode for HessenbergDecomposition
    //Eigen::internal::set_is_malloc_allowed(false);
    eig.compute(a,b);
    //Eigen::internal::set_is_malloc_allowed(true);
    for(Index k=0; k<cols; ++k)
    {
      Matrix<ComplexScalar,Dynamic,Dynamic> tmp = (eig.betas()(k)*a).template cast<ComplexScalar>() - eig.alphas()(k)*b;
      if(tmp.size()>1 && tmp.norm()>(std::numeric_limits<Scalar>::min)())
        tmp /= tmp.norm();
      VERIFY_IS_MUCH_SMALLER_THAN( std::abs(tmp.determinant()), Scalar(1) );
    }
    // check eigenvectors
    typename GeneralizedEigenSolver<MatrixType>::EigenvectorsType D = eig.eigenvalues().asDiagonal();
    typename GeneralizedEigenSolver<MatrixType>::EigenvectorsType V = eig.eigenvectors();
    VERIFY_IS_APPROX(a*V, b*V*D);
  }

  // regression test for bug 1098
  {
    GeneralizedSelfAdjointEigenSolver<MatrixType> eig1(a.adjoint() * a,b.adjoint() * b);
    eig1.compute(a.adjoint() * a,b.adjoint() * b);
    GeneralizedEigenSolver<MatrixType> eig2(a.adjoint() * a,b.adjoint() * b);
    eig2.compute(a.adjoint() * a,b.adjoint() * b);
  }
}

void test_eigensolver_generalized_real()
{
  for(int i = 0; i < g_repeat; i++) {
    int s = 0;
    CALL_SUBTEST_1( generalized_eigensolver_real(Matrix4f()) );
    s = internal::random<int>(1,EIGEN_TEST_MAX_SIZE/4);
    CALL_SUBTEST_2( generalized_eigensolver_real(MatrixXd(s,s)) );

    // some trivial but implementation-wise special cases
    CALL_SUBTEST_2( generalized_eigensolver_real(MatrixXd(1,1)) );
    CALL_SUBTEST_2( generalized_eigensolver_real(MatrixXd(2,2)) );
    CALL_SUBTEST_3( generalized_eigensolver_real(Matrix<double,1,1>()) );
    CALL_SUBTEST_4( generalized_eigensolver_real(Matrix2d()) );
    TEST_SET_BUT_UNUSED_VARIABLE(s)
  }
}


//
// Created by dchansen on 9/19/18.
//

#include <cpu/hoNDArray_utils.h>
#include "hoNDFFT.h"
#include "mri_core_girf_correction.h"
#include "hoNDArray_utils.h"
#include "hoNDArray_math.h"
#include <boost/filesystem.hpp>
#include <cpu/hoNDArray_fileio.h>
#include <boost/math/constants/constants.hpp>

#include <boost/config/warning_disable.hpp>
#include <boost/spirit/include/qi.hpp>
#include <boost/spirit/include/phoenix_core.hpp>
#include <boost/spirit/include/phoenix_operator.hpp>
#include <boost/spirit/include/phoenix_stl.hpp>
#include <boost/fusion/include/adapt_adt.hpp>

using namespace std::complex_literals;

BOOST_FUSION_ADAPT_ADT(
            std::complex<float>,
    (float, float, obj.real(),obj.real(val))
    (float,float,obj.imag(),obj.imag(val))
)
namespace Gadgetron {
    namespace {
        constexpr float PI = boost::math::constants::pi<float>();
        template<unsigned int D>
        hoNDArray<vector_td<float,D>> girf_correct_D(const hoNDArray<vector_td<float,D>> &gradients,
                                        const hoNDArray<std::complex<float>> &girf_kernel,
                                        const arma::fmat33 &rotation_matrix, float gradient_sampling_time,
                                        float girf_sampling_time, float TE) {

            size_t nlines = gradients.get_size(0);
//            size_t nlines_new = size_t(nlines * girf_sampling_time / gradient_sampling_time);
            size_t nlines_new = girf_kernel.get_size(0);

            size_t nbatches = gradients.get_size(1);

            hoNDArray<vector_td<float,D>> result(gradients.dimensions());

            for (int batch = 0; batch < nbatches; batch++) {

                hoNDArray<std::complex<float>> rotated(nlines_new, 3);
                std::fill(rotated.begin(), rotated.end(), 0);

                for (size_t i = 0; i < nlines; i++) {
                    arma::fvec3 gradient_vector(arma::fill::zeros);
                    for (size_t k = 0; k < D; k++)
                        gradient_vector[k] = gradients(i,batch)[k];

                    gradient_vector = rotation_matrix * gradient_vector;
                    for (size_t k = 0; k < 3; k++)
                        rotated(i, k) = gradient_vector[k];
                }

                hoNDFFT<float>::instance()->fft1(rotated);
                hoNDFFT<float>::instance()->fftshift1D(rotated);
//            auto padded_kernel = std::move(*pad<std::complex<float>,1>(uint64d1(nlines_new), &girf_kernel));

//            rotated *= padded_kernel;
            rotated *= girf_kernel;
            float ADCShift = -TE-0.5*gradient_sampling_time;
                float frequency_resolution = 1.0 / (girf_sampling_time * nlines_new);
            for (size_t k = 0; k < 3; k++) {
                for (size_t i = 0; i < nlines_new; i++){
                    rotated(i,k) *= std::exp(-2if*ADCShift*PI*frequency_resolution*float(i));
                }
            }

                size_t full_domain = size_t(std::round(1.0 / (2 * frequency_resolution * gradient_sampling_time))) * 2;

//                rotated = *pad<std::complex<float>, 1>(uint64d1(full_domain), &rotated);

                hoNDFFT<float>::instance()->ifftshift1D(rotated);
                hoNDFFT<float>::instance()->ifft1(rotated);

//            hoNDFFT<float>::instance()->ifft(&rotated,0);

                arma::fmat33 inv_rotation = arma::inv(rotation_matrix);

                for (size_t i = 0; i < nlines; i++) {
                    // arma::cx_vec3 gradient_vector = {rotated(i, 0), rotated(i, 1), rotated(i, 2)};
                    arma::cx_vec3 gradient_vector;
                    gradient_vector[0] = rotated(i, 0);
                    gradient_vector[1] = rotated(i, 1);
                    gradient_vector[2] = rotated(i, 2);
                    gradient_vector = inv_rotation * gradient_vector;
                    for (size_t k = 0; k < D; k++) {
                        result(i,batch)[k] = std::copysign(std::abs(gradient_vector[k]), std::real(gradient_vector[k]));
                    }
                };
            }

            return result;
        }
    }

    namespace {
        template<typename Iterator>
        bool parse_girf_text(Iterator first, Iterator last, std::vector<std::vector<std::complex<float>>> &v) {
            using namespace boost::spirit::qi;
            namespace qi = boost::spirit::qi;
            using boost::spirit::ascii::space;

            rule<Iterator, std::complex<float>(), ascii::space_type> base = float_ >> ((float_ >> (lit('i') | lit('j'))) | attr(0.0f));
            decltype(base) complex = (lit('(') >> base >> lit(')')) | base;
            rule<Iterator, std::vector<std::complex<float>>(), ascii::space_type> row = (lit('[') >> complex % ','
                                                                                >> lit(']'));
            bool r = phrase_parse(first, last,
                                  lit('[') >> row >> qi::repeat(2)[lit(',') >> row] >> lit(']'),
                                  space, v);
            if (!r || first != last) // fail if we did not get a full match
                return false;
            return r;
        }
    }

    hoNDArray<std::complex<float>> GIRF::load_girf_kernel(const std::string& girf_string) {
        std::vector<std::vector<std::complex<float>>> temp_array;

        bool success = parse_girf_text(girf_string.begin(),girf_string.end(),temp_array);
        if (!success) throw std::runtime_error("Failed to parse girf string");

        hoNDArray<std::complex<float>> result(temp_array[0].size(),temp_array.size());
        auto data_ptr = result.get_data_ptr();
        for (auto& v: temp_array)
            for (auto c: v)
                *(data_ptr++) = c;

        return result;

    }

    hoNDArray<floatd3>
    GIRF::girf_correct(const hoNDArray<floatd3> &gradients, const hoNDArray<std::complex<float>> &girf_kernel,
                       const arma::fmat33 &rotation_matrix, float gradient_sampling_time, float girf_sampling_time,
                       float TE) {
        return girf_correct_D(gradients,girf_kernel,rotation_matrix,girf_sampling_time,girf_sampling_time,TE);
    }

    hoNDArray<floatd2>
    GIRF::girf_correct(const hoNDArray<floatd2> &gradients, const hoNDArray<std::complex<float>> &girf_kernel,
                       const arma::fmat33 &rotation_matrix, float gradient_sampling_time, float girf_sampling_time,
                       float TE) {
        return girf_correct_D(gradients,girf_kernel,rotation_matrix,girf_sampling_time,girf_sampling_time,TE);
    }

}


// Filename: insert_class_expensive.cpp

#include <iostream>
#include <boost/numeric/mtl/mtl.hpp>

using namespace mtl;

class world_matrix
{
  public:
    world_matrix(unsigned nrows, unsigned ncols) : A(nrows, ncols) {}

    void add_entry(unsigned row, unsigned col, double value)
    {
	// Extremely expensive -> must not be done
	mat::inserter<compressed2D<double>, update_plus<double> > ins(A);
	ins[row][col] << value;
    }

    friend inline std::ostream& operator<<(std::ostream& os, const world_matrix& w)
    { return os << w.A; }

  private:
    compressed2D<double>              A;
};

int main(int, char**)
{
    world_matrix              A(3, 3);

    A.add_entry(0, 0, 2.0);
    A.add_entry(1, 2, 0.5);
    A.add_entry(2, 1, 3.0);

    std::cout << "A is \n" << A;  // we can access A now

    return 0;
}


/*
@copyright Louis Dionne 2014
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
 */

#include <boost/hana/set.hpp>

#include <boost/hana/comparable/laws.hpp>
#include <boost/hana/detail/assert.hpp>
#include <boost/hana/integral.hpp>
#include <boost/hana/list/instance.hpp>
using namespace boost::hana;

template <int i>
constexpr auto x = int_<i>;

int main() {
    BOOST_HANA_CONSTANT_ASSERT(Comparable::laws::check(
        list(
            set(),
            set(x<0>),
            set(x<0>, x<1>),
            set(x<1>, x<0>),
            set(x<0>, x<1>, x<2>)
        )
    ));
}


#include <frovedis.hpp>
#include <frovedis/matrix/crs_matrix.hpp>
#include <frovedis/ml/nb/naive_bayes.hpp>
#include <boost/program_options.hpp>

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

template <class T, class MATRIX>
void do_nb_train(MATRIX& mat,
                 dvector<T>& lbl,
                 const std::string& output,
                 const std::string& model_type,
                 double lambda) {
  naive_bayes_model<T> model;
  time_spent train_t(INFO);
  train_t.lap_start();
  if(model_type == "multinomial")
    model = multinomial_nb(mat, lbl, lambda);
  else if(model_type == "bernoulli")
    model = bernoulli_nb(mat, lbl, lambda);
  else REPORT_ERROR(USER_ERROR, "Currently frovedis supports only multinomial and bernoulli naive bayes!\n");
  train_t.lap_stop();
  train_t.show_lap("total train time: ");

  model.save(output);
}

template <class T, class MATRIX>
void do_nb_predict(MATRIX& mat,
                   const std::string& modelfile,
                   const std::string& output,
                   bool is_prob) {
  time_spent pred_t(INFO);
  naive_bayes_model<T> model;
  model.load(modelfile);

  pred_t.lap_start();
  auto plbl =  is_prob? model.predict_probability(mat) : model.predict(mat);
  pred_t.lap_stop();
  pred_t.show_lap("total prediction time: ");

  make_dvector_scatter(plbl).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")
    ("predict,p", "predict mode")
    ("predict-probability,y", "probability-predict mode")
    ("input,i", value<string>(), "input matrix")
    ("label,l", value<string>(), "input label (for train)")
    ("model,m", value<string>(), "input model (for predict)")
    ("output,o", value<string>(), "output model or predict result")
    ("lambda,s", value<double>(), "smoothing parameter (0 for no smoothing) (default: 1.0) ")
    ("model_type,t", value<string>(), "target naive bayes model type (bernoulli or multinomial) (default: multinomial)")
    ("dtype", value<string>(), "target data type for input (float or double) (default: double)")
    ("mtype", value<string>(), "target matrix type for input (dense or sparse) (default: sparse)")
    ("verbose", "set loglevel to DEBUG")
    ("verbose2", "set loglevel to TRACE");

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

  bool ispredict = false;
  bool predict_probability = false;
  string input, label, model, output;
  string dtype = "double";
  string mtype = "sparse";
  string model_type = "multinomial";
  double lambda = 1.0;

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

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

  if(argmap.count("predict-probability")){
    ispredict = true;
    predict_probability = true;
  }

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

  if(ispredict) {
    if(argmap.count("model")){
      model = argmap["model"].as<string>();
    } else {
      cerr << "model file is not specified" << endl;
      cerr << opt << endl;
      exit(1);
    }
  }
  else {
    if(argmap.count("label")){
      label = argmap["label"].as<string>();
    } else {
      cerr << "label file 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("lambda")){
    lambda = argmap["lambda"].as<double>();
  }

  if(argmap.count("dtype")){
    dtype = argmap["dtype"].as<string>();
  }

  if(argmap.count("mtype")){
    mtype = argmap["mtype"].as<string>();
  }

  if(argmap.count("model_type")){
    model_type = argmap["model_type"].as<string>();
  }

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

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

  try {
    if(ispredict) {
      if(mtype == "dense") {
        if(dtype == "float") {
          auto mat = make_rowmajor_matrix_local_load<float>(input);
          do_nb_predict<float>(mat, model, output, predict_probability);
        }
        else if(dtype == "double") {
          auto mat = make_rowmajor_matrix_local_load<double>(input);
          do_nb_predict<double>(mat, model, output, predict_probability);
        }
        else REPORT_ERROR(USER_ERROR, "supported dtypes are float or double!\n");
      }
      else if(mtype == "sparse") {
        if(dtype == "float") {
          auto mat = make_crs_matrix_local_load<float>(input);
          do_nb_predict<float>(mat, model, output, predict_probability);
        }
        else if(dtype == "double") {
          auto mat = make_crs_matrix_local_load<double>(input);
          do_nb_predict<double>(mat, model, output, predict_probability);
        }
        else REPORT_ERROR(USER_ERROR, "supported dtypes are float or double!\n");
      }
      else REPORT_ERROR(USER_ERROR, "supported mtypes are dense or sparse!\n");
    }
    else {
      if(mtype == "dense") {
        if(dtype == "float") {
          auto mat = make_rowmajor_matrix_load<float>(input);
          auto lbl = make_dvector_loadline<float>(label);
          do_nb_train(mat, lbl, output, model_type, lambda);
        }
        else if(dtype == "double") {
          auto mat = make_rowmajor_matrix_load<double>(input);
          auto lbl = make_dvector_loadline<double>(label);
          do_nb_train(mat, lbl, output, model_type, lambda);
        }
        else REPORT_ERROR(USER_ERROR, "supported dtypes are float or double!\n");
      }
      else if(mtype == "sparse") {
        if (dtype == "float") {
          auto mat = make_crs_matrix_load<float>(input);
          auto lbl = make_dvector_loadline<float>(label);
          do_nb_train(mat, lbl, output, model_type, lambda);
        }
        else if(dtype == "double") {
          auto mat = make_crs_matrix_load<double>(input);
          auto lbl = make_dvector_loadline<double>(label);
          do_nb_train(mat, lbl, output, model_type, lambda);
        }
        else REPORT_ERROR(USER_ERROR, "supported dtypes are float or double!\n");
      }
      else REPORT_ERROR(USER_ERROR, "supported mtypes are dense or sparse!\n");
    }
  }
  catch (std::exception& e) {
    std::cout << "exception caught: " << e.what() << std::endl;
  }
  return 0;
}


#include <ctime>
#include <cstdlib>
#include <string>
#include <iostream>
#include <fstream>
#include <Eigen/Dense>
#include <Eigen/Geometry>

#include "collision_detect.h"

using namespace COLLISION_DETECTION;

int main(int argc, char *argv[])
{
  std::string filename1("../meshes/BIG_SCREW.off");
  std::string filename2("../meshes/BIG_SCREW.off");

  CollisionDetect cd;
  cd.initialize(filename1, filename2);
  double begin = std::clock();
  for (int i = 0; i < 5000; ++i) {
    Eigen::Vector3d p;
    p(0) = double(std::rand())/double(RAND_MAX) - 0.5;
    p(1) = double(std::rand())/double(RAND_MAX) - 0.5;
    p(2) = double(std::rand())/double(RAND_MAX) - 0.5;
    p = 100.0*p;
    cd.setTransformation1(Eigen::Quaterniond::UnitRandom().toRotationMatrix(), p);
    std::cout << "test No." << i << ", intersecting: " << cd.checkCollisions() << std::endl;
  }
  std::cout << "Computation Time: " << (std::clock() - begin) / CLOCKS_PER_SEC << " sec" << std::endl;
}


// Boost.Geometry (aka GGL, Generic Geometry Library)
// QuickBook Example

// Copyright (c) 2011-2012 Barend Gehrels, Amsterdam, the Netherlands.

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

//[make_with_range
//` Using make to construct a linestring with two different range types

#include <iostream>

#include <boost/geometry.hpp>
#include <boost/geometry/geometries/linestring.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/geometries/adapted/c_array.hpp>

BOOST_GEOMETRY_REGISTER_C_ARRAY_CS(cs::cartesian) /*< Necessary to register a C array like {1,2} as a point >*/

int main()
{
    using boost::geometry::make;
    using boost::geometry::detail::make::make_points;

    typedef boost::geometry::model::d2::point_xy<double> point;
    typedef boost::geometry::model::linestring<point> linestring;

    double coordinates[][2] = {{1,2}, {3,4}, {5, 6}}; /*< Initialize with C array points >*/
    linestring ls = make_points<linestring>(coordinates);
    std::cout << boost::geometry::dsv(ls) << std::endl;

    point points[3] = { make<point>(9,8), make<point>(7,6), make<point>(5,4) }; /*< Construct array with points, using make which should work for any point type >*/
    std::cout << boost::geometry::dsv(make_points<linestring>(points)) << std::endl; /*< Construct linestring with point-array and output it as Delimiter Separated Values >*/

    return 0;
}

//]

//[make_with_range_output
/*`
Output:
[pre
((1, 2), (3, 4), (5, 6))
((9, 8), (7, 6), (5, 4))
]
*/
//]


// Copyright (c) 2019 CNES
//
// All rights reserved. Use of this source code is governed by a
// BSD-style license that can be found in the LICENSE file.
#pragma once
#include "pyinterp/detail/geometry/point.hpp"
#include <boost/geometry.hpp>

namespace pyinterp {
namespace detail {
namespace geometry {

/// Defines a box made of two describing points.
///
/// @tparam T Type of data handled by this box
/// @tparam N Number of dimensions in coordinate systems handled.
template <typename T, size_t N>
using BoxND = boost::geometry::model::box<PointND<T, N>>;

/// Defines a box made of two describing points in spherical equatorial space.
///
/// @tparam T Type of data handled by this box
template <typename T>
using EquatorialBox2D = boost::geometry::model::box<EquatorialPoint2D<T>>;

/// Defines a box made of three describing points in spherical equatorial
/// space.
///
/// @tparam T Type of data handled by this box
template <typename T>
using EquatorialBox3D = boost::geometry::model::box<EquatorialPoint3D<T>>;

}  // namespace geometry
}  // namespace detail
}  // namespace pyinterp


// (C) 2014 Arek Olek

#pragma once

#include <algorithm>
#include <stack>
#include <vector>

#include <boost/graph/adjacency_list.hpp>

#include "graph.hpp"
#include "range.hpp"

template <class Graph, class Tree>
void visit(unsigned v, Graph& G,
    std::vector<bool>& visited, std::vector<unsigned>& degree,
    Tree& T) {
  auto rdfs_rule = [&]() {
    unsigned best = 0, min = UINT_MAX;
    for(auto u : range(adjacent_vertices(v, G)))
      if(!visited[u] && degree[u] < min) {
        best = u;
        min = degree[u];
      }
    return best;
  };

  visited[v] = true;
  for(auto u : range(adjacent_vertices(v, G))) --degree[u];
  while(degree[v] > 0) {
    auto w = rdfs_rule();
    add_edge(v, w, T);
    visit(w, G, visited, degree, T);
  }
}

template <class Graph, class Tree>
Tree rdfs_tree(Graph& G) {
  unsigned n = num_vertices(G);
  Tree T(n);
  std::vector<bool> visited(n, false);
  std::vector<unsigned> deg(n, 0);

  for(auto v : range(vertices(G))) deg[v] = out_degree(v, G);

  auto s = std::distance(deg.begin(), std::min_element(deg.begin(), deg.end()));
  visit(s, G, visited, deg, T);

  return T;
}

template<typename Iter>
Iter select_randomly(Iter start, Iter end) {
    static std::default_random_engine g;
    std::uniform_int_distribution<> dis(0, std::distance(start, end) - 1);
    std::advance(start, dis(g));
    return start;
}

template<typename Iter>
std::vector<unsigned> minima(Iter start, Iter end) {
  std::vector<unsigned> mins;
  auto min = *std::min_element(start, end);
  for(auto it = start; it != end; ++it)
    if(*it == min) mins.push_back(std::distance(start, it));
  return mins;
}

template <class Graph, class Tree>
void visit_node(unsigned v, Graph& G,
    std::vector<bool>& visited, std::vector<int>& degree,
    Tree& T) {
  auto rdfs_rule = [&]() {
    std::vector<unsigned> neighbors;
    int min = INT_MAX;
    for(auto u : range(adjacent_vertices(v, G)))
      if(!visited[u] && degree[u] <= min) {
        if(degree[u] < min) neighbors.clear();
        neighbors.push_back(u);
        min = degree[u];
      }
    return *select_randomly(neighbors.begin(), neighbors.end());
  };

  visited[v] = true;
  for(auto u : range(adjacent_vertices(v, G))) --degree[u];
  while(degree[v] > 0) {
    auto w = rdfs_rule();
    add_edge(v, w, T);
    visit_node(w, G, visited, degree, T);
  }
}

template <class Tree, class Graph>
Tree rdfs_rand_tree(Graph& G) {
  unsigned n = num_vertices(G);
  Tree T(n);
  std::vector<bool> visited(n, false);
  std::vector<int> deg(n, 0);

  for(auto v : range(vertices(G))) deg[v] = out_degree(v, G);

  auto mins = minima(deg.begin(), deg.end());
  auto s = *select_randomly(mins.begin(), mins.end());

  visit_node(s, G, visited, deg, T);

  return T;
}

template <class Graph, class Tree>
Tree rdfs_best_tree(Graph& G) {
  auto upper = upper_bound(G);
  auto best_T = rdfs_rand_tree<Tree>(G);
  auto best_n = num_internal(best_T);
  for(int i = 0; i < 50; ++i) {
    if(best_n == upper) break;
    auto T = rdfs_rand_tree<Tree>(G);
    auto n = num_internal(T);
    if(n > best_n) {
      best_T = T;
      best_n = n;
    }
  }
  return best_T;
}