///=======================================================================
// Copyright 2015-2020 Clemson University
// Authors: Bradley S. Meyer
//
// 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 RANK_SPANNING_BRANCHINGS_HPP
#define RANK_SPANNING_BRANCHINGS_HPP

/*
 * Rank spanning branchings
 *         Camerini et al Algorithm
 *
 * Requirement:
 *      directed graph with single root vertex
 */

#include <vector>
#include <limits>

#include <boost/concept_check.hpp>
#include <boost/config.hpp>
#include <boost/foreach.hpp>
#include <boost/graph/properties.hpp>
#include <boost/graph/filtered_graph.hpp>
#include <boost/graph/named_function_params.hpp>
#include <boost/graph/iteration_macros.hpp>
#include <boost/graph/depth_first_search.hpp>
#include <boost/heap/fibonacci_heap.hpp>
#include <boost/pending/disjoint_sets.hpp>
#include <boost/property_map/property_map.hpp>
#include <boost/optional.hpp>
#include <boost/unordered_set.hpp>

using namespace boost;

namespace rsb {

  namespace detail {

    typedef adjacency_list < vecS, vecS, bidirectionalS,
      no_property, no_property > BranchingGraph;

    typedef graph_traits<BranchingGraph>::vertex_descriptor BranchingVertex;

    // Structure to store edges and compare them by weight.

    template <typename Edge, typename WeightMap, typename Compare>
    struct EdgeNode
    {
      Edge                                                            edge;
      typename property_traits<WeightMap>::value_type                 weight;
      Compare                                                         compare;
      EdgeNode(){}
      EdgeNode(
        const Edge& e,
        const typename property_traits<WeightMap>::value_type & w,
        Compare c ) :
        edge( e ), weight( w ), compare( c ) {}
      bool operator<( EdgeNode const & rhs ) const
      { return compare( weight, rhs.weight ); }
    };

    // Insert edges from graph into queue, taking into account constraints.

    template <typename Graph, typename Edge, typename WeightMap,
              typename Compare, typename MergablePriorityQueueMap>
    bool
    insert_edges
    (
      const Graph& g,
      WeightMap& weight_map,
      Compare& comp,
      MergablePriorityQueueMap& in_edges,
      const unordered_set<Edge>& include_edges,
      const unordered_set<Edge>& exclude_edges
    )
    {
      typedef typename graph_traits<Graph>::vertex_descriptor Vertex;
      unordered_set<Vertex> include_vertices, in_set, out_set;
      typename unordered_set<Vertex>::iterator it;

      // Insert vertices in sets to check for spanning branching.

      BGL_FORALL_VERTICES_T( v, g, Graph )
      {
        in_set.insert( v );
        out_set.insert( v );
      }

      // Insert edges that must be present and note the invertex.  Remove
      // vertices from relevant sets.

      BOOST_FOREACH( const Edge& e, include_edges )
      {
        include_vertices.insert( target( e, g ) );
        in_edges[target( e, g )].push(
          EdgeNode<Edge,WeightMap,Compare>( e, get( weight_map, e ), comp )
        );
        it = in_set.find( target( e, g ) );
        if( it != in_set.end() ) { in_set.erase( it ); }
        it = out_set.find( source( e, g ) );
        if( it != out_set.end() ) { out_set.erase( it ); }
      }

      // Insert edges, but not edges to be excluded or into vertices that
      // already have in edges.  Remove vertices from relevant sets.

      BOOST_FOREACH( const Edge &e, edges(g) )
      {
        if(
          include_vertices.find( target(e, g) ) == include_vertices.end()
          &&
          exclude_edges.find( e ) == exclude_edges.end()
        )
        {
          if( source(e, g) != target(e, g) )
          {
            in_edges[target(e,g)].push(
              EdgeNode<Edge,WeightMap,Compare>( e, get( weight_map, e ), comp )
            );
            it = in_set.find( target( e, g ) );
            if( it != in_set.end() ) { in_set.erase( it ); }
            it = out_set.find( source( e, g ) );
            if( it != out_set.end() ) { out_set.erase( it ); }
          }
        }
      }

      // Check for correct number of roots.
      //
      if( in_set.size() != 1 )
        return false;   // Zero roots or more than one root.
      else if( out_set.find( *in_set.begin() ) != out_set.end() )
        return false;   // Root is isolated.
      else
        return true;

    }

    // Retrieve the path back from leaf to root in cycle branching.

    void
    find_back_path(
      BranchingGraph& cycle_branching,
      std::vector<BranchingVertex>& bv
    )
    {

      BGL_FORALL_INEDGES( bv[0], e, cycle_branching, BranchingGraph )
      {
        bv.insert( bv.begin(), source( e, cycle_branching ) );
        find_back_path( cycle_branching, bv );
      }

    }

    // Expand cycles.

    template <typename Graph, typename Edge, typename IndexMap,
              typename WeightMap, typename Compare>
    void
    expand(
      const Graph& g,
      IndexMap& v_id,
      BranchingGraph& cycle_branching,
      unordered_set<BranchingVertex>& root_set,
      std::vector<EdgeNode< Edge, WeightMap, Compare> >& beta
    )
    {

      BOOST_FOREACH( BranchingVertex v, root_set )
      {
        if(
          in_degree( v, cycle_branching ) == 0 &&
          out_degree( v, cycle_branching ) != 0
        )
        {
          std::vector<BranchingVertex> bv;
          bv.push_back( v_id[target( beta[v].edge, g )] );
          find_back_path( cycle_branching, bv );
          for( std::size_t i = 0; i < bv.size() - 1; i++ )
          {
            beta[bv[i+1]] = beta[bv[i]];
            clear_vertex( bv[i], cycle_branching );
          }
        }
      }

      // Remove isolated vertices.

      std::vector<BranchingVertex> vertices_to_remove_from_set;

      BOOST_FOREACH( BranchingVertex v, root_set )
      {
        if(
           in_degree( v, cycle_branching ) == 0 &&
           out_degree( v, cycle_branching ) == 0
        )
        {
          vertices_to_remove_from_set.push_back( v );
        }
      }

      BOOST_FOREACH( BranchingVertex v, vertices_to_remove_from_set )
      {
        root_set.erase( v );
      }

    }

    // Camerini et al. BEST routine.

    template <typename EdgeNodeType, typename MergablePriorityQueue,
              typename Graph, typename Edge, typename IndexMap,
              typename WeightMap, typename Rank, typename Pred,
              typename Compare>
    void
    best_spanning_branching( const Graph& g,
                             unordered_set<Edge>& branching,
                             IndexMap& v_id,
                             WeightMap& weight_map,
                             Compare& comp,
                             Rank rank,
                             Pred pred1,
                             Pred pred2,
                             unordered_set<Edge>& include_edges,
                             unordered_set<Edge>& exclude_edges
                           )
    {

      // Define types.

      typedef typename graph_traits<Graph>::vertex_descriptor Vertex;
      typedef typename graph_traits<Graph>::vertices_size_type vertex_idx_t;

      typedef std::map<Vertex, EdgeNodeType> exit_map_t;

      // Create various objects.  Note in particular the two disjoint
      // sets.  The set of weakly connected components is used to determine
      // cycles.  The set of strongly connected components is used to
      // represent supervertices (condensed cycles).

      unordered_set<Vertex> unvisited_vertex_set;

      Vertex root_vertex;

      BranchingGraph cycle_branching;

      vertex_idx_t n = num_vertices(g);

      std::map<Vertex, MergablePriorityQueue> in_edges;

      // Create disjoint sets.  The set of weakly connected components is
      // used to determine cycles.  The set of strongly connected components
      // is used to represent supervertices (condensed cycles).

      disjoint_sets<Rank, Pred>
        weak_cc( rank, pred1 ), strong_cc( rank, pred2 );

      if(
        !insert_edges(
          g,
          weight_map,
          comp,
          in_edges,
          include_edges,
          exclude_edges
        )
      ) return;

      std::vector<EdgeNodeType> beta( 2 * n );

      std::map<Vertex, vertex_idx_t> parent;

      BGL_FORALL_VERTICES_T( v, g, Graph )
      {
        weak_cc.make_set( v );
        strong_cc.make_set( v );
        parent[v]  = v_id[v];
        add_vertex( cycle_branching );
        unvisited_vertex_set.insert( v );
      }

      while( !unvisited_vertex_set.empty() )
      {

        typename unordered_set<Vertex>::iterator it =
          unvisited_vertex_set.begin();

        if( in_edges[*it].empty() )
        {
          root_vertex = *it;
	  unvisited_vertex_set.erase( it );
        }
        else
        {

	  EdgeNodeType critical_edge_node = in_edges[*it].top();

	  beta[parent[*it]] = critical_edge_node;

          // Done with this vertex.

	  unvisited_vertex_set.erase( it );

          // Check for cycle and, if present, condense.

	  if(
	    weak_cc.find_set( source( critical_edge_node.edge, g ) ) !=
	    weak_cc.find_set( target( critical_edge_node.edge, g ) )
	  )
	  {
	    weak_cc.union_set(
              source( critical_edge_node.edge, g ),
              target( critical_edge_node.edge, g )
	    );
	  }
	  else
	  {
	    BranchingVertex v_new = add_vertex( cycle_branching );

	    EdgeNodeType least_costly_edge_node = critical_edge_node;

	    boost::unordered_set<Vertex> cycle_vertex_set;

	    for(
              Vertex v = source( critical_edge_node.edge, g );
	      cycle_vertex_set.find( strong_cc.find_set( v ) ) ==
                cycle_vertex_set.end();
	      v = source( beta[parent[strong_cc.find_set( v )]].edge, g )
	    )
	    {
              Vertex u = strong_cc.find_set( v );
	      cycle_vertex_set.insert( u );
	      add_edge( v_new, parent[u], cycle_branching );
              if(
                comp( beta[parent[u]].weight, least_costly_edge_node.weight )
              )
              {
		least_costly_edge_node = beta[parent[u]];
              }
	    }

            Vertex new_repr = *cycle_vertex_set.begin();

	    BOOST_FOREACH( Vertex u, cycle_vertex_set )
	    {
              strong_cc.link( u, new_repr );
              new_repr = strong_cc.find_set( new_repr );
            }

	    BOOST_FOREACH( Vertex v, cycle_vertex_set )
	    {
              exit_map_t v_exit;
              BOOST_FOREACH( EdgeNodeType en, in_edges[v] )
              {
                if( strong_cc.find_set( source( en.edge, g ) ) != new_repr )
                {
                  en.weight += least_costly_edge_node.weight -
                      beta[parent[v]].weight;
                  Vertex u = strong_cc.find_set( source( en.edge, g ) );
                  if( v_exit.find(u) != v_exit.end() )
                  {
                    if( comp( v_exit[u].weight, en.weight ) )
                    {
                      v_exit[u] = en;
                    }
                  }
                  else
                  {
                    v_exit[u] = en;
                  }
                }
	      }
              MergablePriorityQueue tmp_queue;
              BOOST_FOREACH( typename exit_map_t::value_type& t, v_exit )
              {
                tmp_queue.push( t.second );
              }
              in_edges[v].swap( tmp_queue );
	    }

	    BOOST_FOREACH( Vertex v, cycle_vertex_set )
	    {
              if( v != new_repr ) in_edges[new_repr].merge( in_edges[v] );
            }

	    unvisited_vertex_set.insert( new_repr );

            parent[new_repr] = v_new;

	  }

        }

      }

      // Create a set containing possible roots of the cycle branching.
      // In each cycle expansion, remove isolated vertices of the
      // cycle branching to avoid considering them in subsequent cycle
      // expansions.

      unordered_set<BranchingVertex> root_set;

      BGL_FORALL_VERTICES( u, cycle_branching, BranchingGraph )
      {
        root_set.insert( u );
      }

      while( !root_set.empty() )
      {
        expand( g, v_id, cycle_branching, root_set, beta );
      }

      BGL_FORALL_VERTICES_T( v, g, Graph )
      {
        if( v != root_vertex )
        {
          branching.insert( beta[v_id[v]].edge );
        }
      }

    }

    // Depth-first visitor to set up pre-order and post-order maps.

    template<typename OrderMap>
    class dfs_order_visitor:public default_dfs_visitor
    {

      public:
	dfs_order_visitor(
	  OrderMap& pr,
	  OrderMap& po
	) :  m_pr( pr ), m_po( po ) { td = 0; tf = 0; }

	template<typename Vertex, typename Graph>
	void discover_vertex(const Vertex u, const Graph & g)
	{
	  m_pr[u] = td++;
	}

	template <typename Vertex, typename Graph>
	void finish_vertex(const Vertex u, const Graph & g)
	{
	  m_po[u] = tf++;
	}

	OrderMap& m_pr;
	OrderMap& m_po;

      private:
	std::size_t td;
	std::size_t tf;

    };

    // The ancestor checker.  A vertex u is a proper ancestor of
    // vertex v (in the parent branching) if pr[u] < pr[v] and po[u] > po[v].

    template<typename OrderMap>
    struct ancestor_checker
    {
      OrderMap& m_pr;
      OrderMap& m_po;
      ancestor_checker( OrderMap& pr, OrderMap& po ) :
	m_pr( pr ), m_po( po ) {}

      template<typename Vertex>
      bool operator()( const Vertex& v1, const Vertex& v2 ) const
      { return ( m_pr[v1] < m_pr[v2] && m_po[v1] > m_po[v2] ); }
    };

    // Create a new branching from an input edge set.

    template<typename Graph, typename Edge, typename IndexMap>
    BranchingGraph
    create_branching_graph_from_edge_set(
      Graph& g,
      IndexMap& v_id,
      const unordered_set<Edge>& branching
    )
    {

      BranchingGraph new_branching;

      for( size_t i = 0; i < num_vertices( g ); i++ )
      {
        add_vertex( new_branching );
      }

      BOOST_FOREACH( const Edge& e, branching )
      {
        add_edge(
          v_id[source( e, g )], v_id[target( e, g )], new_branching
        );
      }

      return new_branching;

    }

    // Camerini et al. SEEK routine.  Find the in edge that, when removed,
    //  gives the next best branching for a vertex and the weight difference.

    template <typename Graph, typename Edge, typename IndexMap,
              typename MergablePriorityQueue, typename EdgeNodeType,
              typename Compare, typename OrderMap, typename OptionalWeight>
    void
    seek_next_edge_weight_diff(
      Graph& g,
      MergablePriorityQueue& in_edges,
      IndexMap& v_id,
      ancestor_checker<OrderMap>& is_ancestor,
      const unordered_set<Edge>& branching,
      EdgeNodeType& b,
      Compare& comp,
      Edge& return_edge,
      OptionalWeight& delta
    )
    {

      if( branching.find( b.edge ) != branching.end() )
      {
        for(
          typename MergablePriorityQueue::ordered_iterator ei =
            in_edges.ordered_begin();
          ei != in_edges.ordered_end();
          ei++
        )
        {
          if( (*ei).edge != b.edge )
          {
            if(
              !is_ancestor(
                v_id[target( b.edge, g )],
                v_id[source( (*ei).edge, g )]
              )
            )
            {
              if( !delta || comp( b.weight - (*ei).weight, delta.get() ) )
              {
                delta = b.weight - (*ei).weight;
                return_edge = b.edge;
                break;
              }
            }
          }
        }
      }
    }

    // Camerini et al. NEXT routine.  Find the edge that, when removed,
    // gives the next best branching and the resulting weight difference.

    template <typename EdgeNodeType, typename MergablePriorityQueue,
              typename Graph, typename Edge, typename IndexMap,
              typename WeightMap, typename Rank, typename Pred,
              typename Compare>
    boost::optional<
      std::pair<Edge, typename property_traits<WeightMap>::value_type>
    >
    next_spanning_branching( const Graph& g,
                             const unordered_set<Edge>& branching,
                             IndexMap& v_id,
                             WeightMap& weight_map,
                             Compare& comp,
                             Rank rank,
                             Pred pred1,
                             Pred pred2,
                             unordered_set<Edge>& include_edges,
                             unordered_set<Edge>& exclude_edges
                           )
    {

      typedef typename graph_traits<Graph>::vertex_descriptor Vertex;

      typedef std::map<Vertex, EdgeNodeType> exit_map_t;

      typedef typename property_traits<WeightMap>::value_type weight_t;

      Edge return_edge;

      boost::optional<weight_t> delta;

      EdgeNodeType b;

      // Create various objects.

      boost::optional<
        std::pair<Edge, typename property_traits<WeightMap>::value_type>
      > next_edge_and_weight_delta;

      unordered_set<Vertex> unvisited_vertex_set;

      std::map<Vertex, MergablePriorityQueue> in_edges;

      std::map<Vertex, EdgeNodeType> max_e;

      // Create disjoint sets.  The set of weakly connected components is
      // used to determine cycles.  The set of strongly connected components
      // is used to represent supervertices (condensed cycles).

      disjoint_sets<Rank, Pred>
        weak_cc( rank, pred1 ), strong_cc( rank, pred2 );

      // Create a branching graph to check whether a vertex is an ancestor of
      // another vertex in the branching.

      BranchingGraph ancestor_branching =
        create_branching_graph_from_edge_set( g, v_id, branching );

      // Insert edges.

      if(
        !insert_edges(
          g,
          weight_map,
          comp,
          in_edges,
          include_edges,
          exclude_edges
        )
      )
      {
        return next_edge_and_weight_delta;
      }

      // Initialize data structures and find start vertex for depth
      // first search.

      BranchingVertex start_vertex;

      BGL_FORALL_VERTICES_T( v, g, Graph )
      {
        weak_cc.make_set( v );
        strong_cc.make_set( v );
        unvisited_vertex_set.insert( v );
        if( in_edges[v].empty() ) { start_vertex = v_id[v]; }
      }

      // Create the ancestor checker from ancestor_branching.

      typedef std::map<BranchingVertex, std::size_t> OrderMap;

      OrderMap pr;
      OrderMap po;

      dfs_order_visitor<OrderMap> vis(pr, po);

      depth_first_search(
        ancestor_branching, visitor(vis).root_vertex( start_vertex )
      );

      ancestor_checker<OrderMap> is_ancestor(pr, po);

      // Main loop.

      while( !unvisited_vertex_set.empty() )
      {

        typename unordered_set<Vertex>::iterator it =
          unvisited_vertex_set.begin();

        if( in_edges[*it].empty() )
        {
	  unvisited_vertex_set.erase( it );
        }
        else
        {

          // Get largest in edge with ties solved in favor of edges in
          // input branching.

          for(
            typename MergablePriorityQueue::ordered_iterator ei =
              in_edges[*it].ordered_begin();
            ei != in_edges[*it].ordered_end();
            ei++
          )
          {
            if( comp( (*ei).weight, in_edges[*it].top().weight ) )
              break;
            b = *ei;
            if( branching.find( b.edge ) != branching.end() )
              break;
          }

	  max_e[*it] = b;

          // Seek next edge weight difference.

          seek_next_edge_weight_diff(
            g, in_edges[*it], v_id, is_ancestor, branching, b, comp,
            return_edge, delta
          );

          // Done with this vertex.

	  unvisited_vertex_set.erase( it );

          // Check for cycle and, if present, condense.

	  if(
	    weak_cc.find_set( source( b.edge, g ) ) !=
	    weak_cc.find_set( target( b.edge, g ) )
	  )
	  {
	    weak_cc.union_set( source( b.edge, g ), target( b.edge, g ) );
	  }
	  else
	  {

	    EdgeNodeType least_costly_edge_node = b;

	    boost::unordered_set<Vertex> cycle_vertex_set;

	    for(
              Vertex v = source( b.edge, g );
	      cycle_vertex_set.find( strong_cc.find_set( v ) ) ==
                cycle_vertex_set.end();
	      v = source( max_e[strong_cc.find_set( v )].edge, g )
	    )
	    {
              Vertex u = strong_cc.find_set( v );
	      cycle_vertex_set.insert( u );
	      if( comp( max_e[u].weight, least_costly_edge_node.weight ) )
              {
		least_costly_edge_node = max_e[u];
              }
	    }

            Vertex new_repr = *cycle_vertex_set.begin();

	    BOOST_FOREACH( Vertex v, cycle_vertex_set )
	    {
              strong_cc.link( v, new_repr );
              new_repr = strong_cc.find_set( new_repr );
            }

            // Adjust arc weights and remove parallel arcs.  Keep the
            // the largest weight in arc and the largest viable alternative
            // arc from each source outside the cycle.  Make sure that
            // an arc from the branching is among the added edges, if present.

	    BOOST_FOREACH( Vertex v, cycle_vertex_set )
	    {
              std::vector<exit_map_t> v_exit(3);
              BOOST_FOREACH( EdgeNodeType en, in_edges[v] )
              {
                if( strong_cc.find_set( source( en.edge, g ) ) != new_repr )
                {
                  en.weight += least_costly_edge_node.weight - max_e[v].weight;
                  Vertex u = strong_cc.find_set( source( en.edge, g ) );
                  if( branching.find( en.edge ) != branching.end() )
                  {
                    v_exit[0][u] = en;
                  }
                  else
                  {
                    if( v_exit[1].find(u) != v_exit[1].end() )
                    {
                      if( comp( v_exit[1][u].weight, en.weight ) )
                      {
                        if(
                          !is_ancestor(
                             v_id[target( v_exit[1][u].edge, g )],
                             v_id[source( v_exit[1][u].edge, g )]
                          )
                        )
                        {
                          v_exit[2][u] = v_exit[1][u];
                        }
                        v_exit[1][u] = en;
                      }
                      else if(
                        v_exit[2].find(u) == v_exit[2].end() ||
                        comp( v_exit[2][u].weight, en.weight )
                      )
                      {
                        if(
                          !is_ancestor(
                            v_id[target( en.edge, g )],
                            v_id[source( en.edge, g )]
                          )
                        )
                        {
                          v_exit[2][u] = en;
                        }
                      }
                    }
                    else
                    {
                      v_exit[1][u] = en;
                    }
                  }
                }
	      }
              MergablePriorityQueue tmp_queue;
              BOOST_FOREACH( exit_map_t& exit_map, v_exit )
              {
                BOOST_FOREACH( typename exit_map_t::value_type& t, exit_map )
                {
                  tmp_queue.push( t.second );
                }
              }
              in_edges[v].swap( tmp_queue );
	    }

	    BOOST_FOREACH( Vertex v, cycle_vertex_set )
	    {
              if( v != new_repr ) in_edges[new_repr].merge( in_edges[v] );
            }

	    unvisited_vertex_set.insert( new_repr );

	  }

        }

      }

      if( delta )
      {
        next_edge_and_weight_delta = std::make_pair( return_edge, delta.get() );
      }

      return next_edge_and_weight_delta;

    }

    // Class to filter graph.

    template<typename EdgeSet>
    class branching_filter
    {

      public:
        branching_filter(){}
        branching_filter( const EdgeSet * _es ) : p_es( _es ){}

        template <typename Edge>
        bool operator()( const Edge& e ) const
        {
          if( p_es->find( e ) != p_es->end() )
            return true;
          else
            return false;
        }

     private:
       const EdgeSet * p_es;

    };

    // Structure to store branchings and compare them by weight.

    template<typename Edge, typename WeightMap, typename Compare>
    struct BranchingEntry
    {
      Edge                                                        edge;
      typename property_traits<WeightMap>::value_type             weight;
      Compare                                                     compare;
      unordered_set<Edge>                                         branching;
      unordered_set<Edge>                                         include_edges;
      unordered_set<Edge>                                         exclude_edges;
      BranchingEntry(){}
      BranchingEntry(
	const typename property_traits<WeightMap>::value_type& w,
	const Edge& e,
	Compare& comp,
	const unordered_set<Edge>& b,
	const unordered_set<Edge>& include,
	const unordered_set<Edge>& exclude
      ) :
	  edge( e ), weight( w ), compare( comp ),
	  branching( b ), include_edges( include ),
	  exclude_edges( exclude ){}
      bool operator<( BranchingEntry const & rhs ) const
      { return compare( weight, rhs.weight ); }
    };

    // Compute the weight of a branching.

    template<typename WeightMap, typename Edge>
    typename property_traits<WeightMap>::value_type
    compute_branching_weight(
      WeightMap& w,
      const unordered_set<Edge>& branching
    )
    {

       typedef typename property_traits<WeightMap>::value_type weight_t;

       boost::optional<weight_t> weight;

       BOOST_FOREACH( const Edge& e, branching )
       {

         if( !weight )
         {
           weight = get( w, e );
         }
         else
         {
	   weight = weight.get() + get( w, e );
         }
       }

       return weight.get();

    }

    // Routine implementation.

    template <template<class...> class PriorityQueue,
              typename Graph, typename BranchingProcessor, typename IndexMap,
              typename WeightMap, typename Compare,
              typename Rank, typename Parent>
    void
    rank_spanning_branchings_impl( const Graph& g,
			           BranchingProcessor bp,
                                   IndexMap v_id,
                                   WeightMap w,
			           Compare comp,
                                   Rank rank,
                                   Parent pred1,
                                   Parent pred2
			         )
    {

      typedef typename graph_traits<Graph>::vertex_descriptor Vertex;
      typedef typename graph_traits<Graph>::edge_descriptor Edge;

      typedef typename property_traits<WeightMap>::value_type weight_t;

      typedef BranchingEntry<Edge, WeightMap, Compare> branching_entry_t;

      typedef EdgeNode<Edge, WeightMap, Compare> edge_node_t;

      typedef PriorityQueue<branching_entry_t> branching_queue_t;

      typedef PriorityQueue<edge_node_t> edge_node_queue_t;

      BOOST_CONCEPT_ASSERT(( VertexAndEdgeListGraphConcept<Graph> ));

      BOOST_CONCEPT_ASSERT(( ReadablePropertyMapConcept<IndexMap, Vertex> ));
      BOOST_CONCEPT_ASSERT(( ReadablePropertyMapConcept<WeightMap, Edge> ));

      BOOST_CONCEPT_ASSERT(( heap::PriorityQueue<branching_queue_t> ));
      BOOST_CONCEPT_ASSERT(( heap::MergablePriorityQueue<edge_node_queue_t> ));

      unordered_set<Edge> best_branching, empty_set;

      boost::optional<std::pair<Edge, weight_t> > next_edge_and_weight_delta;

      Edge e;

      branching_queue_t branching_queue;

      best_spanning_branching<edge_node_t, edge_node_queue_t>
                             ( g,
                               best_branching,
                               v_id,
                               w,
                               comp,
                               rank,
                               pred1,
                               pred2,
                               empty_set,
                               empty_set
                             );

      branching_filter<unordered_set<Edge> > filter1( &best_branching );
      filtered_graph<Graph, branching_filter<unordered_set<Edge> > >
        fg1( g, filter1 );

      if( !bp( fg1 ) ) return;

      next_edge_and_weight_delta =
        next_spanning_branching<edge_node_t, edge_node_queue_t>
                              ( g,
                                 best_branching,
                                  v_id,
                                  w,
                                  comp,
                                  rank,
                                  pred1,
                                  pred2,
                                  empty_set,
                                  empty_set
                               );

      if( next_edge_and_weight_delta )
      {
	branching_queue.push(
          branching_entry_t(
	    compute_branching_weight( w, best_branching ) -
              (next_edge_and_weight_delta.get()).second,
	    (next_edge_and_weight_delta.get()).first,
	    comp,
	    best_branching,
	    empty_set,
	    empty_set
	  )
	);
      }
      else
      {
	return;
      }

      while( !branching_queue.empty() )
      {

	unordered_set<Edge> branching;

	branching_entry_t P = branching_queue.top();

	branching_queue.pop();

	unordered_set<Edge> include_edges = P.include_edges;

	unordered_set<Edge> exclude_edges = P.exclude_edges;

	include_edges.insert( P.edge );

	exclude_edges.insert( P.edge );

	best_spanning_branching<edge_node_t, edge_node_queue_t>
                              ( g,
                                 branching,
                                 v_id,
                                 w,
                                 comp,
                                 rank,
                                 pred1,
                                 pred2,
                                 P.include_edges,
                                 exclude_edges
                               );

        branching_filter<unordered_set<Edge> > filter( &branching );
        filtered_graph<Graph, branching_filter<unordered_set<Edge> > >
          fg( g, filter );

        if( !bp( fg ) ) return;

	next_edge_and_weight_delta =
	  next_spanning_branching<edge_node_t, edge_node_queue_t>
                                ( g,
                                   P.branching,
                                   v_id,
                                   w,
                                   comp,
                                   rank,
                                   pred1,
                                   pred2,
                                   include_edges,
                                   P.exclude_edges
                                 );

	if( next_edge_and_weight_delta )
	{
	  branching_queue.push(
            branching_entry_t(
	      compute_branching_weight( w, P.branching ) -
                (next_edge_and_weight_delta.get()).second,
	      (next_edge_and_weight_delta.get()).first,
	      comp,
	      P.branching,
	      include_edges,
	      P.exclude_edges
	    )
	  );
	}

	next_edge_and_weight_delta =
	  next_spanning_branching<edge_node_t, edge_node_queue_t>
                                ( g,
                                   branching,
                                   v_id,
                                   w,
                                   comp,
                                   rank,
                                   pred1,
                                   pred2,
                                   P.include_edges,
                                   exclude_edges
                                 );

	if( next_edge_and_weight_delta )
	{
	  branching_queue.push(
            branching_entry_t(
	      P.weight - (next_edge_and_weight_delta.get()).second,
	      (next_edge_and_weight_delta.get()).first,
	      comp,
	      branching,
	      P.include_edges,
	      exclude_edges
	    )
	  );
	}

      }

    }

    template <template<class...> class PriorityQueue,
              typename Graph, typename BranchingProcessor, typename IndexMap,
              typename WeightMap, typename Compare>
    void
    rank_spanning_branchings_dispatch2( const Graph& g,
                                        BranchingProcessor bp,
                                        IndexMap id_map,
                                        WeightMap weight_map,
                                        Compare compare
                                      )
    {

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

      if( num_vertices( g ) == 0 ) return; // Nothing to do.

      typedef typename graph_traits<Graph>::vertices_size_type vertex_idx_t;
      typedef typename graph_traits<Graph>::vertex_descriptor Vertex;

      // Set up rank and parent for disjoint sets.

      std::vector<vertex_idx_t> rank( n );

      std::vector<Vertex> pred1( n ), pred2( n );

      rank_spanning_branchings_impl<PriorityQueue>(
        g,
        bp,
        id_map,
        weight_map,
        compare,
        make_iterator_property_map( rank.begin(), id_map, rank[0] ),
        make_iterator_property_map( pred1.begin(), id_map, pred1[0] ),
        make_iterator_property_map( pred2.begin(), id_map, pred2[0])
      );

    }

    template <template<class...> class PriorityQueue,
              typename Graph, typename BranchingProcessor, typename Compare,
              typename P, typename T, typename R>
    void rank_spanning_branchings_dispatch1(
      const Graph& g,
      BranchingProcessor bp,
      Compare compare,
      const bgl_named_params<P, T, R>& params
    )
    {

      detail::rank_spanning_branchings_dispatch2<PriorityQueue>(
        g,
        bp,
        choose_param(
          get_param( params, vertex_index_t()), get( vertex_index, g )
        ),
        choose_param(
          get_param( params, edge_weight_t()), get( edge_weight, g )
        ),
        compare
      );

    }

    template <template<class...> class PriorityQueue,
              typename Graph, typename BranchingProcessor,
              typename P, typename T, typename R>
    void rank_spanning_branchings_dispatch1(
      const Graph& g,
      BranchingProcessor bp,
      param_not_found,
      const bgl_named_params<P, T, R>& params
    )
    {

      typedef
        typename
        property_traits<
          typename property_map<Graph, edge_weight_t>::const_type
        >::value_type weight_t;

      BOOST_CONCEPT_ASSERT(( ComparableConcept<weight_t> ));

      detail::rank_spanning_branchings_dispatch2<PriorityQueue>(
        g,
        bp,
        choose_param(
          get_param( params, vertex_index_t()), get( vertex_index, g )
        ),
        choose_param(
          get_param( params, edge_weight_t()), get( edge_weight, g )
        ),
        std::less<weight_t>()
      );

    }

  } // namespace detail

  template <template<class...> class PriorityQueue = heap::fibonacci_heap,
            typename Graph, typename BranchingProcessor,
            typename P, typename T, typename R>
  void
  inline rank_spanning_branchings(
    const Graph& g,
    BranchingProcessor bp,
    const bgl_named_params<P, T, R>& params
  )
  {

    detail::rank_spanning_branchings_dispatch1<PriorityQueue>(
      g,
      bp,
      get_param( params, distance_compare_t() ),
      params
    );

  }

  template <template<class...> class PriorityQueue = heap::fibonacci_heap,
            typename Graph, typename BranchingProcessor>
  void
  inline rank_spanning_branchings( const Graph& g,
                            BranchingProcessor bp
                          )
  {

    bgl_named_params<int,int> params(0);
    rank_spanning_branchings<PriorityQueue>( g, bp, params );

  }

} // namespace rsb

#endif // RANK_SPANNING_BRANCHINGS_HPP


#pragma once

#include <map>

#include <cvode/cvode.h>               /* prototypes for CVODE fcts., consts.  */
#include <nvector/nvector_serial.h>    /* access to serial N_Vector            */
#include <sunmatrix/sunmatrix_dense.h> /* access to dense SUNMatrix            */
#include <sunlinsol/sunlinsol_dense.h> /* access to dense SUNLinearSolver      */
#include <sundials/sundials_types.h>   /* defs. of realtype, sunindextype      */

//#include <Eigen/Core>
//#include <Eigen/Dense>

#include <autodiff/forward/real.hpp>
#include <autodiff/forward/real/eigen.hpp>
#include "types.hh"
//#define Ith(v,i)    NV_Ith_S(v,i-1)         /* Ith numbers components 1..NEQ */
//#define IJth(A,i,j) SM_ELEMENT_D(A,i-1,j-1) /* IJth numbers rows,cols 1..NEQ */

using namespace types;

namespace cvode_wrapper
{

std::map<std::string_view, int> cv_lmm = {{"BDF", CV_BDF}, {"ADAMS", CV_ADAMS}};

struct cv_options
{
    // Integrator settings
    std::string step_method = "BDF";
    double      rtol        = 1.0E-4;   // relative tolerance
    double      atol        = 1.0E-14;   // absolute tolerance
    int         max_steps   = 500;      // max number of steps between outputs
    int         max_order   = 5;        // max order of linear multistep
    double      t0          = 0.0;      // inital time
    double      tf          = 1.0;      // final time
    double      step_min    = 0.0;      // minimum step size
    double      step_max    = 0.0;      // maximum step size (0 -> no max size)

    // Linear solver and preconditioner settings
    double      epslin      = 1.0E-6;    // linear solver tolerance factor

    // output options
    int         output_lvl  = 0;
};

static int check_retval(void *returnvalue, const char *funcname, int opt)
{
  int *retval;

  /* Check if SUNDIALS function returned NULL pointer - no memory allocated */
  if (opt == 0 && returnvalue == NULL) {
    fmt::print(stderr, "\nSUNDIALS_ERROR: {} failed - returned NULL pointer\n\n",
	    funcname);
    return(1); }

  /* Check if retval < 0 */
  else if (opt == 1) {
    retval = (int *) returnvalue;
    if (*retval < 0) {
      fmt::print(stderr, "\nSUNDIALS_ERROR: {} failed with retval = %d\n\n",
	      funcname, *retval);
      return(1); }}

  /* Check if function returned NULL pointer - no memory allocated */
  else if (opt == 2 && returnvalue == NULL) {
    fmt::print(stderr, "\nMEMORY_ERROR: {} failed - returned NULL pointer\n\n",
	    funcname);
    return(1); }

  return(0);
}

/* compare the solution at the final time 4e10s to a reference solution computed
   using a relative tolerance of 1e-8 and absoltue tolerance of 1e-14 */
static int check_ans(N_Vector y, realtype t, realtype rtol, N_Vector atol)
{
  int      passfail=0;        /* answer pass (0) or fail (1) retval */
  N_Vector ref;               /* reference solution vector        */
  N_Vector ewt;               /* error weight vector              */
  realtype err;               /* wrms error                       */
  realtype ONE=RCONST(1.0);
  realtype ZERO=RCONST(0.0);

  /* create reference solution and error weight vectors */
  ref = N_VClone(y);
  ewt = N_VClone(y);

  /* set the reference solution data */
  NV_Ith_S(ref,0) = RCONST(5.2083495894337328e-08);
  NV_Ith_S(ref,1) = RCONST(2.0833399429795671e-13);
  NV_Ith_S(ref,2) = RCONST(9.9999994791629776e-01);

  /* compute the error weight vector, loosen atol */
  N_VAbs(ref, ewt);
  N_VLinearSum(rtol, ewt, RCONST(10.0), atol, ewt);
  if (N_VMin(ewt) <= ZERO) {
    fmt::print(stderr, "\nSUNDIALS_ERROR: check_ans failed - ewt <= 0\n\n");
    return(-1);
  }
  fmt::print(stdout, "\n{} {} {}\n", NV_Ith_S(ewt,0),NV_Ith_S(ewt,1),NV_Ith_S(ewt,2));
  N_VInv(ewt, ewt);

  /* compute the solution error */
  N_VLinearSum(ONE, y, -ONE, ref, ref);
  err = N_VWrmsNorm(ref, ewt);

  /* is the solution within the tolerances? */
  passfail = (err < ONE) ? 0 : 1;

  if (passfail) {
    fmt::print(stdout, "\nSUNDIALS_WARNING: check_ans error={}\n\n", err);
  }

  /* Free vectors */
  N_VDestroy(ref);
  N_VDestroy(ewt);

  return(passfail);
}

template<class System>
struct cvode_stepper
{
    // options
    cv_options options;

    // cvode context pointer
    void *cvode_mem;

    size_t N;           // system size
    N_Vector cv_y, abst;     // ongoing solution
    SUNMatrix A;        //
    SUNLinearSolver LS;

    cvode_stepper(cv_options opts) : options(std::move(opts)), cvode_mem(nullptr)
    {

    }

    ~cvode_stepper()
    {
        CVodeFree(&cvode_mem);
        N_VDestroy(cv_y); N_VDestroy(abst);
        SUNLinSolFree(LS);
        SUNMatDestroy(A);
    }

    auto copy_nvect(const N_Vector a, vector_t& b)
    {
        for(int i = 0; i < N; ++i)
        {
            b(i) = NV_Ith_S(a, i);
        }
    }

    auto initialize(vector_t& y0)
    {
        // set system size
        N = y0.rows();

        // allocate solution vector
        //y = N_VNew_Serial(N);

        //cv_y = N_VMake_Serial(N, static_cast<Eigen::VectorXd>(y0).data());
        cv_y = N_VMake_Serial(N, y0.data());
        //cv_y = N_VNew_Serial(N);

        // fill solution vector
        //copy_evect(y0, cv_y);

        // linear solver data
        // TODO: implement sparse
        A = SUNDenseMatrix(N, N);
        LS = SUNLinSol_Dense(cv_y, A);

        // init a new cvode context
        cvode_mem = CVodeCreate(cv_lmm[options.step_method]);
        // init integrator memory

        CVodeInit(cvode_mem, [](realtype t, N_Vector y, N_Vector ydot, void* user_data)
            {
                System* sys = static_cast<System*>(user_data);
                vector_map_t yref(NV_DATA_S(y), NV_LENGTH_S(y));
                vector_map_t fref(NV_DATA_S(ydot), NV_LENGTH_S(ydot));

                fref = sys->f(yref, t);

                return 0;

            },
        options.t0, cv_y);
        // set max step attempts
        CVodeSetMaxNumSteps(cvode_mem, options.max_steps);
        // set max/min step sizes
        CVodeSetMinStep(cvode_mem, options.step_min);
        CVodeSetMaxStep(cvode_mem, options.step_max);
        // set max order
        CVodeSetMaxOrd(cvode_mem, options.max_order);
        // set tolarances
        // TODO: allow vector tolerance
        abst = N_VNew_Serial(N);
        NV_Ith_S(abst, 0) = 1.0E-8; NV_Ith_S(abst, 1) = 1.0E-14; NV_Ith_S(abst, 2) = 1.0E-6;
        CVodeSVtolerances(cvode_mem, options.rtol, abst);
        //CVodeSStolerances(cvode_mem, options.rtol, options.atol);
        // set linear solver
        CVodeSetLinearSolver(cvode_mem, LS, A);
        // set jacobin
        CVodeSetJacFn(cvode_mem, [](realtype t, N_Vector y, N_Vector fy, SUNMatrix J,
            void *user_data, N_Vector tmp1, N_Vector tmp2, N_Vector tmp3)
            {
                System* sys = static_cast<System*>(user_data);

                vector_map_t yref(NV_DATA_S(y), NV_LENGTH_S(y));
                matrix_map_t Jref(SM_DATA_D(J), SM_ROWS_D(J), SM_COLUMNS_D(J));

                Jref = sys->J(yref, t);
                return 0;
            }
        );

    }

    auto print_sol(double sol_time)
    {
        fmt::print("At t = {} y = {} {} {}\n", sol_time, NV_Ith_S(cv_y,0), NV_Ith_S(cv_y,1),NV_Ith_S(cv_y,2));
    }

    auto step()
    {

    }

    auto check_retval(void *returnvalue, const char *funcname, int opt)
    {
        int *retval;

        /* Check if SUNDIALS function returned NULL pointer - no memory allocated */
        if (opt == 0 && returnvalue == NULL) {
            fmt::print(stderr, "\nSUNDIALS_ERROR: {} failed - returned NULL pointer\n\n",
                funcname);
            return(1); }

        /* Check if retval < 0 */
        else if (opt == 1) {
            retval = (int *) returnvalue;
            if (*retval < 0) {
            fmt::print(stderr, "\nSUNDIALS_ERROR: {} failed with retval = %d\n\n",
                funcname, *retval);
            return(1); }}

        /* Check if function returned NULL pointer - no memory allocated */
        else if (opt == 2 && returnvalue == NULL) {
            fmt::print(stderr, "\nMEMORY_ERROR: {} failed - returned NULL pointer\n\n",
                funcname);
            return(1); }

        return(0);
    }
    auto letsgo()
    {
        auto iout = 0;
        auto tout = 0.4;
        double sol_time;
        while(1)
        {
            auto rstep = CVode(cvode_mem, tout, cv_y, &sol_time, CV_NORMAL);
            print_sol(sol_time);
            if (check_retval(&rstep, "CVode", 1)) break;
            if (rstep == CV_SUCCESS) {
                iout++;
                tout *= 10.0;
            }
        if (iout == 12) break;
        }
        check_ans(cv_y,sol_time,options.rtol,abst);
    }

};

}


#ifndef SPARSETENSOR_EINSUM_SUBSCRIPT_HPP
#define SPARSETENSOR_EINSUM_SUBSCRIPT_HPP

#include <vector>
#include <tuple>
#include <map>
#include <numeric>
#include <algorithm>
#include <memory>
#include <ostream>

#include <boost/container/flat_map.hpp>

#include <fmt/core.h>
#include <fmt/format.h>
#include <fmt/ostream.h>

#include "Dice/einsum/internal/util/UndirectedGraph.hpp"
#include "Dice/einsum/internal/RawSubscript.hpp"

#include <vector>
#include <tuple>
#include <boost/container_hash/hash.hpp>
#include <tsl/hopscotch_set.h>
#include <tsl/hopscotch_map.h>

namespace einsum::internal {

	using DependencyGraph = util::UndirectedGraph<Label>;
	using ConnectedComponent = typename DependencyGraph::NodeSet;
	using ConnectedComponents = std::vector<ConnectedComponent>;

	using CartesianOperandPos = OperandPos;
	using OriginalOperandPos = OperandPos;
	using OriginalResultPos = OperandPos;
	using OriginalOperandPoss = std::vector<OriginalOperandPos>;
	using OriginalResultPoss = std::vector<OriginalResultPos>;

	/**
	 * Representation of the subscript of a expression in einstein summation convention.
	 * This provides also  bracketing of independently computable parts and resulting in a
	 * cross product of the bracketed parts.
	 */
	class Subscript {
	public:
		enum class Type {
			None = 0, Join, Cartesian, Resolve, Count, EntryGenerator, CarthesianMapping
		};
		using Label = char;

		class CartesianSubSubscripts {

		private:
			mutable std::vector<std::shared_ptr<Subscript>> sub_subscripts;
			mutable std::vector<OriginalOperandPoss> original_operand_poss_of_sub_subscript;
			mutable std::vector<OriginalResultPoss> original_result_poss_of_sub_subscript;
			mutable std::shared_ptr<Subscript> subscript;

		public:
			CartesianSubSubscripts() = default;

			CartesianSubSubscripts(const Subscript &original_subscript) {
				OperandsSc operands_labels{};
				for (const auto &connected_component: original_subscript.connected_components) {
					auto[cartesian_sub_subscript, original_op_poss, original_result_poss] =
					extractCartesianSubSubscript(original_subscript, connected_component);
					operands_labels.push_back(cartesian_sub_subscript->raw_subscript.result);
					sub_subscripts.emplace_back(std::move(cartesian_sub_subscript));
					original_operand_poss_of_sub_subscript.emplace_back(std::move(original_op_poss));
					original_result_poss_of_sub_subscript.emplace_back(std::move(original_result_poss));
				}
				ResultSc result_labels = original_subscript.raw_subscript.result;
				subscript = std::make_shared<Subscript>(operands_labels, result_labels, Type::CarthesianMapping);
			}

			const std::shared_ptr<Subscript> &getSubscript() const {
				return subscript;
			}

			const std::vector<std::shared_ptr<Subscript>> &getSubSubscripts() const {
				return sub_subscripts;
			}

			const OriginalOperandPoss &getOriginalOperandPoss(CartesianOperandPos cart_op_pos) const {
				return original_operand_poss_of_sub_subscript[cart_op_pos];
			}

			const std::vector<OriginalResultPoss> &getOriginalResultPoss() const {
				return original_result_poss_of_sub_subscript;
			}

		private:
			static std::tuple<std::shared_ptr<Subscript>, OriginalOperandPoss, OriginalResultPoss>
			extractCartesianSubSubscript(const Subscript &subscripts, const ConnectedComponent &label_subset) {
				OperandsSc operands_labels{};
				OriginalOperandPoss original_op_poss{};
				OriginalResultPoss original_result_poss{};
				for (const auto &[parent_op_pos, parent_op_labels] :
						iter::enumerate(subscripts.raw_subscript.operands)) {
					OperandSc op_labels{};
					for (const Label label : parent_op_labels)
						if (label_subset.count(label))
							op_labels.emplace_back(label);

					if (not op_labels.empty()) {
						operands_labels.push_back(op_labels);
						original_op_poss.push_back(parent_op_pos);
					}
				}
				ResultSc result_labels{};
				for (const auto[result_pos, label] : iter::enumerate(subscripts.raw_subscript.result))
					if (label_subset.count(label)) {
						result_labels.emplace_back(label);
						original_result_poss.push_back(result_pos);
					}

				return {std::make_shared<Subscript>(operands_labels, result_labels),
				        original_op_poss,
				        original_result_poss};
			}

			std::size_t size() const noexcept {
				return sub_subscripts.size();
			}
		};

	private:
		mutable tsl::hopscotch_map<Label, std::shared_ptr<Subscript>> sub_subscripts{};

		RawSubscript raw_subscript{};

		tsl::hopscotch_set<Label> operands_label_set{};

		tsl::hopscotch_set<Label> result_label_set{};

		tsl::hopscotch_set<Label> lonely_non_result_labels{};

		DependencyGraph dependency_graph{};

		ConnectedComponents connected_components{};

		// Join
		mutable tsl::hopscotch_map<Label, LabelPossInOperands> label_poss_in_operands{};
		// Join & resolve
		mutable tsl::hopscotch_map<Label, LabelPos> label_poss_in_result{};
		// Resolve
		LabelPossInOperand operand2result_mapping_resolveType{};

		// Join
		tsl::hopscotch_map<Label, std::vector<OperandPos>> poss_of_operands_with_labels{};

		// Cartesian
		CartesianSubSubscripts cartesian_sub_subscripts;
	public:
		std::shared_ptr<Subscript> removeLabel(Label label) const {
			auto iterator = sub_subscripts.find(label);
			if (iterator != sub_subscripts.end())
				return iterator->second;
			else
				return sub_subscripts.insert(
								{label, std::make_shared<Subscript>(raw_subscript.removeLabel(label))})
						.first->second;
		}

		const tsl::hopscotch_set<Label> &getLonelyNonResultLabelSet() const {
			return lonely_non_result_labels;
		}

		const tsl::hopscotch_set<Label> &getOperandsLabelSet() const {
			return operands_label_set;
		}

		const tsl::hopscotch_set<Label> &getResultLabelSet() const {
			return result_label_set;
		}

		const std::vector<OperandPos> &getPossOfOperandsWithLabel(Label label) const {
			auto iterator = poss_of_operands_with_labels.find(label);
			if (iterator != poss_of_operands_with_labels.end())
				return iterator->second;
			else
				throw std::invalid_argument("label is not used in operands.");
		}

		/**
		 * for Join
		 * @param label
		 * @return
		 */
		const LabelPossInOperands &getLabelPossInOperands(const Label label) const {
			assert(operands_label_set.count(label));
			auto iterator = label_poss_in_operands.find(label);
			if (iterator != label_poss_in_operands.end())
				return iterator->second;
			else
				return label_poss_in_operands.insert(
								{label, raw_subscript.getLabelPossInOperands(label)})
						.first->second;
		}

		bool isResultLabel(const Label label) const noexcept {
			return result_label_set.count(label);
		}

		LabelPos getLabelPosInResult(const Label label) const {
			auto iterator = label_poss_in_result.find(label);
			if (iterator != label_poss_in_result.end())
				return iterator->second;
			else
				throw std::invalid_argument("label is not in result.");
		}

		[[nodiscard]] auto operandsCount() const noexcept {
			return raw_subscript.operandsCount();
		}

		[[nodiscard]] auto labelCount(const OperandPos operand_pos) const {
			return raw_subscript.labelCount(operand_pos);
		}

		[[nodiscard]] auto resultLabelCount() const noexcept {
			return raw_subscript.resultLabelCount();
		}

		[[nodiscard]] auto getOperandLabels(const OperandPos label_pos) const {
			assert(operandsCount() > label_pos);
			return raw_subscript.operands[label_pos];
		}

		auto operand2resultMapping_ResolveType() const noexcept {
			assert(type == Type::Resolve);
			return operand2result_mapping_resolveType;
		}

	public:
		Type type;
		mutable bool all_result_done;

		Subscript() = default;

		Subscript(const Subscript &) = default;

		Subscript(Subscript &) = default;

		Subscript(Subscript &&) = default;

		Subscript &operator=(const Subscript &) = default;

		Subscript &operator=(Subscript &) = default;

		Subscript(const std::string &subscript_str) : Subscript(from_string(subscript_str)) {}

		bool calcAllResultDone(const tsl::hopscotch_set<Label> &operand_labels,
		                       const tsl::hopscotch_set<Label> &result_labels) {
			for (auto result_label : result_labels) {
				if (operand_labels.count(result_label))
					return false;
			}
			return true;
		}

		Subscript(RawSubscript raw_subscript, Type type = Type::None)
				: raw_subscript(raw_subscript),
				  operands_label_set(raw_subscript.getOperandsLabelSet()),
				  result_label_set(raw_subscript.getResultLabelSet()),
				  dependency_graph(calcDependencyGraph(raw_subscript)),
				  connected_components(dependency_graph.getConnectedComponents()),
				  type((type == Type::CarthesianMapping) ? Type::CarthesianMapping : calcState(raw_subscript,
				                                                                            operands_label_set,
				                                                                            result_label_set,
				                                                                            connected_components)),
				  all_result_done(calcAllResultDone(operands_label_set, result_label_set)) {

			for (const auto [op_pos, labels] : iter::enumerate(raw_subscript.operands))
				for (const Label label : labels)
					poss_of_operands_with_labels[label].push_back(op_pos);

			for (auto label : operands_label_set) {
				if (not result_label_set.count(label)) {
					const auto &op_poss = poss_of_operands_with_labels[label];
					if (op_poss.size() == 1) {
						const auto &op = raw_subscript.operands[op_poss[0]];
						if (std::count(op.begin(), op.end(), label) == 1)
							lonely_non_result_labels.insert(label);
					}
				}
			}

			switch (this->type) {

				case Type::Join: {
					label_poss_in_result = raw_subscript.getLabelPossInResult();
					break;
				}

				case Type::Cartesian: {
					cartesian_sub_subscripts = {*this};
					break;
				}
				case Type::Resolve: {
					label_poss_in_result = raw_subscript.getLabelPossInResult();

					operand2result_mapping_resolveType.reserve(resultLabelCount());
					for (auto label: getOperandLabels(0))
						operand2result_mapping_resolveType.emplace_back(
								getLabelPosInResult(label)
						);
					break;
				}
				case Type::Count:
					break;
				case Type::EntryGenerator:
					break;
				case Type::None:
					break;
				case Type::CarthesianMapping:
					break;
			}
		}

		Subscript(OperandsSc operands, ResultSc result, Type type = Type::None) : Subscript{
				RawSubscript{std::move(operands), std::move(result)}, type} {}

		std::string to_string() const {
			std::vector<std::string> operand_strings;
			operand_strings.reserve(raw_subscript.operands.size());
			for (const auto &operand : raw_subscript.operands) {
				operand_strings.push_back(fmt::format("{}", fmt::join(operand, "")));
			}
			return fmt::format("{}->{}", fmt::join(operand_strings, ","), fmt::join(raw_subscript.result, ""));
		}

		static Subscript from_string(const std::string &subscript_str) {
			auto iter = subscript_str.cbegin();
			auto end = subscript_str.end();
			boost::container::flat_map<char, Label> char_mapping{};
			OperandsSc operands_sc{};
			ResultSc result_sc{};
			Label next_label = 'a';

			while (*iter != '-') {
				OperandSc &operand_sc = operands_sc.emplace_back();
				while (*iter != ',' and *iter != '-') {
					if (not char_mapping.count(*iter)) {
						char_mapping[*iter] = next_label++;
					}
					operand_sc.push_back(char_mapping[*iter]);
					++iter;
				}
				if (*iter == '-')
					break;
				++iter;
			}
			if (operands_sc.empty())
				operands_sc.emplace_back();
			iter = iter + 2;
			while (iter != end) {
				assert(char_mapping.count(*iter));
				result_sc.push_back(char_mapping[*iter]);
				++iter;
			}
			return {std::move(operands_sc), std::move(result_sc)};
		}

		const RawSubscript &getRawSubscript() const {
			return raw_subscript;
		}

	public:
		const CartesianSubSubscripts &getCartesianSubscript() const {
			return cartesian_sub_subscripts;
		}

	private:
		Type calcState(const RawSubscript &raw_subscript,
		               const tsl::hopscotch_set<Label> &operands_label_set,
		               const tsl::hopscotch_set<Label> &result_label_set,
		               const ConnectedComponents &connected_components) {
			switch (raw_subscript.operandsCount()) {
				case 0:
					return Type::EntryGenerator;
				case 1:
					// single operand
					if (auto operand_label_count = raw_subscript.labelCount(0);
							operand_label_count == operands_label_set.size()) {
						// at least one label occures multiple times and thus there is a diagonal
						// check if  all operand labels are either in result or all are not in result
						// none of both may also be the case
						bool all_in_result = true;
						bool none_in_result = true;
						for (auto label : operands_label_set) {
							if (result_label_set.count(label))
								none_in_result = false;
							else
								all_in_result = false;
						}
						if (all_in_result)
							return Type::Resolve;
						else if (none_in_result)
							return Type::Count;
					}
					break;
				default: {
					// multiple operands
					if (connected_components.size() > 1) {
						// more than one connected component means that there is a Cartesian product
						return Type::Cartesian;
					}
					break;
				}
			}
			return Type::Join;
		}

		static DependencyGraph calcDependencyGraph(const RawSubscript &raw_subscript) {
			DependencyGraph label_dependency_graph{};
			for (const auto &operand : raw_subscript.operands)
				label_dependency_graph.addCompleteGraph(operand);
			return label_dependency_graph;
		}

	public:
		bool operator!=(const Subscript &other) const { return hash() != other.hash(); };
		// TODO: estimate the risk of a hash collision
		// bool operator!=(const Subscript &other) const { return this->raw_subscript != other.raw_subscript; };

		[[nodiscard]] inline size_t hash() const { return raw_subscript.hash; }

		[[nodiscard]] bool valid() const {
			for (auto result_label : raw_subscript.result)
				if (not operands_label_set.count(result_label))
					return false;
			return true;
		}

	};

	using CartesianSubSubscripts = Subscript::CartesianSubSubscripts;
}

inline std::ostream &operator<<(std::ostream &stream, const std::shared_ptr<einsum::internal::Subscript> &sub_script) {
	stream << sub_script->to_string();
	return stream;
}

inline std::ostream &operator<<(std::ostream &stream, std::shared_ptr<einsum::internal::Subscript> sub_script) {
	stream << sub_script->to_string();
	return stream;
}

inline std::ostream &operator<<(std::ostream &stream, einsum::internal::Subscript const *const sub_script) {
	stream << sub_script->to_string();
	return stream;
}

inline std::ostream &operator<<(std::ostream &stream, const einsum::internal::Subscript &sub_script) {
	stream << sub_script.to_string();
	return stream;
}

namespace fmt {
	template<>
	struct formatter<std::shared_ptr<einsum::internal::Subscript>> {
		template<typename ParseContext>
		constexpr auto parse(ParseContext &ctx) { return ctx.begin(); }

		template<typename FormatContext>
		auto format(const std::shared_ptr<einsum::internal::Subscript> &sub_script, FormatContext &ctx) {
			return format_to(ctx.out(), "{}", sub_script->to_string());
		}
	};
}

template<>
struct std::hash<einsum::internal::Subscript> {
	size_t operator()(const einsum::internal::Subscript &s) const { return s.hash(); }
};

#endif //SPARSETENSOR_EINSUM_SUBSCRIPT_HPP


#include <pybind11/pybind11.h>
#include <pybind11/stl.h>
#include <pybind11/numpy.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <Eigen/Dense>
#include <string>
#include <vector>
#include <array>
#include <memory>
#include <stdexcept>
#include <iostream>
#include "../include/gpd/grasp_detector.h"
#include "../include/gpd/util/cloud.h"
#include "../include/gpd/candidate/hand.h"

// 3 for position, 9 for orientation, 1 for width, 1 for score
#define GRASP_DESC_LEN 14

/**
 *  Detect grasps from a given point cloud
 *  pcd: (n,3), each row is a point in the cloud
 *  visibility: (num_cams, n), each row is a camera, each column j denotes if
 *      the corresponding point is visible from the given camera.
 *  camera: (3, num_cams), each column j is the position of the camera.
 */
pybind11::array_t<double> detectGrasps(
    gpd::GraspDetector &detector,
    pybind11::array_t<double, pybind11::array::c_style> pcd,
    pybind11::array_t<double, pybind11::array::c_style> visibility,
    pybind11::array_t<double, pybind11::array::c_style> camera)
{
    if (pcd.ndim() != 2) {
        throw std::invalid_argument("PCD must have two dimensions");
    }
    ssize_t len = pcd.shape()[0];
    if (len == 0) {
        throw std::invalid_argument("PCD must more than 0 points");
    }
    if (pcd.shape()[1] < 3) {
        throw std::invalid_argument("Each point must have at least 3 coordinates");
    }
    ssize_t num_cams = camera.shape()[1];
    pcl::PointCloud<pcl::PointXYZRGBA> e_cloud;
    pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud_ptr = e_cloud.makeShared();
    for(ssize_t i = 0; i < len; i++){
        pcl::PointXYZRGBA pt;
        pt.x = pcd.at(i, 0);
        pt.y = pcd.at(i, 1);
        pt.z = pcd.at(i, 2);
        cloud_ptr->push_back(pt);
    }
    Eigen::MatrixXi vis(num_cams, len);
    for(ssize_t i = 0; i < num_cams; i++){
        for (ssize_t j = 0; j < len; j++){
            vis(i, j) = visibility.at(i, j);
        }
    }
    Eigen::Matrix3Xd view_points(3, num_cams);
    for(ssize_t i = 0; i < 3; i++){
        for(ssize_t j = 0; j < num_cams; j++){
            view_points(i, j) = camera.at(i, j);
        }
    }
    gpd::util::Cloud gpd_cloud(cloud_ptr, vis, view_points);
    detector.preprocessPointCloud(gpd_cloud);
    std::vector<std::unique_ptr<gpd::candidate::Hand>> grasps = detector.detectGrasps(gpd_cloud);
    std::vector<std::array<double, GRASP_DESC_LEN>> ret;
    for(size_t i = 0; i < grasps.size(); i++){
        const Eigen::Vector3d pos = grasps[i]->getPosition();
        const Eigen::Matrix3d rot = grasps[i]->getOrientation();
        const double w = grasps[i]->getGraspWidth();
        const double score = grasps[i]->getScore();
        std::array<double, GRASP_DESC_LEN> gd = {
            pos[0], pos[1], pos[2],
            rot(0,0), rot(0,1), rot(0,2),
            rot(1,0), rot(1,1), rot(1,2),
            rot(2,0), rot(2,1), rot(2,2),
            w, score
        };
        ret.push_back(gd);
    }
    return pybind11::cast(ret);
}

PYBIND11_MODULE(gpd_python, m) {
    pybind11::class_<gpd::GraspDetector>(m, "GraspDetector")
        .def(pybind11::init<const std::string &>());
    m.def("detectGrasps", &detectGrasps);
}


// Copyright © 2016-2021 Thomas Nagler and Thibault Vatter
//
// This file is part of the vinecopulib library and licensed under the terms of
// the MIT license. For a copy, see the LICENSE file in the root directory of
// vinecopulib or https://vinecopulib.github.io/vinecopulib/.

#pragma once

#include <boost/graph/adjacency_list.hpp>
#include <vinecopulib/bicop/class.hpp>
#include <vinecopulib/misc/tools_interface.hpp>
#include <vinecopulib/vinecop/fit_controls.hpp>
#include <vinecopulib/vinecop/rvine_structure.hpp>

// to allow for (auto e : boost::edges(g)) notation
namespace std {
template<class T>
T
begin(const std::pair<T, T>& eItPair)
{
  return eItPair.first;
}

template<class T>
T
end(const std::pair<T, T>& eItPair)
{
  return eItPair.second;
}
}
namespace vinecopulib {

namespace tools_select {

double
calculate_criterion(const Eigen::MatrixXd& data,
                    std::string tree_criterion,
                    Eigen::VectorXd weights);

Eigen::MatrixXd
calculate_criterion_matrix(const Eigen::MatrixXd& data,
                           const std::string& tree_criterion,
                           const Eigen::VectorXd& weights);

std::vector<size_t>
get_disc_cols(std::vector<std::string> var_types);

// boost::graph represenation of a vine tree
struct VertexProperties
{
  std::vector<size_t> conditioning;
  std::vector<size_t> conditioned;
  std::vector<size_t> all_indices;
  std::vector<size_t> prev_edge_indices;
  Eigen::VectorXd hfunc1;
  Eigen::VectorXd hfunc2;
  Eigen::VectorXd hfunc1_sub;
  Eigen::VectorXd hfunc2_sub;
  std::vector<std::string> var_types{ "c", "c" };
};
struct EdgeProperties
{
  std::vector<size_t> conditioning;
  std::vector<size_t> conditioned;
  std::vector<size_t> all_indices;
  Eigen::MatrixXd pc_data;
  Eigen::VectorXd hfunc1;
  Eigen::VectorXd hfunc2;
  Eigen::VectorXd hfunc1_sub;
  Eigen::VectorXd hfunc2_sub;
  std::vector<std::string> var_types{ "c", "c" };
  double weight;
  double crit;
  vinecopulib::Bicop pair_copula;
  double fit_id;
};
typedef boost::adjacency_list<
  boost::vecS,
  boost::vecS,
  boost::undirectedS,
  VertexProperties,
  boost::property<boost::edge_weight_t, double, EdgeProperties>>
  VineTree;

typedef boost::graph_traits<VineTree>::edge_descriptor EdgeIterator;
typedef std::pair<EdgeIterator, bool> FoundEdge;

class VinecopSelector
{
public:
  VinecopSelector(const Eigen::MatrixXd& data,
                  const FitControlsVinecop& controls,
                  std::vector<std::string> var_types);

  VinecopSelector(const Eigen::MatrixXd& data,
                  const RVineStructure& vine_struct,
                  const FitControlsVinecop& controls,
                  std::vector<std::string> var_types);

  std::vector<std::vector<Bicop>> get_pair_copulas() const;

  RVineStructure get_rvine_structure() const;

  static std::vector<std::vector<Bicop>> make_pair_copula_store(
    size_t d,
    size_t trunc_lvl);

  void select_all_trees(const Eigen::MatrixXd& data);

  void sparse_select_all_trees(const Eigen::MatrixXd& data);

  double get_loglik() const;

  double get_threshold() const;

  size_t get_nobs() const;

  std::vector<VineTree> get_trees() const { return trees_; };
  std::vector<VineTree> get_trees_opt() const { return trees_opt_; };

protected:
  virtual void select_tree(size_t t);

  void finalize(size_t trunc_lvl);

  double get_mbicv_of_tree(size_t t, double loglik);

  double get_loglik_of_tree(size_t t);

  double get_npars_of_tree(size_t t);

  void set_tree_to_indep(size_t t);

  void print_pair_copulas_of_tree(size_t);

  std::vector<double> get_thresholded_crits();

  void initialize_new_fit(const Eigen::MatrixXd& data);

  void set_current_fit_as_opt(const double& loglik);

  void add_allowed_edges(VineTree& vine_tree);

  void select_edges(VineTree& vine_tree);

  Eigen::MatrixXd get_pc_data(size_t v0, size_t v1, const VineTree& tree);

  Eigen::VectorXd get_hfunc(const VertexProperties& vertex_data, bool is_first);

  Eigen::VectorXd get_hfunc_sub(const VertexProperties& vertex_data,
                                bool is_first);

  ptrdiff_t find_common_neighbor(size_t v0, size_t v1, const VineTree& tree);

  virtual double compute_fit_id(const EdgeProperties& e);

  size_t n_;
  size_t d_;
  bool structure_known_{ true };
  std::vector<std::string> var_types_;
  FitControlsVinecop controls_;
  tools_thread::ThreadPool pool_;
  std::vector<VineTree> trees_;
  RVineStructure vine_struct_;
  std::vector<std::vector<Bicop>> pair_copulas_;
  // for sparse selction
  std::vector<VineTree> trees_opt_;
  double loglik_;
  double threshold_;
  double psi0_; // initial prior probability for mbicv

  double get_next_threshold(std::vector<double>& thresholded_crits);

  // functions for manipulation of trees ----------------
  VineTree make_base_tree(const Eigen::MatrixXd& data);

  VineTree edges_as_vertices(const VineTree& prev_tree);

  void min_spanning_tree(VineTree& tree);

  void add_edge_info(VineTree& tree);

  void add_pc_info(const EdgeIterator& e, VineTree& tree);

  void remove_edge_data(VineTree& tree);

  void remove_vertex_data(VineTree& tree);

  void select_pair_copulas(VineTree& tree,
                           const VineTree& tree_opt = VineTree());

  FoundEdge find_old_fit(double fit_id, const VineTree& old_graph);

  double get_tree_loglik(const VineTree& tree);

  double get_tree_npars(const VineTree& tree);

  size_t get_num_non_indeps_of_tree(size_t t);

  std::string get_pc_index(const EdgeIterator& e, const VineTree& tree);
};
}
}

#include <vinecopulib/vinecop/implementation/tools_select.ipp>


/** \file mean_functors.hpp \brief Mean (parametric) functions. */
/*
-------------------------------------------------------------------------
   This file is part of BayesOpt, an efficient C++ library for
   Bayesian optimization.

   Copyright (C) 2011-2015 Ruben Martinez-Cantin <rmcantin@unizar.es>

   BayesOpt is free software: you can redistribute it and/or modify it
   under the terms of the GNU Affero General Public License as published by
   the Free Software Foundation, either version 3 of the License, or
   (at your option) any later version.

   BayesOpt is distributed in the hope that it will be useful, but
   WITHOUT ANY WARRANTY; without even the implied warranty of
   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
   GNU Affero General Public License for more details.

   You should have received a copy of the GNU Affero General Public License
   along with BayesOpt.  If not, see <http://www.gnu.org/licenses/>.
------------------------------------------------------------------------
*/

#ifndef  _MEAN_FUNCTORS_HPP_
#define  _MEAN_FUNCTORS_HPP_

#include <map>
#include <boost/scoped_ptr.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>
#include "bayesopt/parameters.hpp"
#include "specialtypes.hpp"

namespace bayesopt
{

  /**\addtogroup ParametricFunctions
   *  \brief Set of parametric models for surrogate modelling
   */
  //@{

  /** \brief Interface for mean functors */
  class ParametricFunction
  {
  public:
    virtual int init(size_t input_dim) {return 0;};
    virtual int init(size_t input_dim, ParametricFunction* left,
		     ParametricFunction* right) {return 0;};

    virtual void setParameters(const vectord& params) = 0;
    virtual vectord getParameters() = 0;
    virtual size_t nParameters() = 0;

    virtual double getMean(const vectord& x) = 0;
    virtual vectord operator()(const vecOfvec& x)
    {
      vectord result(x.size());
      std::vector<vectord>::const_iterator x_it = x.begin();
      std::vector<vectord>::const_iterator x_end = x.end();
      vectord::iterator res_it = result.begin();
      while(x_it != x_end)
	{
	  *res_it++ = getMean(*x_it++);
	}
      return result;
    };

    virtual size_t nFeatures() = 0;
    virtual vectord getFeatures(const vectord& x) = 0;
    virtual matrixd getAllFeatures(const vecOfvec& x)
    {
      size_t nf = nFeatures();
      matrixd result(nf,x.size());

      for(size_t ii=0; ii< x.size(); ++ii)
	{
	  column(result,ii) = getFeatures(x[ii]);
	}

      return result;
    };

    virtual ~ParametricFunction(){};

  protected:
    size_t n_inputs;
  };

  template <typename MeanType> ParametricFunction * create_func()
  {
    return new MeanType();
  }

  /**
   * \brief Factory model for parametric functions
   * This factory is based on the libgp library by Manuel Blum
   *      https://bitbucket.org/mblum/libgp
   * which follows the squeme of GPML by Rasmussen and Nickisch
   *     http://www.gaussianprocess.org/gpml/code/matlab/doc/
   */
  class MeanFactory
  {
  public:
    MeanFactory ();
    virtual ~MeanFactory () {};

    ParametricFunction* create(std::string name, size_t input_dim);

  private:
    typedef ParametricFunction* (*create_func_definition)();
    std::map<std::string , MeanFactory::create_func_definition> registry;
  };

  class MeanModel
  {
  public:
    MeanModel(size_t dim, Parameters parameters);
    virtual ~MeanModel() {};

    ParametricFunction* getMeanFunc();

    void setParameters(const vectord &theta);
    vectord getParameters();
    size_t nParameters();

    vectord getFeatures(const vectord& x);
    void getFeatures(const vectord& x, vectord& kx);
    size_t nFeatures();

    void setPoints(const vecOfvec &x);
    void addNewPoint(const vectord &x);

    vectord muTimesFeat();
    double muTimesFeat(const vectord& x);

    /**
     * \brief Select the parametric part of the surrogate process.
     *
     * @param muv mean function parameters
     * @param smu std function parameters
     * @param m_name mean function name
     * @param dim number of input dimensions
     * @return error_code
     */
    void setMean (const vectord &muv, const vectord &smu,
		 std::string m_name, size_t dim);

    /** Wrapper of setMean for the C++ structure */
    void setMean (MeanParameters mean, size_t dim);

    matrixd mFeatM;           ///< Value of the mean features at the input points

  private:
    vectord mMu;                 ///< Mean of the parameters of the mean function
    vectord mS_Mu;    ///< Variance of the params of the mean function W=mS_Mu*I

    boost::scoped_ptr<ParametricFunction> mMean;    ///< Pointer to mean function
  };

  inline ParametricFunction* MeanModel::getMeanFunc()
  { return mMean.get(); }

  inline void MeanModel::setParameters(const vectord &theta)
  { mMean->setParameters(theta); };

  inline vectord MeanModel::getParameters(){return mMean->getParameters();};
  inline size_t MeanModel::nParameters(){return mMean->nParameters();};

  inline vectord MeanModel::getFeatures(const vectord& x)
  { return mMean->getFeatures(x); }

  inline void MeanModel::getFeatures(const vectord& x, vectord& kx)
  { kx = mMean->getFeatures(x); }

  inline size_t MeanModel::nFeatures()
  { return mMean->nFeatures(); }

  inline void MeanModel::setPoints(const vecOfvec &x)
  { mFeatM = mMean->getAllFeatures(x); };

  inline void MeanModel::addNewPoint(const vectord &x)
  {
    using boost::numeric::ublas::column;

    mFeatM.resize(mFeatM.size1(),mFeatM.size2()+1);
    column(mFeatM,mFeatM.size2()-1) = mMean->getFeatures(x);
  }

  inline vectord MeanModel::muTimesFeat()
  {  return boost::numeric::ublas::prod(mMu,mFeatM); }

  inline double MeanModel::muTimesFeat(const vectord& x)
  { return boost::numeric::ublas::inner_prod(mMu,mMean->getFeatures(x));}

  //@}

} //namespace bayesopt

#endif


#include "face.h"
#include "vertex.h"
#include "colmap.h"
#include "tinyply.h"

#include <Eigen/Core>

#include <fstream>
#include <string>

#define UNCERTAINTY_UB 0.0001
#define UNCERTAINTY_LB 0.00005

int main(int argc, const char * argv[])
{
    if (argc != 3)
    {
        std::cerr << "Usage: " << argv[0] << " input.ply points3d.bin" << std::endl;
        return 1;
    }

    const std::string finply = argv[1];
    const std::string rawname = finply.substr(0, finply.find_last_of('.'));

    const std::string foutply = rawname + "_out.ply";

    const std::string finpoints = argv[2];

    // Vector of Eigen::Vector3f storing vertex coordinates
    std::vector<meshtool::Vertex> vertices;
    std::vector<meshtool::Face> faces;

    std::vector<float> verts;
    std::vector<uint32_t> indices;
    std::vector<float> norms;
    std::vector<uint8_t> colors;
    uint32_t nverts, nfaces;

    // Read points from COLMAP
    std::vector<meshtool::Vertex> points3d = meshtool::ReadCOLMAP(finpoints);

    try
    {
        std::ifstream ss(finply, std::ios::binary);

        std::filebuf fb;
        fb.open(foutply, std::ios::out | std::ios::binary);
        std::ostream os(&fb);

        tinyply::PlyFile infile(ss);
        tinyply::PlyFile outfile;

        nverts = infile.request_properties_from_element("vertex", { "x", "y", "z" }, verts);
        nfaces = infile.request_properties_from_element("face", { "vertex_indices" }, indices, 3);

        // Allocate appropriate memory
        vertices.resize(nverts);
        faces.resize(nfaces);

        norms.resize(verts.size());
        colors.resize(verts.size());

        std::cout << "COLMAP file has " << points3d.size() << " points." << std::endl;
        std::cout << "Input PLY file has " << nverts << " points." << std::endl;

        infile.read(ss);

        // Convert vertices into Eigen vectors
        for (size_t i = 0; i < nverts; i++)
        {
            // There are 3 values in `verts` per vertex
            const size_t n = 3*i;
            meshtool::Vertex vertex(verts[n], verts[n+1], verts[n+2]);
            vertices[i] = vertex;

            // Find vertices that match to the COLMAP 3d points. Inflate a sphere
            // until we find at least one point within the sphere
            double radius = 0.005; // initial radius of 5 mm
            size_t num_points = 0;
            double uncertainty = 0;
            while (num_points == 0 && radius <= 1)
            {
                auto it = std::find_if(points3d.begin(), points3d.end(),
                    [&vertex, &radius](const meshtool::Vertex& p) {
                        return vertex.WithinRadius(p, radius);
                    }
                );

                while (it != points3d.end())
                {
                    uncertainty += it->Uncertainty();
                    num_points++;
                    it = std::find_if(++it, points3d.end(),
                        [&vertex, &radius](const meshtool::Vertex& p) {
                            return vertex.WithinRadius(p, radius);
                        }
                    );
                }

                // If no points were found, increase radius
                if (num_points == 0)
                {
                    radius += 0.005;
                }
            }

            if (num_points > 0)
            {
                vertex.SetUncertainty(uncertainty / num_points);
                if (vertex.Uncertainty() > UNCERTAINTY_UB)
                {
                    vertex.SetColor({ 255, 0, 0 });
                }
                else if (vertex.Uncertainty() < UNCERTAINTY_LB)
                {
                    vertex.SetColor({ 0, 255, 0 });
                }
                else
                {
                    const uint8_t r = static_cast<uint8_t>(nearbyint((255 / (UNCERTAINTY_UB - UNCERTAINTY_LB)) * (vertex.Uncertainty() - UNCERTAINTY_LB)));
                    const uint8_t g = 255 - r;
                    vertex.SetColor({ r, g, 0 });
                }
            }

            colors[n] = vertex.Color(0);
            colors[n+1] = vertex.Color(1);
            colors[n+2] = vertex.Color(2);
        }

        // Get vertex indices for each face. The `faces` vector contains the
        // ordered indices of the vertices of each face, three per face.
        for (size_t i = 0; i < nfaces; i++)
        {
            // There are 3 indices in `faces` per face
            const size_t n = 3*i;

            meshtool::Face face(
                    vertices[indices[n]],
                    vertices[indices[n+1]],
                    vertices[indices[n+2]]
            );

            faces[i] = face;
        }

        // Iterate through the vertices one more time to populate the `norms'
        // vector. We MUST do this AFTER calculating the `faces' vector,
        // because the Face constructor calculates the normals for each Vertex
        // that makes up a Face
        for (size_t i = 0; i < nverts; i++)
        {
            const size_t n = 3*i;
            meshtool::Vertex vertex = vertices[i];
            norms[n] = static_cast<float>(vertex.Normal(0));
            norms[n+1] = static_cast<float>(vertex.Normal(1));
            norms[n+2] = static_cast<float>(vertex.Normal(2));
        }

        outfile.add_properties_to_element("vertex", { "x", "y", "z" }, verts);
        outfile.add_properties_to_element("vertex", { "nx", "ny", "nz" }, norms);
        outfile.add_properties_to_element("vertex", { "red", "green", "blue" }, colors);
        outfile.add_properties_to_element("face", { "vertex_indices" }, indices, 3, tinyply::PlyProperty::Type::UINT8);
        outfile.comments.emplace_back("Generated by meshtool");
        outfile.write(os, true);

        fb.close();

    }
    catch (const std::exception & e)
    {
        std::cerr << "Caught exception: " << e.what() << std::endl;
    }

    return 0;
}


//==============================================================================
//         Copyright 2014 - Jean-Thierry Lapresté
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2013   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef NT2_LINALG_FUNCTIONS_FACTORIZATIONS_SCHUR_HPP_INCLUDED
#define NT2_LINALG_FUNCTIONS_FACTORIZATIONS_SCHUR_HPP_INCLUDED

#include <nt2/include/functions/schur.hpp>
#include <boost/dispatch/attributes.hpp>
#include <nt2/core/container/table/table.hpp>
#include <nt2/include/functions/geesx.hpp>
#include <nt2/include/functions/geesx_no_w.hpp>
#include <nt2/include/functions/geesx1.hpp>
#include <nt2/include/functions/geesxw.hpp>
#include <nt2/include/functions/height.hpp>
#include <nt2/include/functions/isreal.hpp>
#include <nt2/include/functions/of_size.hpp>
#include <nt2/include/functions/real.hpp>
#include <nt2/include/functions/resize.hpp>
#include <nt2/include/functions/tie.hpp>
#include <nt2/include/functions/issquare.hpp>
#include <nt2/sdk/meta/as_real.hpp>
#include <nt2/sdk/complex/meta/is_complex.hpp>
#include <nt2/linalg/options.hpp>
#include <nt2/linalg/details/utility/lapack_verify.hpp>
#include <nt2/core/container/dsl/as_terminal.hpp>
#include <nt2/core/utility/assign_swap.hpp>
#include <boost/assert.hpp>
#include <boost/mpl/bool.hpp>
#include <boost/mpl/if.hpp>
#include <boost/type_traits/is_same.hpp>
#include <boost/dispatch/meta/strip.hpp>

namespace nt2 { namespace ext
{
  //============================================================================
  //SCHUR Scalar
  //============================================================================
  BOOST_DISPATCH_IMPLEMENT  ( schur_, tag::cpu_
                            , (A0)
                            , (scalar_<unspecified_<A0> >)
                            )
  {
    typedef A0 result_type;

    BOOST_FORCEINLINE result_type operator()(const A0& a0) const
    {
      return a0;
    }
  };

  BOOST_DISPATCH_IMPLEMENT  ( schur_, tag::cpu_
                            , (A0)(A1)
                            , (scalar_<unspecified_<A0> >)
                              (unspecified_<A1>)
                            )
  {
    typedef A0                                                      otype_t;
    typedef typename nt2::meta::as_real<A0>::type                   rtype_t;
    typedef typename meta::strip<A1>::type                            opt_t;
    typedef typename boost::mpl::if_ < boost::is_same< opt_t, nt2::policy<ext::real_> >
                              , rtype_t
                              , otype_t
                              >::type                           result_type;

    BOOST_FORCEINLINE result_type operator()(const A0& a0, const A1&) const
    {
      return eval(a0, opt_t());
    }
  private :
    template < class T >
    BOOST_FORCEINLINE result_type eval( const A0& a0, const T& ) const
    {
      return a0;
    }

    BOOST_FORCEINLINE result_type eval( const A0& a0, nt2::policy<ext::real_> const & ) const
    {
      BOOST_ASSERT_MSG(isreal(a0),
                       "all input matrix elements are to be real to support"
                       "'real_' option with complex type input");
      return real(a0);
    }

  };

  //============================================================================
  //SCHUR
  //============================================================================
  BOOST_DISPATCH_IMPLEMENT  ( schur_, tag::cpu_
                            , (A0)(N0)(A1)(N1)
                            , ((node_<A0, nt2::tag::schur_
                                    , N0, nt2::container::domain
                                      >
                              ))
                              ((node_<A1, nt2::tag::tie_
                                    , N1, nt2::container::domain
                                     >
                              ))
                            )
  {
    typedef void  result_type;
    typedef typename boost::proto::result_of::child_c<A0&,0>::value_type child0;
    typedef typename child0::value_type                                  type_t;
    typedef typename nt2::meta::as_complex<type_t>::type                ctype_t;
    typedef typename nt2::meta::as_real<type_t>::type                   rtype_t;
    typedef rtype_t T;
    typedef nt2::memory::container<tag::table_,  type_t, nt2::_2D>   o_semantic;
    typedef nt2::memory::container<tag::table_, rtype_t, nt2::_2D>   r_semantic;
    typedef nt2::memory::container<tag::table_, ctype_t, nt2::_2D>   c_semantic;

    BOOST_FORCEINLINE result_type operator()( A0& a0, A1& a1 ) const
    {
      BOOST_ASSERT_MSG(issquare(boost::proto::child_c<0>(a0)),"first input must be square");
      eval(a0, a1, N0(), N1());
    }
  private:
    //==========================================================================
    /// INTERNAL ONLY - T = SCHUR(A)
    // returns the schur matrix
    BOOST_FORCEINLINE
    void eval ( A0& a0, A1& a1
              , boost::mpl::long_<1> const&
              , boost::mpl::long_<1> const&
              ) const
    {
      NT2_AS_TERMINAL_INOUT(o_semantic, a, boost::proto::child_c<0>(a0), boost::proto::child_c<0>(a1));
      NT2_LAPACK_VERIFY(nt2::geesx(boost::proto::value(a), type_t(0)));
      boost::proto::child_c<0>(a1) = a;
    }

    //==========================================================================
    /// INTERNAL ONLY - T = SCHUR(A, cmplx_/real_)
    // returns T in its real or complex form or only the eigenvalues
    BOOST_FORCEINLINE
    void eval ( A0& a0, A1& a1
              , boost::mpl::long_<2> const&
              , boost::mpl::long_<1> const&
              ) const
    {
       eval1_2(a0, a1,
               boost::proto::value(boost::proto::child_c<1>(a0)));
    }

    //==========================================================================
    /// INTERNAL ONLY
    BOOST_FORCEINLINE
    void eval1_2 ( A0& a0, A1& a1
                 , nt2::policy<ext::real_>
                 ) const
    {
      BOOST_ASSERT_MSG(isreal(boost::proto::child_c<0>(a0)),
                       "all input matrix elements are to be real to support"
                       "'real_' option with complex type input");
      NT2_AS_TERMINAL_INOUT(r_semantic, t
                           , real(boost::proto::child_c<0>(a0))
                           , boost::proto::child_c<0>(a1));
      // Here one cannot be sure that boost::proto::child_c<0>(a1) is real typed
      // as the 'real'schur decomposition can be put in a complex table
      // so we cannot pass it to geesx that needs a real table
      NT2_LAPACK_VERIFY(nt2::geesx(boost::proto::value(t), rtype_t(0)));
      assign_swap(boost::proto::child_c<0>(a1), t);
    }

    //==========================================================================
    /// INTERNAL ONLY
    BOOST_FORCEINLINE
    void eval1_2 ( A0& a0, A1& a1
                 , nt2::policy<ext::eigs_>
                 ) const
    {
      nt2::container::table<type_t> work;
      NT2_AS_TERMINAL_INOUT(o_semantic, t
                           , boost::proto::child_c<0>(a0), work);
      NT2_AS_TERMINAL_OUT  (c_semantic, w
                           , boost::proto::child_c<0>(a1));
      NT2_LAPACK_VERIFY( nt2::geesxw(boost::proto::value(t),
                                     boost::proto::value(w)));
      w.resize(of_size(height(t), 1));
      assign_swap(boost::proto::child_c<0>(a1), w);
   }

    //==========================================================================
    /// INTERNAL ONLY
    BOOST_FORCEINLINE
    void eval1_2 ( A0& a0, A1& a1
                 , nt2::policy<ext::cmplx_>
                 ) const
    {
      typedef typename meta::is_complex<type_t>::type is_cmplx_t;
      eval1_2c(a0, a1, is_cmplx_t());
    }

    BOOST_FORCEINLINE
    void eval1_2c ( A0& a0, A1& a1
                 , boost::mpl::false_
                 ) const
    {
      // Here boost::proto::child_c<0>(a0) is real and geesx has to receive a complex table
      NT2_AS_TERMINAL_INOUT(c_semantic, t, boost::proto::child_c<0>(a0), boost::proto::child_c<0>(a1));
      NT2_LAPACK_VERIFY(nt2::geesx(boost::proto::value(t), ctype_t(0)));
      assign_swap(boost::proto::child_c<0>(a1), t);
    }

    BOOST_FORCEINLINE
    void eval1_2c ( A0& a0, A1& a1
                 , boost::mpl::true_
                 ) const
    {
      // Here  boost::proto::child_c<0>(a0) is complex
      NT2_AS_TERMINAL_INOUT(c_semantic, t
                           , boost::proto::child_c<0>(a0), boost::proto::child_c<0>(a1));
      NT2_LAPACK_VERIFY(nt2::geesx(boost::proto::value(t), ctype_t(0)));
      assign_swap(boost::proto::child_c<0>(a1), t);
     }

    //==========================================================================
    /// INTERNAL ONLY - [U, T]= SCHUR(A)
    BOOST_FORCEINLINE
    void eval ( A0& a0, A1& a1
              , boost::mpl::long_<1> const&
              , boost::mpl::long_<2> const&
              ) const
    {
      NT2_AS_TERMINAL_INOUT(o_semantic, t
                           , boost::proto::child_c<0>(a0), boost::proto::child_c<1>(a1));
      NT2_AS_TERMINAL_OUT  (o_semantic, u
                           , boost::proto::child_c<0>(a1));
      size_t n = height(t);
      u.resize(of_size(n, n));
      NT2_LAPACK_VERIFY(nt2::geesx_no_w( boost::proto::value(t)
                                       , boost::proto::value(u)
                                   ));
      assign_swap(boost::proto::child_c<0>(a1), u);
      assign_swap(boost::proto::child_c<1>(a1), t);
    }

    //==========================================================================
    /// INTERNAL ONLY - [T, U]= SCHUR(A, sort_/cmplx_/real_ )
    BOOST_FORCEINLINE
    void eval ( A0& a0, A1& a1
              , boost::mpl::long_<2> const&
              , boost::mpl::long_<2> const&
              ) const
    {
      eval2_2( a0, a1
             , boost::proto::value(boost::proto::child_c<1>(a0)));

    }

    //==========================================================================
    /// INTERNAL ONLY
    BOOST_FORCEINLINE
    void eval2_2 ( A0& a0, A1& a1
                 , nt2::policy<ext::real_>
                 ) const
    {
      BOOST_ASSERT_MSG(isreal(boost::proto::child_c<0>(a0)),
                       "all input matrix elements are to be real to support "
                       "'real_' option with complex type input");
      NT2_AS_TERMINAL_INOUT(r_semantic, t
                           , real(boost::proto::child_c<0>(a0)), boost::proto::child_c<1>(a1));
      NT2_AS_TERMINAL_OUT  (r_semantic, u
                           , boost::proto::child_c<0>(a1));
      size_t n = height(t);
      u.resize(of_size(n, n));
      NT2_LAPACK_VERIFY(nt2::geesx_no_w( boost::proto::value(t)
                                       , boost::proto::value(u)
                                   ));
      assign_swap(boost::proto::child_c<0>(a1), u);
      assign_swap(boost::proto::child_c<1>(a1), t);
    }

    //==========================================================================
    /// INTERNAL ONLY
    BOOST_FORCEINLINE
    void eval2_2 ( A0& a0, A1& a1
                 , nt2::policy<ext::cmplx_>
                 ) const
    {
      NT2_AS_TERMINAL_INOUT(c_semantic, t
                           , boost::proto::child_c<0>(a0), boost::proto::child_c<1>(a1));
      NT2_AS_TERMINAL_OUT  (c_semantic, u
                           , boost::proto::child_c<0>(a1));
      size_t n = nt2::height(t);
      u.resize(of_size(n, n));
      NT2_LAPACK_VERIFY(nt2::geesx_no_w( boost::proto::value(t)
                                       , boost::proto::value(u)
                                   ));
      assign_swap(boost::proto::child_c<0>(a1), u);
      assign_swap(boost::proto::child_c<1>(a1), t);
    }

    //==========================================================================
    /// INTERNAL ONLY - [U, T, W]= SCHUR(A)
    BOOST_FORCEINLINE
    void eval ( A0& a0, A1& a1
              , boost::mpl::long_<1> const&
              , boost::mpl::long_<3> const&
              ) const
    {
      NT2_AS_TERMINAL_INOUT(o_semantic, t
                           , boost::proto::child_c<0>(a0), boost::proto::child_c<1>(a1));
      NT2_AS_TERMINAL_OUT  (o_semantic, u
                           , boost::proto::child_c<0>(a1));
      NT2_AS_TERMINAL_OUT  (c_semantic, w
                           , boost::proto::child_c<2>(a1));
      size_t n =   height(t);
      u.resize(of_size(n, n));
      w.resize(of_size(n, 1));
      NT2_LAPACK_VERIFY(nt2::geesx( boost::proto::value(t)
                                  , boost::proto::value(w)
                                  , boost::proto::value(u)
                              ));
      assign_swap(boost::proto::child_c<0>(a1), u);
      assign_swap(boost::proto::child_c<1>(a1), t);
      assign_swap(boost::proto::child_c<2>(a1), w);

    }
    //==========================================================================
    /// INTERNAL ONLY - [U, T, W]= SCHUR(A, real_/complex_)
    BOOST_FORCEINLINE
    void eval ( A0& a0, A1& a1
              , boost::mpl::long_<2> const&
              , boost::mpl::long_<3> const&
              ) const
    {
      eval3_2(a0, a1,
              boost::proto::value(boost::proto::child_c<1>(a0)));
    }

    //==========================================================================
    /// INTERNAL ONLY
    BOOST_FORCEINLINE
    void eval3_2 ( A0& a0, A1& a1
                 , nt2::policy<ext::cmplx_>
                 ) const
    {
      NT2_AS_TERMINAL_INOUT(c_semantic, t
                           , boost::proto::child_c<0>(a0), boost::proto::child_c<1>(a1));
      NT2_AS_TERMINAL_OUT  (c_semantic, u
                           , boost::proto::child_c<0>(a1));
      NT2_AS_TERMINAL_OUT  (c_semantic, w
                           , boost::proto::child_c<2>(a1));
      size_t n = nt2::height(t);
      u.resize(of_size(n, n));
      w.resize(of_size(n, 1));
      NT2_LAPACK_VERIFY(nt2::geesx( boost::proto::value(t)
                                  , boost::proto::value(w)
                                  , boost::proto::value(u)
                                  ));
      assign_swap(boost::proto::child_c<0>(a1), u);
      assign_swap(boost::proto::child_c<1>(a1), t);
      assign_swap(boost::proto::child_c<2>(a1), w);
    }

    //==========================================================================
    /// INTERNAL ONLY
    BOOST_FORCEINLINE
    void eval3_2 ( A0& a0, A1& a1
                 , nt2::policy<ext::real_>
                 ) const
    {
      BOOST_ASSERT_MSG(isreal(boost::proto::child_c<0>(a0)),
                       "all input matrix elements are to be real to support "
                       "'real_' option with complex type input");
      NT2_AS_TERMINAL_INOUT(r_semantic, t
                           , real(boost::proto::child_c<0>(a0)), boost::proto::child_c<1>(a1));
      NT2_AS_TERMINAL_OUT  (r_semantic, u
                           , boost::proto::child_c<0>(a1));
      NT2_AS_TERMINAL_OUT  (c_semantic, w
                           , boost::proto::child_c<2>(a1));
      size_t n = nt2::height(t);
      u.resize(of_size(n, n));
      w.resize(of_size(n, 1));
      NT2_LAPACK_VERIFY(nt2::geesx( boost::proto::value(t)
                                  , boost::proto::value(w)
                                  , boost::proto::value(u)
                                  ));
      assign_swap(boost::proto::child_c<0>(a1), u);
      assign_swap(boost::proto::child_c<1>(a1), t);
      assign_swap(boost::proto::child_c<2>(a1), w);
    }
  };

} }

#endif


#pragma once

#include <Eigen/Core>
#include <ipc/utils/eigen_ext.hpp>

namespace ipc {

// Discrete collision detection

bool point_edge_aabb_cd(
    const VectorMax3d& p,
    const VectorMax3d& e0,
    const VectorMax3d& e1,
    double dist);

bool edge_edge_aabb_cd(
    const VectorMax3d& ea0,
    const VectorMax3d& ea1,
    const VectorMax3d& eb0,
    const VectorMax3d& eb1,
    double dist);

bool point_triangle_aabb_cd(
    const Eigen::Vector3d& p,
    const Eigen::Vector3d& t0,
    const Eigen::Vector3d& t1,
    const Eigen::Vector3d& t2,
    double dist);

bool edge_triangle_aabb_cd(
    const Eigen::Vector3d& e0,
    const Eigen::Vector3d& e1,
    const Eigen::Vector3d& t0,
    const Eigen::Vector3d& t1,
    const Eigen::Vector3d& t2,
    double dist);

// Continous collision detection

bool point_edge_aabb_ccd(
    const VectorMax3d& p_t0,
    const VectorMax3d& e0_t0,
    const VectorMax3d& e1_t0,
    const VectorMax3d& p_t1,
    const VectorMax3d& e0_t1,
    const VectorMax3d& e1_t1,
    double dist);

bool edge_edge_aabb_ccd(
    const Eigen::Vector3d& ea0_t0,
    const Eigen::Vector3d& ea1_t0,
    const Eigen::Vector3d& eb0_t0,
    const Eigen::Vector3d& eb1_t0,
    const Eigen::Vector3d& ea0_t1,
    const Eigen::Vector3d& ea1_t1,
    const Eigen::Vector3d& eb0_t1,
    const Eigen::Vector3d& eb1_t1,
    double dist);

bool point_triangle_aabb_ccd(
    const Eigen::Vector3d& p_t0,
    const Eigen::Vector3d& t0_t0,
    const Eigen::Vector3d& t1_t0,
    const Eigen::Vector3d& t2_t0,
    const Eigen::Vector3d& p_t1,
    const Eigen::Vector3d& t0_t1,
    const Eigen::Vector3d& t1_t1,
    const Eigen::Vector3d& t2_t1,
    double dist);

} // namespace ipc