// Copyright Louis Dionne 2013-2017
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)

#include <boost/hana/assert.hpp>
#include <boost/hana/at_key.hpp>
#include <boost/hana/equal.hpp>
#include <boost/hana/find.hpp>
#include <boost/hana/integral_constant.hpp>
#include <boost/hana/optional.hpp>
#include <boost/hana/set.hpp>
namespace hana = boost::hana;

int main() {
    constexpr auto xs = hana::make_set(hana::int_c<0>, hana::int_c<1>, hana::int_c<2>);
    BOOST_HANA_CONSTANT_CHECK(hana::find(xs, hana::int_c<0>) == hana::just(hana::int_c<0>));
    BOOST_HANA_CONSTANT_CHECK(hana::find(xs, hana::int_c<3>) == hana::nothing);

    // operator[] is equivalent to at_key
    BOOST_HANA_CONSTANT_CHECK(xs[hana::int_c<2>] == hana::int_c<2>);

    // long_c<0> == int_<0>, and therefore int_<0> is found
    BOOST_HANA_CONSTANT_CHECK(xs[hana::long_c<0>] == hana::int_c<0>);
}


// r_c_shortest_paths.hpp header file

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

#ifndef BOOST_GRAPH_R_C_SHORTEST_PATHS_HPP
#define BOOST_GRAPH_R_C_SHORTEST_PATHS_HPP

#include <map>
#include <queue>
#include <vector>
#include <list>

#include <boost/make_shared.hpp>
#include <boost/enable_shared_from_this.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/iteration_macros.hpp>
#include <boost/property_map/property_map.hpp>

namespace boost {

// r_c_shortest_paths_label struct
template<class Graph, class Resource_Container>
struct r_c_shortest_paths_label : public boost::enable_shared_from_this<r_c_shortest_paths_label<Graph, Resource_Container> >
{
  r_c_shortest_paths_label
  ( const unsigned long n,
    const Resource_Container& rc = Resource_Container(),
    const boost::shared_ptr<r_c_shortest_paths_label<Graph, Resource_Container> > pl = boost::shared_ptr<r_c_shortest_paths_label<Graph, Resource_Container> >(),
    const typename graph_traits<Graph>::edge_descriptor& ed = graph_traits<Graph>::edge_descriptor(),
    const typename graph_traits<Graph>::vertex_descriptor& vd = graph_traits<Graph>::vertex_descriptor() )
  : num( n ),
    cumulated_resource_consumption( rc ),
    p_pred_label( pl ),
    pred_edge( ed ),
    resident_vertex( vd ),
    b_is_dominated( false ),
    b_is_processed( false )
  {}

  r_c_shortest_paths_label& operator=( const r_c_shortest_paths_label& other )
  {
    if( this == &other )
      return *this;
    this->~r_c_shortest_paths_label();
    new( this ) r_c_shortest_paths_label( other );
    return *this;
  }
  const unsigned long num;
  Resource_Container cumulated_resource_consumption;
  const boost::shared_ptr<r_c_shortest_paths_label<Graph, Resource_Container> > p_pred_label;
  const typename graph_traits<Graph>::edge_descriptor pred_edge;
  const typename graph_traits<Graph>::vertex_descriptor resident_vertex;
  bool b_is_dominated;
  bool b_is_processed;
}; // r_c_shortest_paths_label

template<class Graph, class Resource_Container>
inline bool operator==
( const r_c_shortest_paths_label<Graph, Resource_Container>& l1,
  const r_c_shortest_paths_label<Graph, Resource_Container>& l2 )
{
  return
    l1.cumulated_resource_consumption == l2.cumulated_resource_consumption;
}

template<class Graph, class Resource_Container>
inline bool operator!=
( const r_c_shortest_paths_label<Graph, Resource_Container>& l1,
  const r_c_shortest_paths_label<Graph, Resource_Container>& l2 )
{
  return
    !( l1 == l2 );
}

template<class Graph, class Resource_Container>
inline bool operator<
( const r_c_shortest_paths_label<Graph, Resource_Container>& l1,
  const r_c_shortest_paths_label<Graph, Resource_Container>& l2 )
{
  return
    l1.cumulated_resource_consumption < l2.cumulated_resource_consumption;
}

template<class Graph, class Resource_Container>
inline bool operator>
( const r_c_shortest_paths_label<Graph, Resource_Container>& l1,
  const r_c_shortest_paths_label<Graph, Resource_Container>& l2 )
{
  return
    l2.cumulated_resource_consumption < l1.cumulated_resource_consumption;
}

template<class Graph, class Resource_Container>
inline bool operator<=
( const r_c_shortest_paths_label<Graph, Resource_Container>& l1,
  const r_c_shortest_paths_label<Graph, Resource_Container>& l2 )
{
  return
    l1 < l2 || l1 == l2;
}

template<class Graph, class Resource_Container>
inline bool operator>=
( const r_c_shortest_paths_label<Graph, Resource_Container>& l1,
  const r_c_shortest_paths_label<Graph, Resource_Container>& l2 )
{
  return l2 < l1 || l1 == l2;
}

template<typename Graph, typename Resource_Container>
inline bool operator<
        ( const boost::shared_ptr<r_c_shortest_paths_label<Graph, Resource_Container> > &t,
          const boost::shared_ptr<r_c_shortest_paths_label<Graph, Resource_Container> > &u) {
    return *t < *u;
}

template<typename Graph, typename Resource_Container>
inline bool operator<=( const boost::shared_ptr<r_c_shortest_paths_label<Graph, Resource_Container> > &t,
                        const boost::shared_ptr<r_c_shortest_paths_label<Graph, Resource_Container> > &u ) {
    return *t <= *u;
}

template<typename Graph, typename Resource_Container>
inline bool operator>
        (
          const boost::shared_ptr<r_c_shortest_paths_label<Graph, Resource_Container> > &t,
          const boost::shared_ptr<r_c_shortest_paths_label<Graph, Resource_Container> > &u ) {
    return *t > *u;
}

template<typename Graph, typename Resource_Container>
inline bool operator>=
        (
          const boost::shared_ptr<r_c_shortest_paths_label<Graph, Resource_Container> > &t,
          const boost::shared_ptr<r_c_shortest_paths_label<Graph, Resource_Container> > &u) {
    return *t >= *u;
}

namespace detail {

// r_c_shortest_paths_dispatch function (body/implementation)
template<class Graph,
         class VertexIndexMap,
         class EdgeIndexMap,
         class Resource_Container,
         class Resource_Extension_Function,
         class Dominance_Function,
         class Label_Allocator,
         class Visitor>
void r_c_shortest_paths_dispatch
( const Graph& g,
  const VertexIndexMap& vertex_index_map,
  const EdgeIndexMap& /*edge_index_map*/,
  typename graph_traits<Graph>::vertex_descriptor s,
  typename graph_traits<Graph>::vertex_descriptor t,
  // each inner vector corresponds to a pareto-optimal path
  std::vector
    <std::vector
      <typename graph_traits
        <Graph>::edge_descriptor> >& pareto_optimal_solutions,
  std::vector
    <Resource_Container>& pareto_optimal_resource_containers,
  bool b_all_pareto_optimal_solutions,
  // to initialize the first label/resource container
  // and to carry the type information
  const Resource_Container& rc,
  Resource_Extension_Function& ref,
  Dominance_Function& dominance,
  // to specify the memory management strategy for the labels
  Label_Allocator /*la*/,
  Visitor vis )
{
  pareto_optimal_resource_containers.clear();
  pareto_optimal_solutions.clear();

  size_t i_label_num = 0;
#if defined(BOOST_NO_CXX11_ALLOCATOR)
  typedef
    typename
      Label_Allocator::template rebind
        <r_c_shortest_paths_label
          <Graph, Resource_Container> >::other LAlloc;
#else
  typedef
     typename
     std::allocator_traits<Label_Allocator>::template rebind_alloc
     <r_c_shortest_paths_label
     <Graph, Resource_Container> > LAlloc;
  typedef std::allocator_traits<LAlloc> LTraits;
#endif
  LAlloc l_alloc;
  typedef
    boost::shared_ptr<r_c_shortest_paths_label<Graph, Resource_Container> > Splabel;
  std::priority_queue<Splabel, std::vector<Splabel>, std::greater<Splabel> >
    unprocessed_labels;

  bool b_feasible = true;
  Splabel splabel_first_label = boost::allocate_shared<r_c_shortest_paths_label<Graph, Resource_Container> >(
          l_alloc,
          i_label_num++,
          rc,
          boost::shared_ptr<r_c_shortest_paths_label<Graph, Resource_Container> >(),
          typename graph_traits<Graph>::edge_descriptor(),
          s );

  unprocessed_labels.push( splabel_first_label );
  std::vector<std::list<Splabel> > vec_vertex_labels_data( num_vertices( g ) );
  iterator_property_map<typename std::vector<std::list<Splabel> >::iterator,
                        VertexIndexMap>
    vec_vertex_labels(vec_vertex_labels_data.begin(), vertex_index_map);
  vec_vertex_labels[s].push_back( splabel_first_label );
  typedef
    std::vector<typename std::list<Splabel>::iterator>
    vec_last_valid_positions_for_dominance_data_type;
  vec_last_valid_positions_for_dominance_data_type
    vec_last_valid_positions_for_dominance_data( num_vertices( g ) );
  iterator_property_map<
      typename vec_last_valid_positions_for_dominance_data_type::iterator,
      VertexIndexMap>
    vec_last_valid_positions_for_dominance
      (vec_last_valid_positions_for_dominance_data.begin(),
       vertex_index_map);
  BGL_FORALL_VERTICES_T(v, g, Graph) {
    put(vec_last_valid_positions_for_dominance, v, vec_vertex_labels[v].begin());
  }
  std::vector<size_t> vec_last_valid_index_for_dominance_data( num_vertices( g ), 0 );
  iterator_property_map<std::vector<size_t>::iterator, VertexIndexMap>
    vec_last_valid_index_for_dominance
      (vec_last_valid_index_for_dominance_data.begin(), vertex_index_map);
  std::vector<bool>
    b_vec_vertex_already_checked_for_dominance_data( num_vertices( g ), false );
  iterator_property_map<std::vector<bool>::iterator, VertexIndexMap>
    b_vec_vertex_already_checked_for_dominance
      (b_vec_vertex_already_checked_for_dominance_data.begin(),
       vertex_index_map);

  while( !unprocessed_labels.empty()  && vis.on_enter_loop(unprocessed_labels, g) )
  {
    Splabel cur_label = unprocessed_labels.top();
    unprocessed_labels.pop();
    vis.on_label_popped( *cur_label, g );
    // an Splabel object in unprocessed_labels and the respective Splabel
    // object in the respective list<Splabel> of vec_vertex_labels share their
    // embedded r_c_shortest_paths_label object
    // to avoid memory leaks, dominated
    // r_c_shortest_paths_label objects are marked and deleted when popped
    // from unprocessed_labels, as they can no longer be deleted at the end of
    // the function; only the Splabel object in unprocessed_labels still
    // references the r_c_shortest_paths_label object
    // this is also for efficiency, because the else branch is executed only
    // if there is a chance that extending the
    // label leads to new undominated labels, which in turn is possible only
    // if the label to be extended is undominated
    if( !cur_label->b_is_dominated )
    {
      typename boost::graph_traits<Graph>::vertex_descriptor
        i_cur_resident_vertex = cur_label->resident_vertex;
      std::list<Splabel>& list_labels_cur_vertex =
        get(vec_vertex_labels, i_cur_resident_vertex);
      if( list_labels_cur_vertex.size() >= 2
          && vec_last_valid_index_for_dominance[i_cur_resident_vertex]
               < list_labels_cur_vertex.size() )
      {
        typename std::list<Splabel>::iterator outer_iter =
          list_labels_cur_vertex.begin();
        bool b_outer_iter_at_or_beyond_last_valid_pos_for_dominance = false;
        while( outer_iter != list_labels_cur_vertex.end() )
        {
          Splabel cur_outer_splabel = *outer_iter;
          typename std::list<Splabel>::iterator inner_iter = outer_iter;
          if( !b_outer_iter_at_or_beyond_last_valid_pos_for_dominance
              && outer_iter ==
                   get(vec_last_valid_positions_for_dominance,
                       i_cur_resident_vertex) )
            b_outer_iter_at_or_beyond_last_valid_pos_for_dominance = true;
          if( !get(b_vec_vertex_already_checked_for_dominance, i_cur_resident_vertex)
              || b_outer_iter_at_or_beyond_last_valid_pos_for_dominance )
          {
            ++inner_iter;
          }
          else
          {
            inner_iter =
              get(vec_last_valid_positions_for_dominance,
                  i_cur_resident_vertex);
            ++inner_iter;
          }
          bool b_outer_iter_erased = false;
          while( inner_iter != list_labels_cur_vertex.end() )
          {
            Splabel cur_inner_splabel = *inner_iter;
            if( dominance( cur_outer_splabel->
                             cumulated_resource_consumption,
                           cur_inner_splabel->
                             cumulated_resource_consumption ) )
            {
              typename std::list<Splabel>::iterator buf = inner_iter;
              ++inner_iter;
              list_labels_cur_vertex.erase( buf );
              if( cur_inner_splabel->b_is_processed )
              {
                cur_inner_splabel.reset();
              }
              else
                cur_inner_splabel->b_is_dominated = true;
              continue;
            }
            else
              ++inner_iter;
            if( dominance( cur_inner_splabel->
                             cumulated_resource_consumption,
                           cur_outer_splabel->
                             cumulated_resource_consumption ) )
            {
              typename std::list<Splabel>::iterator buf = outer_iter;
              ++outer_iter;
              list_labels_cur_vertex.erase( buf );
              b_outer_iter_erased = true;
              if( cur_outer_splabel->b_is_processed )
              {
                cur_outer_splabel.reset();
              }
              else
                cur_outer_splabel->b_is_dominated = true;
              break;
            }
          }
          if( !b_outer_iter_erased )
            ++outer_iter;
        }
        if( list_labels_cur_vertex.size() > 1 )
          put(vec_last_valid_positions_for_dominance, i_cur_resident_vertex,
            (--(list_labels_cur_vertex.end())));
        else
          put(vec_last_valid_positions_for_dominance, i_cur_resident_vertex,
            list_labels_cur_vertex.begin());
        put(b_vec_vertex_already_checked_for_dominance,
            i_cur_resident_vertex, true);
        put(vec_last_valid_index_for_dominance, i_cur_resident_vertex,
          list_labels_cur_vertex.size() - 1);
      }
    }
    if( !b_all_pareto_optimal_solutions && cur_label->resident_vertex == t )
    {
      // the devil don't sleep
      if( cur_label->b_is_dominated )
      {
        cur_label.reset();
      }
      while( unprocessed_labels.size() )
      {
        Splabel l = unprocessed_labels.top();
        unprocessed_labels.pop();
        // delete only dominated labels, because nondominated labels are
        // deleted at the end of the function
        if( l->b_is_dominated )
        {
          l.reset();
        }
      }
      break;
    }
    if( !cur_label->b_is_dominated )
    {
      cur_label->b_is_processed = true;
      vis.on_label_not_dominated( *cur_label, g );
      typename graph_traits<Graph>::vertex_descriptor cur_vertex =
        cur_label->resident_vertex;
      typename graph_traits<Graph>::out_edge_iterator oei, oei_end;
      for( boost::tie( oei, oei_end ) = out_edges( cur_vertex, g );
           oei != oei_end;
           ++oei )
      {
        b_feasible = true;
        Splabel new_label = boost::allocate_shared<r_c_shortest_paths_label<Graph, Resource_Container> >(
                l_alloc,
                i_label_num++,
                cur_label->cumulated_resource_consumption,
                cur_label,
                *oei,
                target( *oei, g ) );
        b_feasible =
          ref( g,
               new_label->cumulated_resource_consumption,
               new_label->p_pred_label->cumulated_resource_consumption,
               new_label->pred_edge );

        if( !b_feasible )
        {
          vis.on_label_not_feasible( *new_label, g );
          new_label.reset();
        }
        else
        {
          vis.on_label_feasible( *new_label, g );
          vec_vertex_labels[new_label->resident_vertex].
            push_back( new_label );
          unprocessed_labels.push( new_label );
        }
      }
    }
    else
    {
      vis.on_label_dominated( *cur_label, g );
      cur_label.reset();
    }
  }
  std::list<Splabel> dsplabels = get(vec_vertex_labels, t);
  typename std::list<Splabel>::const_iterator csi = dsplabels.begin();
  typename std::list<Splabel>::const_iterator csi_end = dsplabels.end();
  // if d could be reached from o
  if( !dsplabels.empty() )
  {
    for( ; csi != csi_end; ++csi )
    {
      std::vector<typename graph_traits<Graph>::edge_descriptor>
        cur_pareto_optimal_path;
      boost::shared_ptr<r_c_shortest_paths_label<Graph, Resource_Container> > p_cur_label = *csi;
      pareto_optimal_resource_containers.
        push_back( p_cur_label->cumulated_resource_consumption );
      while( p_cur_label->num != 0 )
      {
        cur_pareto_optimal_path.push_back( p_cur_label->pred_edge );
        p_cur_label = p_cur_label->p_pred_label;

        // assertion b_is_valid beyond this point is not correct if the domination function
        // requires resource levels to be strictly greater than existing values
        //
        // Example
        // Customers
        // id   min_arrival   max_departure
        //  2             0             974
        //  3             0             972
        //  4             0             964
        //  5           678             801
        //
        // Path A: 2-3-4-5 (times: 0-16-49-84-678)
        // Path B: 3-2-4-5 (times: 0-18-51-62-678)
        // The partial path 3-2-4 dominates the other partial path 2-3-4,
        // though the path 3-2-4-5 does not strictly dominate the path 2-3-4-5
      }
      pareto_optimal_solutions.push_back( cur_pareto_optimal_path );
      if( !b_all_pareto_optimal_solutions )
        break;
    }
  }

  BGL_FORALL_VERTICES_T(i, g, Graph) {
    std::list<Splabel>& list_labels_cur_vertex = vec_vertex_labels[i];
    typename std::list<Splabel>::iterator si = list_labels_cur_vertex.begin();
    const typename std::list<Splabel>::iterator si_end = list_labels_cur_vertex.end();
    for(; si != si_end; ++si )
    {
      (*si).reset();
    }
  }
} // r_c_shortest_paths_dispatch

} // detail

// default_r_c_shortest_paths_visitor struct
struct default_r_c_shortest_paths_visitor
{
  template<class Label, class Graph>
  void on_label_popped( const Label&, const Graph& ) {}
  template<class Label, class Graph>
  void on_label_feasible( const Label&, const Graph& ) {}
  template<class Label, class Graph>
  void on_label_not_feasible( const Label&, const Graph& ) {}
  template<class Label, class Graph>
  void on_label_dominated( const Label&, const Graph& ) {}
  template<class Label, class Graph>
  void on_label_not_dominated( const Label&, const Graph& ) {}
  template<class Queue, class Graph>
  bool on_enter_loop(const Queue& queue, const Graph& graph) {return true;}
}; // default_r_c_shortest_paths_visitor

// default_r_c_shortest_paths_allocator
typedef
  std::allocator<int> default_r_c_shortest_paths_allocator;
// default_r_c_shortest_paths_allocator

// r_c_shortest_paths functions (handle/interface)
// first overload:
// - return all pareto-optimal solutions
// - specify Label_Allocator and Visitor arguments
template<class Graph,
         class VertexIndexMap,
         class EdgeIndexMap,
         class Resource_Container,
         class Resource_Extension_Function,
         class Dominance_Function,
         class Label_Allocator,
         class Visitor>
void r_c_shortest_paths
( const Graph& g,
  const VertexIndexMap& vertex_index_map,
  const EdgeIndexMap& edge_index_map,
  typename graph_traits<Graph>::vertex_descriptor s,
  typename graph_traits<Graph>::vertex_descriptor t,
  // each inner vector corresponds to a pareto-optimal path
  std::vector<std::vector<typename graph_traits<Graph>::edge_descriptor> >&
    pareto_optimal_solutions,
  std::vector<Resource_Container>& pareto_optimal_resource_containers,
  // to initialize the first label/resource container
  // and to carry the type information
  const Resource_Container& rc,
  const Resource_Extension_Function& ref,
  const Dominance_Function& dominance,
  // to specify the memory management strategy for the labels
  Label_Allocator la,
  Visitor vis )
{
  r_c_shortest_paths_dispatch( g,
                               vertex_index_map,
                               edge_index_map,
                               s,
                               t,
                               pareto_optimal_solutions,
                               pareto_optimal_resource_containers,
                               true,
                               rc,
                               ref,
                               dominance,
                               la,
                               vis );
}

// second overload:
// - return only one pareto-optimal solution
// - specify Label_Allocator and Visitor arguments
template<class Graph,
         class VertexIndexMap,
         class EdgeIndexMap,
         class Resource_Container,
         class Resource_Extension_Function,
         class Dominance_Function,
         class Label_Allocator,
         class Visitor>
void r_c_shortest_paths
( const Graph& g,
  const VertexIndexMap& vertex_index_map,
  const EdgeIndexMap& edge_index_map,
  typename graph_traits<Graph>::vertex_descriptor s,
  typename graph_traits<Graph>::vertex_descriptor t,
  std::vector<typename graph_traits<Graph>::edge_descriptor>&
    pareto_optimal_solution,
  Resource_Container& pareto_optimal_resource_container,
  // to initialize the first label/resource container
  // and to carry the type information
  const Resource_Container& rc,
  const Resource_Extension_Function& ref,
  const Dominance_Function& dominance,
  // to specify the memory management strategy for the labels
  Label_Allocator la,
  Visitor vis )
{
  // each inner vector corresponds to a pareto-optimal path
  std::vector<std::vector<typename graph_traits<Graph>::edge_descriptor> >
    pareto_optimal_solutions;
  std::vector<Resource_Container> pareto_optimal_resource_containers;
  r_c_shortest_paths_dispatch( g,
                               vertex_index_map,
                               edge_index_map,
                               s,
                               t,
                               pareto_optimal_solutions,
                               pareto_optimal_resource_containers,
                               false,
                               rc,
                               ref,
                               dominance,
                               la,
                               vis );
  if (!pareto_optimal_solutions.empty()) {
    pareto_optimal_solution = pareto_optimal_solutions[0];
    pareto_optimal_resource_container = pareto_optimal_resource_containers[0];
  }
}

// third overload:
// - return all pareto-optimal solutions
// - use default Label_Allocator and Visitor
template<class Graph,
         class VertexIndexMap,
         class EdgeIndexMap,
         class Resource_Container,
         class Resource_Extension_Function,
         class Dominance_Function>
void r_c_shortest_paths
( const Graph& g,
  const VertexIndexMap& vertex_index_map,
  const EdgeIndexMap& edge_index_map,
  typename graph_traits<Graph>::vertex_descriptor s,
  typename graph_traits<Graph>::vertex_descriptor t,
  // each inner vector corresponds to a pareto-optimal path
  std::vector<std::vector<typename graph_traits<Graph>::edge_descriptor> >&
    pareto_optimal_solutions,
  std::vector<Resource_Container>& pareto_optimal_resource_containers,
  // to initialize the first label/resource container
  // and to carry the type information
  const Resource_Container& rc,
  const Resource_Extension_Function& ref,
  const Dominance_Function& dominance )
{
  r_c_shortest_paths_dispatch( g,
                               vertex_index_map,
                               edge_index_map,
                               s,
                               t,
                               pareto_optimal_solutions,
                               pareto_optimal_resource_containers,
                               true,
                               rc,
                               ref,
                               dominance,
                               default_r_c_shortest_paths_allocator(),
                               default_r_c_shortest_paths_visitor() );
}

// fourth overload:
// - return only one pareto-optimal solution
// - use default Label_Allocator and Visitor
template<class Graph,
         class VertexIndexMap,
         class EdgeIndexMap,
         class Resource_Container,
         class Resource_Extension_Function,
         class Dominance_Function>
void r_c_shortest_paths
( const Graph& g,
  const VertexIndexMap& vertex_index_map,
  const EdgeIndexMap& edge_index_map,
  typename graph_traits<Graph>::vertex_descriptor s,
  typename graph_traits<Graph>::vertex_descriptor t,
  std::vector<typename graph_traits<Graph>::edge_descriptor>&
    pareto_optimal_solution,
  Resource_Container& pareto_optimal_resource_container,
  // to initialize the first label/resource container
  // and to carry the type information
  const Resource_Container& rc,
  const Resource_Extension_Function& ref,
  const Dominance_Function& dominance )
{
  // each inner vector corresponds to a pareto-optimal path
  std::vector<std::vector<typename graph_traits<Graph>::edge_descriptor> >
    pareto_optimal_solutions;
  std::vector<Resource_Container> pareto_optimal_resource_containers;
  r_c_shortest_paths_dispatch( g,
                               vertex_index_map,
                               edge_index_map,
                               s,
                               t,
                               pareto_optimal_solutions,
                               pareto_optimal_resource_containers,
                               false,
                               rc,
                               ref,
                               dominance,
                               default_r_c_shortest_paths_allocator(),
                               default_r_c_shortest_paths_visitor() );
  if (!pareto_optimal_solutions.empty()) {
    pareto_optimal_solution = pareto_optimal_solutions[0];
    pareto_optimal_resource_container = pareto_optimal_resource_containers[0];
  }
}
// r_c_shortest_paths

// check_r_c_path function
template<class Graph,
         class Resource_Container,
         class Resource_Extension_Function>
void check_r_c_path( const Graph& g,
                     const std::vector
                       <typename graph_traits
                         <Graph>::edge_descriptor>& ed_vec_path,
                     const Resource_Container& initial_resource_levels,
                     // if true, computed accumulated final resource levels must
                     // be equal to desired_final_resource_levels
                     // if false, computed accumulated final resource levels must
                     // be less than or equal to desired_final_resource_levels
                     bool b_result_must_be_equal_to_desired_final_resource_levels,
                     const Resource_Container& desired_final_resource_levels,
                     Resource_Container& actual_final_resource_levels,
                     const Resource_Extension_Function& ref,
                     bool& b_is_a_path_at_all,
                     bool& b_feasible,
                     bool& b_correctly_extended,
                     typename graph_traits<Graph>::edge_descriptor&
                       ed_last_extended_arc )
{
  size_t i_size_ed_vec_path = ed_vec_path.size();
  std::vector<typename graph_traits<Graph>::edge_descriptor> buf_path;
  if( i_size_ed_vec_path == 0 )
    b_feasible = true;
  else
  {
    if( i_size_ed_vec_path == 1
        || target( ed_vec_path[0], g ) == source( ed_vec_path[1], g ) )
      buf_path = ed_vec_path;
    else
      for( size_t i = i_size_ed_vec_path ; i > 0; --i )
        buf_path.push_back( ed_vec_path[i - 1] );
    for( size_t i = 0; i < i_size_ed_vec_path - 1; ++i )
    {
      if( target( buf_path[i], g ) != source( buf_path[i + 1], g ) )
      {
        b_is_a_path_at_all = false;
        b_feasible = false;
        b_correctly_extended = false;
        return;
      }
    }
  }
  b_is_a_path_at_all = true;
  b_feasible = true;
  b_correctly_extended = false;
  Resource_Container current_resource_levels = initial_resource_levels;
  actual_final_resource_levels = current_resource_levels;
  for( size_t i = 0; i < i_size_ed_vec_path; ++i )
  {
    ed_last_extended_arc = buf_path[i];
    b_feasible = ref( g,
                      actual_final_resource_levels,
                      current_resource_levels,
                      buf_path[i] );
    current_resource_levels = actual_final_resource_levels;
    if( !b_feasible )
      return;
  }
  if( b_result_must_be_equal_to_desired_final_resource_levels )
    b_correctly_extended =
     actual_final_resource_levels == desired_final_resource_levels ?
       true : false;
  else
  {
    if( actual_final_resource_levels < desired_final_resource_levels
        || actual_final_resource_levels == desired_final_resource_levels )
      b_correctly_extended = true;
  }
} // check_path

} // namespace

#endif // BOOST_GRAPH_R_C_SHORTEST_PATHS_HPP


////////////////////////////////////////////////////////////////////////////
// lazy_compose_tests.cpp
//
// tests on compose
//
////////////////////////////////////////////////////////////////////////////
/*=============================================================================
    Copyright (c) 2000-2003 Brian McNamara and Yannis Smaragdakis
    Copyright (c) 2001-2007 Joel de Guzman
    Copyright (c) 2015 John Fletcher

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

#include <boost/phoenix/core/limits.hpp>

#include <boost/detail/lightweight_test.hpp>
#include <boost/phoenix/core.hpp>
#include <boost/phoenix/function/lazy_prelude.hpp>

int main()
{
    namespace phx = boost::phoenix;
    using boost::phoenix::arg_names::arg1;
    using boost::phoenix::arg_names::arg2;
    using namespace phx;

    BOOST_TEST(compose(id,id)(arg1)(1)   == 1);
    BOOST_TEST(compose(id,id)(1)()       == 1);
    BOOST_TEST(compose(inc,inc)(arg1)(1) == 3);
    BOOST_TEST(compose(inc,plus)(arg1,arg2)(2,3)        == 6);
    BOOST_TEST(compose(plus,inc,dec)(arg1)(1)           == 2);
    BOOST_TEST(compose(plus,plus,minus)(arg1,arg2)(3,1) == 6);

    return boost::report_errors();
}


//---------------------------------------------------------------------------//
//!
//! \file   tstAtomUnit.cpp
//! \author Alex Robinson
//! \brief  The atom unit unit tests
//!
//---------------------------------------------------------------------------//

// Std Lib Includes
#include <iostream>

// Boost Includes
#define BOOST_TEST_MAIN
#include <boost/test/unit_test.hpp>
#include <boost/test/tools/floating_point_comparison.hpp>
#include <boost/units/quantity.hpp>
#include <boost/units/systems/si/amount.hpp>

// FRENSIE Includes
#include "Utility_AtomUnit.hpp"
#include "Utility_PhysicalConstants.hpp"

using namespace Utility::Units;
namespace si = boost::units::si;

//---------------------------------------------------------------------------//
// Tests
//---------------------------------------------------------------------------//
// Check that the atom unit can be initialized
BOOST_AUTO_TEST_CASE( initialize )
{
  boost::units::quantity<Utility::Units::Atom> amount( 1.0*atom );

  BOOST_CHECK_EQUAL( amount.value(), 1.0 );

  amount = 2.0*atom;

  BOOST_CHECK_EQUAL( amount.value(), 2.0 );
}

//---------------------------------------------------------------------------//
// Check that the atom unit can be initialized from si units
BOOST_AUTO_TEST_CASE( initialize_from_si )
{
  boost::units::quantity<Utility::Units::Atom> amount( 1.0*si::mole );

  BOOST_CHECK_CLOSE_FRACTION( amount.value(),
                              Utility::PhysicalConstants::avogadro_constant,
                              1e-15 );
}

//---------------------------------------------------------------------------//
// Check that the si units can be initialized from the atom unit
BOOST_AUTO_TEST_CASE( initialize_si )
{
  boost::units::quantity<si::amount> amount( 1.0*atom );

  BOOST_CHECK_CLOSE_FRACTION( amount.value(),
                              1.0/Utility::PhysicalConstants::avogadro_constant,
                              1e-15 );
}

//---------------------------------------------------------------------------//
// end tstAtomUnit.cpp
//---------------------------------------------------------------------------//


#include "crit_crauser.hpp"
#include "crit_dijkstra.hpp"
#include "crit_heuristic.hpp"
#include "crit_oracle.hpp"
#include "crit_traff_bridge.hpp"
#include "crit_paper_in.hpp"
#include "crit_paper_out.hpp"
#include "graph.hpp"
#include "test_graph.hpp"
#include <boost/mpl/list.hpp>
#include <boost/test/data/test_case.hpp>
#include <boost/test/test_case_template.hpp>
#include <boost/test/unit_test.hpp>
#include <cfloat>

using namespace sssp;

namespace {

struct crauser_in_static : crauser_in {
    crauser_in_static(const sssp::graph* g, size_t s) : crauser_in(g, s, false) {}
};

struct crauser_in_dynamic : crauser_in {
    crauser_in_dynamic(const sssp::graph* g, size_t s) : crauser_in(g, s, true) {}
};

struct crauser_out_static : crauser_out {
    crauser_out_static(const sssp::graph* g, size_t s) : crauser_out(g, s, false) {}
};

struct crauser_out_dynamic : crauser_out {
    crauser_out_dynamic(const sssp::graph* g, size_t s) : crauser_out(g, s, true) {}
};

} // namespace

using non_euclidean_tests = boost::mpl::list<paper_out,
                                             paper_in,
                                             traff_bridge,
                                             crauser_out_static,
                                             crauser_out_dynamic,
                                             crauser_in_static,
                                             crauser_in_dynamic,
                                             oracle,
                                             smallest_tentative_distance>;

#ifdef NDEBUG
static constexpr int test_count = 1000;
#else
// Debug builds are very slow and the tests need ages to run through.
static constexpr int test_count = 100;
#endif

BOOST_AUTO_TEST_CASE_TEMPLATE(criteria_non_euclidean_test, Criteria, non_euclidean_tests) {
    for (int seed = 0; seed < test_count; ++seed) {
        BOOST_TEST_CONTEXT("seed = " << seed) {
            const graph g = make_test_graph(seed);

            boost::base_collection<criteria> cs;
            cs.insert(Criteria(&g, 0));
            if (!cs.cbegin()->is_complete()) {
                cs.insert(smallest_tentative_distance(&g, 0));
            }
            node_map<dijkstra_result> result = dijkstra(g, 0, cs);

            cs.clear();
            cs.insert(smallest_tentative_distance(&g, 0));
            node_map<dijkstra_result> reference = dijkstra(g, 0, cs);

            for (size_t node = 0; node < g.node_count(); ++node) {
                BOOST_TEST_CONTEXT("node = " << node) {
                    BOOST_REQUIRE(reference[node].predecessor == result[node].predecessor);
                    BOOST_REQUIRE((reference[node].distance == result[node].distance ||
                                   std::abs(reference[node].distance - result[node].distance) <= DBL_EPSILON));
                }
            }
        }
    }
}

BOOST_AUTO_TEST_CASE_TEMPLATE(traffs_counter_example, Criteria, non_euclidean_tests) {
    //          5 --8--> 4 --3
    //                       |  /-- 1 --> 6
    //                       v /
    // 1 <--100-- 0 --110--> 2 <--\
    //            \--105--> 3 --2-/
    graph g;
    for (size_t i = 0; i <= 6; ++i) {
        g.add_node();
    }
    g.add_edge(0, 1, 100);
    g.add_edge(0, 2, 110);
    g.add_edge(0, 3, 105);
    g.add_edge(2, 6, 1);
    g.add_edge(3, 2, 2);
    g.add_edge(4, 2, 3);
    g.add_edge(5, 4, 8);

    boost::base_collection<criteria> cs;
    cs.insert(Criteria(&g, 0));
    if (!cs.cbegin()->is_complete()) {
        cs.insert(smallest_tentative_distance(&g, 0));
    }
    node_map<dijkstra_result> result = dijkstra(g, 0, cs);

    cs.clear();
    cs.insert(smallest_tentative_distance(&g, 0));
    node_map<dijkstra_result> reference = dijkstra(g, 0, cs);

    for (size_t node = 0; node < g.node_count(); ++node) {
        BOOST_TEST_CONTEXT("node = " << node) {
            BOOST_REQUIRE(reference[node].predecessor == result[node].predecessor);
            BOOST_REQUIRE((reference[node].distance == result[node].distance ||
                           std::abs(reference[node].distance - result[node].distance) <= DBL_EPSILON));
        }
    }
}

BOOST_AUTO_TEST_CASE(critiera_heuristic_tests) {
    for (int seed = 0; seed < test_count; ++seed) {
        BOOST_TEST_CONTEXT("seed = " << seed) {
            const auto g = make_test_graph_euclidean(seed);

            boost::base_collection<criteria> cs;
            auto h = [&](size_t node) { return distance(std::get<1>(g)[node], std::get<1>(g)[0]); };
            cs.insert(heuristic(&std::get<0>(g), 0, h));
            // This criteria is incomplete, therefore I add another one to make it complete.
            cs.insert(smallest_tentative_distance(&std::get<0>(g), 0));
            node_map<dijkstra_result> result = dijkstra(std::get<0>(g), 0, cs);

            cs.clear();
            cs.insert(smallest_tentative_distance(&std::get<0>(g), 0));
            node_map<dijkstra_result> reference = dijkstra(std::get<0>(g), 0, cs);

            for (size_t node = 0; node < std::get<0>(g).node_count(); ++node) {
                BOOST_TEST_CONTEXT("node = " << node) {
                    BOOST_REQUIRE(reference[node].predecessor == result[node].predecessor);
                    BOOST_REQUIRE((reference[node].distance == result[node].distance ||
                                   std::abs(reference[node].distance - result[node].distance) <= DBL_EPSILON));
                }
            }
        }
    }
}


#pragma once

#include <polyvec/api.hpp>

#include <Eigen/Core>

NAMESPACE_BEGIN(polyfit)
NAMESPACE_BEGIN(mc)

// Get the bounding box of a bunch of paths
Eigen::AlignedBox<int, 2> get_bounding_box(const std::vector<Eigen::Matrix2Xi>& regions);

NAMESPACE_END(mc)
NAMESPACE_END(polyfit)


// やめるのだフェネックで学ぶC++の実証コード(インラインアセンブリ)
#include <climits>
#include <cstring>
#include <algorithm>
#include <functional>
#include <iomanip>
#include <iostream>
#include <limits>
#include <string>
#include <unordered_map>
#include <vector>
#include <boost/math/constants/constants.hpp>
#include <boost/multiprecision/cpp_dec_float.hpp>
#include <gtest/gtest.h>
#include "cFriendsCommon.h"
#include "cppFriends.hpp"
#include "cppFriendsClang.hpp"

class TestSaturationArithmetic : public ::testing::Test{};

TEST_F(TestSaturationArithmetic, Add) {
    constexpr size_t XmmRegisterSizeInByte = 16;
    constexpr size_t NumberOfRegister = 2;
    std::aligned_storage<XmmRegisterSizeInByte, XmmRegisterSizeInByte>::type xmmRegisters[NumberOfRegister];
    const uint8_t presetValue[XmmRegisterSizeInByte * NumberOfRegister] = {
        0,   0, 1, 1,   1,   1, 2, 2,   2,   2,   2, 254, 254, 254, 255, 255,   // 足される数
        0, 255, 0, 1, 254, 255, 0, 1, 253, 254, 255,   0,   1,   2,   0,   1    // 足す数
    };

    static_assert(sizeof(xmmRegisters) >= sizeof(presetValue), "Too large");
    ::memmove(xmmRegisters, presetValue, sizeof(presetValue));

    const uint8_t expected[XmmRegisterSizeInByte] = {
        0, 255, 1, 2, 255, 255, 2, 3, 255, 255, 255, 254, 255, 255, 255, 255};  // 和

    // Windows/Linuxの両方で使えるscratch registers
    asm volatile (
        "movdqa  (%0),   %%xmm4 \n\t"
        "movdqa  16(%0), %%xmm5 \n\t"
        "paddusb %%xmm5, %%xmm4 \n\t"
        "movdqa  %%xmm4, (%0)    \n\t"
        ::"r"(xmmRegisters):"xmm4", "xmm5", "memory");

    static_assert(sizeof(expected) <= sizeof(xmmRegisters), "Too large");
    EXPECT_EQ(0, ::memcmp(expected, xmmRegisters, sizeof(expected)));
}

TEST_F(TestSaturationArithmetic, Sub) {
    constexpr size_t XmmRegisterSizeInByte = 16;
    constexpr size_t NumberOfRegister = 2;
    std::aligned_storage<XmmRegisterSizeInByte, XmmRegisterSizeInByte>::type xmmRegisters[NumberOfRegister];
    const uint8_t presetValue[XmmRegisterSizeInByte * NumberOfRegister] = {
        0,   0, 0,   0,   0, 1, 1, 1, 254, 254, 254, 254, 255, 255, 255, 255,  // 引かれる数
        0,   1, 2, 254, 255, 0, 1, 2,   1, 253, 254, 255,   0,   1, 254, 255   // 引く数
    };
    static_assert(sizeof(xmmRegisters) >= sizeof(presetValue), "Too large");
    ::memmove(xmmRegisters, presetValue, sizeof(presetValue));

    const uint8_t expected[XmmRegisterSizeInByte] = {
        0,   0, 0,   0,   0, 1, 0, 0, 253,   1,   0,   0, 255, 254,   1,   0}; // 差

    // Windows/Linuxの両方で使えるscratch registers
    asm volatile (
        "movdqa  (%0),   %%xmm4 \n\t"
        "movdqa  16(%0), %%xmm5 \n\t"
        "psubusb %%xmm5, %%xmm4 \n\t"
        "movdqa  %%xmm4, (%0)    \n\t"
        ::"r"(xmmRegisters):"xmm4", "xmm5", "memory");

    static_assert(sizeof(expected) <= sizeof(xmmRegisters), "Too large");
    EXPECT_EQ(0, ::memcmp(expected, xmmRegisters, sizeof(expected)));
}

TEST_F(TestSaturationArithmetic, ShiftLeft) {
    using Reg32 = uint32_t;
    Reg32 expected = 1;

    for(Reg32 width = 0; width < 35; ++width) {
        Reg32 src = 1;
        Reg32 temp = 0;
        Reg32 actual = 3;

        asm volatile (
            "xor   %%ebx, %%ebx \n\t"
            "cmp   $0x1f, %%ecx \n\t"
            "shlx  %%ecx, %%edx, %%eax \n\t"
            "cmova %%ebx, %%eax  \n\t"
            :"=a"(actual),"=b"(temp):"d"(src),"c"(width):);

        EXPECT_EQ(expected, actual);
        expected <<= 1;
    }
}

TEST_F(TestSaturationArithmetic, ShiftRight) {
    using Reg32 = uint32_t;
    Reg32 expected = 0x80000000;
    for(Reg32 width = 0; width < 35; ++width) {
        Reg32 src = 0x80000000;
        Reg32 temp = 0;
        Reg32 actual = 1;

        asm volatile (
            "mov   %%edx, %%ebx \n\t"
            "sar   $0x1f, %%ebx \n\t"
            "cmp   $0x1f, %%ecx \n\t"
            "sarx  %%ecx, %%edx, %%eax \n\t"
            "cmova %%ebx, %%eax  \n\t"
            :"=a"(actual),"=b"(temp):"d"(src),"c"(width):);

        EXPECT_EQ(expected, actual);
        expected = expected | (expected >> 1);
    }
}

namespace {
    using ProcessorClock = uint64_t;
    using ProcessorClockSet = std::vector<ProcessorClock>;
    using ProcessorClockMap = std::unordered_map<ProcessorClock, size_t>;
    std::string g_expString;

    ProcessorClock getProcessorClock() {
        ProcessorClock clock = 0;
        auto pClock = &clock;

        // クロックだけ測る
        asm volatile (
            "rdtsc \n\t"
            "mov %%eax, (%0)  \n\t"
            "mov %%edx, 4(%0) \n\t"
            ::"r"(pClock):"eax", "edx", "memory");
        return clock;
    }

    ProcessorClock getProcessorClockWithLoad() {
        // 負荷を掛ける
        using LongFloat = boost::multiprecision::number<boost::multiprecision::cpp_dec_float<50>>;
        g_expString += boost::math::constants::e<LongFloat>().str().at(0);
        return getProcessorClock();
    }

    using FuncGetClock = std::function<ProcessorClock()>;
    void checkProcessorClock(FuncGetClock& f) {
        ProcessorClockSet clockSet;
        for(size_t i = 0; i < 120000; ++i) {
            clockSet.push_back(f());
        }

        // 周回しないものと仮定している
        ProcessorClockMap clockMap;
        constexpr ProcessorClock mod = 12;

        for(ProcessorClock i=0; i<mod; ++i) {
            clockMap[i] = 0;
        }

        decltype(clockSet)::value_type prevTimestamp = clockSet.at(0);
        for(auto timestamp : clockSet) {
            ASSERT_LE(prevTimestamp, timestamp);
            clockMap[timestamp % mod] += 1;
            prevTimestamp = timestamp;
        }

        // 出現確率が二項分布なら、標準偏差は sqrt(n*p*(1-p))になる
        // p=1/12なら、1σ = sqrt(n) * 0.28, 6σ = sqrt(n) * 1.66
        // n=120000なら、平均=10000, 6σ=575なので、9000回は出るはず
        // 9000回に満たないのはおかしい
        bool foundBias = false;
        for(ProcessorClock i=0; i<mod; ++i) {
            auto count = clockMap[i];
            std::cout << count << ":";
            foundBias |= (count < 9000);
        }

        EXPECT_TRUE(foundBias);
        std::cout << "\n";
        return;
    }

    using NallowClock = uint32_t;
    // コンテキストスイッチがなければ、これ以上の時刻差は観測されないはず
    constexpr NallowClock ClockThreshold = 4000;
    // Signedとみなして巨大な数なら時刻が逆転している
    constexpr NallowClock MaxDiff = 2 + std::numeric_limits<NallowClock>::max() / 2;

    // atomicにリアルタイムクロックを読まないと不適切な値が得られるという例
    inline NallowClock getProcessorClockBad(ProcessorClock& clock) {
        uint32_t mixed = 0;
        auto pMixed = &mixed;
        auto pClock = &clock;

        asm volatile (
            "rdtsc \n\t"  // 下位12bitを読む
            "andl $0xfff, %%eax \n\t"
            "movl %%eax,  %%ebx \n\t"
            "rdtsc \n\t"  // 上位bitを読む
            "movl %%eax, (%1)  \n\t"
            "movl %%edx, 4(%1) \n\t"
            "andl $0xfffff000, %%eax \n\t"
            "orl  %%ebx, %%eax \n\t"
            "movl %%eax, (%0)  \n\t"
            ::"r"(pMixed),"r"(pClock):"eax","ebx","edx","memory");

        return static_cast<NallowClock>(mixed);
    }

    NallowClock getProcessorClockGood(ProcessorClock& clock) {
        uint32_t mixed = 0;
        auto pMixed = &mixed;
        auto pClock = &clock;

        asm volatile (
            "1: \n\t"
            "rdtsc \n\t"  // 上位bitを読む
            "andl $0xfffff000, %%eax \n\t"
            "movl %%eax,  %%ecx \n\t"
            "rdtsc \n\t"  // 下位12bitを読む
            "andl $0xfff, %%eax \n\t"
            "movl %%eax,  %%edi \n\t"
            "rdtsc \n\t"  // もう一度上位bitを読む
            "movl %%eax, (%1)  \n\t"
            "movl %%edx, 4(%1) \n\t"
            "andl $0xfffff000, %%eax \n\t"
            "cmpl %%ecx,  %%eax \n\t"
            "jne  1b \n\t"  // 読み直す
            "orl  %%edi, %%eax \n\t"
            "movl %%eax, (%0)  \n\t"
            ::"r"(pMixed),"r"(pClock):"eax","ecx","edx","edi","memory");

        return static_cast<NallowClock>(mixed);
    }
}

class TestProcessorClock : public ::testing::Test{
    virtual void SetUp() override {
        g_expString.clear();
    }

    virtual void TearDown() override {
        g_expString.clear();
    }
};

TEST_F(TestProcessorClock, Light) {
    FuncGetClock f(getProcessorClock);
    checkProcessorClock(f);
}

TEST_F(TestProcessorClock, Heavy) {
    FuncGetClock f(getProcessorClockWithLoad);
//  失敗することがある
//  checkProcessorClock(f);
}

TEST_F(TestProcessorClock, Bad) {
    ProcessorClock previousClock = 0;
    auto previous = getProcessorClockBad(previousClock);
    bool found = false;

    // 1秒くらい、十分たくさん行うが、失敗する可能性がないとは言えない
    for(int i=0; i<0x4000000 && !found; ++i) {
        ProcessorClock currentClock = 0;
        auto current = getProcessorClockBad(currentClock);
        // Wrap around
        auto diff = current - previous;
        ASSERT_TRUE(diff);

        // クロックが大きく飛んだ時は、コンテキストスイッチが入ったとみなす
        if (ClockThreshold > (currentClock - previousClock)) {
            // 閾値より大きい差は、getProcessorClockで弾いているはず
            found |= (MaxDiff <= diff);
        }
        previous = current;
        previousClock = currentClock;
    }

    ASSERT_TRUE(found);
}

TEST_F(TestProcessorClock, Good) {
    ProcessorClock previousClock = 0;
    auto previous = getProcessorClockGood(previousClock);

    for(int i=0; i<0x2000000; ++i) {
        ProcessorClock currentClock = 0;
        auto current = getProcessorClockGood(currentClock);
        auto diff = current - previous;
        ASSERT_TRUE(diff);

        // クロックが大きく飛んだ時は、コンテキストスイッチが入ったとみなす
        if (ClockThreshold > (currentClock - previousClock)) {
            ASSERT_GT(MaxDiff, diff);
        }
        previous = current;
        previousClock = currentClock;
    }
}

namespace {
    using Log2Arg = unsigned int;
    using Log2Result = int;
    constexpr Log2Result Log2ResultInvalid = -1;

    Log2Result log2Builtin(Log2Arg arg) {
        return (arg) ? (31 - __builtin_clz(arg)) : Log2ResultInvalid;
    }

    Log2Result log2Asm(Log2Arg arg) {
        Log2Result result = 0;
        Log2Result invalid = Log2ResultInvalid;

        asm volatile (
            "bsr    %1, %0 \n\t"
            "cmovz  %2, %0 \n\t"
            :"=r"(result):"r"(arg),"r"(invalid):);

        return result;
    }

    struct Log2TestCase {
        Log2Arg arg;
        Log2Result expected;
    };

    const Log2TestCase g_log2TestCaseSet [] = {
        {0, Log2ResultInvalid},
        {1,0}, {2,1}, {3,1}, {4,2}, {5,2}, {6,2}, {7,2},
        {8,3}, {9,3}, {14,3}, {15,3}, {16,4}, {17,4}, {31,4},
        {0x7fffffffu, 30}, {0x80000000u, 31}, {0xffffffffu, 31}
    };
}

class TestLog2 : public ::testing::Test{};

TEST_F(TestLog2, Builtin) {
    for(auto& testcase : g_log2TestCaseSet) {
        EXPECT_EQ(testcase.expected, log2Builtin(testcase.arg));
    }
}

TEST_F(TestLog2, Asm) {
    for(auto& testcase : g_log2TestCaseSet) {
        EXPECT_EQ(testcase.expected, log2Asm(testcase.arg));
    }
}

TEST_F(TestLog2, LongLong) {
    int count = 0;
    unsigned long long big = 5000000000000000ull;  // 5000兆

    // unsignedなのでいつかは全ビット0になって終わる
    while(big) {
        ++count;
        big /= 2;
    }

    EXPECT_EQ(53, count);
}

class TestDivideBy2 : public ::testing::Test{};

/*
負の数に対する除算に対して、コンパイラがどんなコードを出力するか確認する
Cで書くとこうなる
  int divide_by_2(int src) {
      return src / 2;
  }

以下はGCCの出力である
  mov  eax, ecx
  shr  eax, 31
  add  eax, ecx
  sar  eax

以下はclang-LLVMの出力である
  mov  eax, ecx
  shr  eax, 31
  lea  eax, [rax + rcx]
  sar  eax

http://www.microhowto.info/howto/round_towards_minus_infinity_when_dividing_integers_in_c_or_c++.html
によると、割る数または割られる数の少なくとも一方が負の場合の丸めは実装依存である。
*/

TEST_F(TestDivideBy2, RoundToZero) {
    // 負の数を2で割るときに、単にsar 1回ではうまくいかない
    // -1を2で割っても-1になってしまう
    // 負の数で小数点以下切り捨て(round to 0)なら1足してからシフトする必要がある
    // -8 : 1000b <+1> 1001b <sar> 1100b (-4)
    // -7 : 1001b <+1> 1010b <sar> 1101b (-3)
    // -2 : 1110b <+1> 1111b <sar> 1111b (-1)
    // -1 : 1111b <+1> 0 <sar> 0
    // 正の数には1足さないので、単にシフトしてLSBを捨てるだけである

    struct TestCase {
        int32_t arg;
        int32_t expected;
        int32_t expectedShifted;
    };

    const TestCase testCaseSet[] = {
        {0, 0, 0},
        {1, 0, 0},
        {2, 1, 1},
        {3, 1, 1},
        {4, 2, 2},
        {7, 3, 3},
        {8, 4, 4},
        {9, 4, 4},
        { 2147483645,  1073741822,  1073741822},
        { 2147483646,  1073741823,  1073741823},
        { 2147483647,  1073741823,  1073741823},
        {-2147483648, -1073741824, -1073741824},
        {-2147483647, -1073741823, -1073741824},
        {-2147483646, -1073741823, -1073741823},
        {-9, -4, -5},
        {-8, -4, -4},
        {-7, -3, -4},
        {-4, -2, -2},
        {-3, -1, -2},
        {-2, -1, -1},
        {-1,  0, -1}
    };

    static_assert(std::numeric_limits<int32_t>::min() == -2147483648, "");
    static_assert(std::numeric_limits<int32_t>::max() == 2147483647, "");

    for(const auto& testCase : testCaseSet) {
        auto arg = testCase.arg;
        decltype(arg) actual = 0;

        // 最近のCPUなら、3オペレータシフト命令を使える
        asm volatile (
            "movl  %%ecx, %%eax \n\t"  // 第一引数 = ecx
            "shrl  $31,   %%eax \n\t"  // 第一引数が非負なら0, 負なら1
            "addl  %%ecx, %%eax \n\t"  // 第一引数が非負ならそのまま, 負なら1足したものを
            "sarl  %%eax        \n\t"  // 符号を保ったままシフトする
            :"=a"(actual):"c"(arg):);

        EXPECT_EQ(testCase.expected, actual);
        EXPECT_EQ(testCase.expected, testCase.arg / 2);

        // 負の数では、/2 を負の無限大方向に切り上げるのと同じになる
        // それはそれで正しいのかもしれないが、-1を2で割って-1になるのは直観に反する
        auto shifted = testCase.arg;
        shifted >>= 1;
        EXPECT_EQ(testCase.expectedShifted, shifted);
    }
}

namespace {
    int32_t divideByZeroUnsafe(int32_t dividend, int32_t divisor, int32_t special) {
        int32_t result = dividend;
        asm volatile (
            "cdq    \n\t"
            "idiv   %%ebx \n\t"
            "or     %%ebx, %%ebx \n\t"
            "cmovz  %%esi, %%eax \n\t"
            :"+a"(result):"b"(divisor),"S"(special):"edx");

        return result;
    }

    int32_t divideByZeroMuchSafe(int32_t dividend, int32_t divisor, int32_t special) {
        // clangの出力を元に作る
        int32_t result = dividend;
        asm volatile (
            "or    %%ebx, %%ebx \n\t"
            "cmovz %%esi, %%eax \n\t"
            "jz    1f \n\t"
            "cdq   \n\t"
            "idiv  %%ebx \n\t"
            "1:"
            :"+a"(result):"b"(divisor),"S"(special):"edx");

        return result;
    }

    int32_t divideByZero(int32_t dividend, int32_t divisor, int32_t special) {
        return (divisor) ? (dividend / divisor) : special;
    }
}

class TestDivideByZero : public ::testing::Test{};

TEST_F(TestDivideByZero, Unsafe) {
    EXPECT_EQ(3,  divideByZeroUnsafe(15, 4, -1));
}

TEST_F(TestDivideByZero, MuchSafe) {
    EXPECT_EQ(3,  divideByZeroMuchSafe(15, 4, -1));
    EXPECT_EQ(-1, divideByZeroMuchSafe(15, 0, -1));
}

TEST_F(TestDivideByZero, Cpp) {
    EXPECT_EQ(3,  divideByZero(15, 4, -1));
    EXPECT_EQ(-1, divideByZero(15, 0, -1));
#if 0
    // int32_tの範囲で解を表現できないのでCPU例外が発生するはずだが
    // 定数式としてコンパイラが値を求めた場合は実行時例外にならない
    constexpr int32_t intMin = std::numeric_limits<int32_t>::min();
    EXPECT_EQ(intMin, divideByZero(intMin, -1, 0));
#endif
}

class TestBitManipulation : public ::testing::Test{
protected:
    using IntType = uint32_t;
    static constexpr IntType bitCount = static_cast<IntType>(sizeof(IntType) * 8);
    static const IntType TestSet[2];
};

const TestBitManipulation::IntType TestBitManipulation::TestSet[2] = {0xffffffffu, 0xaaaaaaaau};

#ifdef CPPFRIENDS_AVX2
// iビット目以上を0にする
TEST_F(TestBitManipulation, BitMask) {
    for(auto src : TestSet) {
        IntType expected = 0;

        for(IntType i = 0; i <= bitCount*3; ++i) {
            IntType actualCpp = src;
            IntType mask = 1;
            if (i < bitCount) {
                mask <<= i;  // 多すぎるシフトに対する動作は未定義
            } else {
                mask = 0;
            }
            --mask;
            actualCpp = src & mask;
            EXPECT_EQ(expected, actualCpp);

            // ビット位置はmod 32/64ではなくsaturated
            IntType actualAsm = src;
            asm volatile (
                "bzhi  %2, %1, %0 \n\t"
                :"=r"(actualAsm):"r"(src),"r"(i):);
            EXPECT_EQ(expected, actualAsm);

            if (src & 1) {
                expected <<= 1;
                ++expected;
            } else {
                if (i & 1) {
                    expected <<= 2;
                    expected |= 2;
                }
            }
        }
    }
}

// 整数を2^n * 奇数で表現できるときの2^nを求める
TEST_F(TestBitManipulation, PowerOf2) {
    for(auto original : TestSet) {
        IntType src = original;
        IntType expected = (original & 1) ? 1 : 2;

        for(IntType index = 0; index <= bitCount; ++index) {
            IntType actualCpp = src & -src;
            EXPECT_EQ(expected, actualCpp);

            IntType actualAsm = src;
            // 0には0を返す
            asm volatile (
                "blsi  %1, %0 \n\t"
                :"=r"(actualAsm):"r"(src):);
            EXPECT_EQ(expected, actualAsm);

            if (original & 1) {
                expected <<= 1;
            } else {
                if (index & 1) {
                    expected <<= 2;
                }
            }

            IntType mask = 1;
            mask <<= index;
            src &= ~mask;
        }
    }
}
#endif

class TestProcessorExceptionDeathTest : public ::testing::Test{};

TEST_F(TestProcessorExceptionDeathTest, Unsafe) {
    // 実行すると0除算エラーになる
    ASSERT_DEATH(divideByZeroUnsafe(15, 0, -1), "");
}

TEST_F(TestProcessorExceptionDeathTest, Abs) {
    EXPECT_EQ(0,  ProcessorException::abs_int(0));
    EXPECT_EQ(1,  ProcessorException::abs_int(1));
    EXPECT_EQ(1,  ProcessorException::abs_int(-1));

    constexpr int intMax = std::numeric_limits<int>::max();
    constexpr int intMin = std::numeric_limits<int>::min();
    EXPECT_EQ(intMax, ProcessorException::abs_int(intMax));
    EXPECT_EQ(intMin, ProcessorException::abs_int(intMin));
}

#if !defined(__MINGW32__) && !defined(__MINGW64__)
// MinGWでは、FPEが発生するが処理が先に進まない
TEST_F(TestProcessorExceptionDeathTest, MuchSafe) {
    // int32_tの範囲で解を表現できないのでCPU例外が発生する
    constexpr int32_t intMin = std::numeric_limits<int32_t>::min();
    ASSERT_DEATH(divideByZeroMuchSafe(intMin, -1, 0), "");
}

TEST_F(TestProcessorExceptionDeathTest, TooLarge) {
    EXPECT_EQ(3,  ProcessorException::may_divide_by_zero(15, 4, -1));
    EXPECT_EQ(-1, ProcessorException::may_divide_by_zero(15, 0, -1));
    // int32_tの範囲で解を表現できないのでCPU例外が発生する
    constexpr int32_t intMin = std::numeric_limits<int32_t>::min();
    ASSERT_DEATH(ProcessorException::may_divide_by_zero(intMin, -1, 0), "");
}

TEST_F(TestProcessorExceptionDeathTest, IntMaxMin) {
    if ((sizeof(int32_t) == sizeof(int)) && (std::numeric_limits<int32_t>::min() == INT_MIN)) {
        EXPECT_EQ(INT_MIN, ProcessorException::abs_int(INT_MIN));
    }
    ASSERT_DEATH(ProcessorException::may_divide_by_zero(INT_MIN, -1, 0), "");
}
#endif

// 元記事は下記によるもの。64bit Cygwin向けに変更した。
// http://blog.onlinedisassembler.com/blog/?p=23
// 15 bytesもある命令。実行はできない。
void LongestInstruction(void) {
#ifdef __x86_64__
    // objdumpの出力
    // <_Z18LongestInstructionv>:
    // 64 67 f0 43 81 84 88    lock addl $0xa0a0a0a0,%fs:0x50505050(%r8d,%r9d,4)
    // 50 50 50 50 a0 a0 a0
    // a0
    asm volatile (
        "lock addl $0xa0a0a0a0, %%fs:0x50505050(%%r8d, %%r9d, 4)\n\t"
        :::);
#endif
}

namespace {
    uint8_t calculate2ndOrGreaterDigits(uint8_t num) {
        uint8_t result = 0;
        // min(255, (num + 1)) * 51 / 512
        asm volatile (
            // min(255, (num + 1))
            "add    $1, %%al \n\t"
            "sbb    $0, %%al \n\t"
            // += num
            "xor    %%bl, %%bl \n\t"
            "mov    %%al, %%cl \n\t"
            "xor    %%dl, %%dl \n\t"
            // += num * 2
            "add    %%al, %%al \n\t"
            "adc    %%bl, %%bl \n\t"
            "add    %%al, %%cl \n\t"
            "adc    %%bl, %%dl \n\t"
            // num * 48 = num * 3 << 4
            "mov    %%cl, %%al \n\t"
            "mov    %%dl, %%bl \n\t"
            "add    %%al, %%al \n\t"
            "adc    %%bl, %%bl \n\t"
            "add    %%al, %%al \n\t"
            "adc    %%bl, %%bl \n\t"
            "add    %%al, %%al \n\t"
            "adc    %%bl, %%bl \n\t"
            "add    %%al, %%al \n\t"
            "adc    %%bl, %%bl \n\t"
            // += num * 48
            "add    %%al, %%cl \n\t"
            "adc    %%bl, %%dl \n\t"
            // /= 512
            "shr    %%dl \n\t"
            :"=d"(result),"+a"(num)::"rbx", "rcx");
       return result;
    }

    uint8_t calculate1stDigit(uint8_t num, uint8_t secondOrGreaterDigits) {
        uint8_t result = 0;
        asm volatile (
            // num - (secondOrGreaterDigits * 10)
            "mov    %%al, %%dl \n\t"
            "mov    %%bl, %%cl \n\t"
            "shl    %%bl \n\t"
            "shl    %%bl \n\t"
            "add    %%cl, %%bl \n\t"
            "shl    %%bl \n\t"
            "sub    %%bl, %%dl \n\t"
            :"=d"(result),"+a"(num),"+b"(secondOrGreaterDigits)::"rcx");
       return result;
    }
}

class TestSplitDigits : public ::testing::Test{};

TEST_F(TestSplitDigits, All) {
    // Calclulate digits for uint8_t
    uint8_t i=0;
    for(;;) {
        const auto digits23 = calculate2ndOrGreaterDigits(i);
        const auto digits1 = calculate1stDigit(i, digits23);
        const auto digits3 = calculate2ndOrGreaterDigits(digits23);
        const auto digits2 = calculate1stDigit(digits23, digits3);
        const uint32_t actualDigits1 = digits1;
        const uint32_t actualDigits2 = digits2;
        const uint32_t actualDigits3 = digits3;

        uint32_t expected = i;
        const auto expectedDigits1 = expected % 10;
        expected /= 10;
        const auto expectedDigits2 = expected % 10;
        const auto expectedDigits3 = expected / 10;

        // uint32_tにすると、値が文字ではなく数値として表示される
        EXPECT_EQ(expectedDigits1, actualDigits1);
        EXPECT_EQ(expectedDigits2, actualDigits2);
        EXPECT_EQ(expectedDigits3, actualDigits3);
        if (!++i) {
            break;
        }
    }
}

class TestAsmInstructions : public ::testing::Test{};

TEST_F(TestAsmInstructions, Coffee) {
    uint32_t arg = 0x8000;
    uint32_t result = 0;
    constexpr uint32_t expected = 0xff00;

    // sar bh, 0xee = c0h, ffh, eeh
    asm volatile (
//      ".byte 0xc0, 0xff, 0xee \n\t"
        "sarb  $0xee, %%bh \n\t"
        "mov   %%ebx, %%eax \n\t"
        :"=a"(result),"+b"(arg)::);
    EXPECT_EQ(expected, arg);
    EXPECT_EQ(expected, result);
}

TEST_F(TestAsmInstructions, Div3_16bit) {
    uint16_t arg = 0;
    do {
        uint16_t expected = (arg % 3) ? 0 : 1;
        uint16_t result = 0;
        asm volatile (
            "mov   $0x5555, %%ax \n\t"
            "mul   %%bx \n\t"
            "mov   %%dx, %%ax \n\t"
            "add   %%ax, %%dx \n\t"
            "add   %%ax, %%dx \n\t"
            "mov   %%bx, %%ax \n\t"
            "sub   %%dx, %%ax \n\t"
            "mov   %%ax, %%dx \n\t"
            "shr   $1, %%ax \n\t"
            "xor   %%dx, %%ax \n\t"
            "not   %%ax \n\t"
            "and   $1, %%ax \n\t"
            :"=a"(result):"b"(arg):"rdx");
        if (expected != result) {
            std::cerr << arg;
        }
        ASSERT_EQ(expected, result);
    } while(++arg);
}

TEST_F(TestAsmInstructions, Div3_32bit) {
    uint32_t arg = 0;
    do {
        uint32_t expected = (arg % 3) ? 0 : 1;
        uint32_t result = 0;
        asm volatile (
            "mov   $0x55555555, %%eax \n\t"
            "mul   %%ebx \n\t"
            "mov   %%edx, %%eax \n\t"
            "add   %%eax, %%edx \n\t"
            "add   %%eax, %%edx \n\t"
            "mov   %%ebx, %%eax \n\t"
            "sub   %%edx, %%eax \n\t"
            "mov   %%eax, %%edx \n\t"
            "shr   $1, %%eax \n\t"
            "xor   %%edx, %%eax \n\t"
            "not   %%eax \n\t"
            "and   $1, %%eax \n\t"
            :"=a"(result):"b"(arg):"rdx");
        if (expected != result) {
            std::cerr << arg;
        }
        ASSERT_EQ(expected, result);
    } while(++arg);
}

namespace {
    using FilledBitSet = uint32_t;

    FilledBitSet SplitByMsb(FilledBitSet arg, FilledBitSet& count) {
        uint32_t result = 0;
        asm volatile (
            "lzcnt %%eax, %%ecx \n\t"
            "mov   $-1, %%ebx \n\t"
            "shrx  %%ecx, %%ebx, %%edx \n\t"
            "cmovb %%eax, %%edx \n\t"
            :"=d"(result),"=c"(count):"a"(arg):"rbx");
        return result;
    }

    FilledBitSet SetOnesForLeadingZeros(FilledBitSet arg, FilledBitSet& count) {
        uint32_t result = 0;
        asm volatile (
            "lzcnt %%eax, %%ecx \n\t"
            "shlx  %%ecx, %%eax, %%ebx \n\t"
            "sarx  %%ecx, %%ebx, %%edx \n\t"
            :"=d"(result),"=c"(count):"a"(arg):"rbx");
        return result;
    }
}

TEST_F(TestAsmInstructions, SetLowOnes) {
    FilledBitSet count = 0;
    constexpr FilledBitSet BitSetWidth = sizeof(FilledBitSet) * 8;
    static_assert(BitSetWidth == 32, "Unexpected uint size");

    for(FilledBitSet i=0; i < BitSetWidth; ++i) {
        const FilledBitSet expectedCount = 31 - i;
        FilledBitSet pattern = 1;
        pattern <<= i;
        const FilledBitSet mask = pattern - 1;
        const FilledBitSet expected = pattern | mask;

        EXPECT_EQ(expected, SplitByMsb(pattern, count));
        EXPECT_EQ(expectedCount, count);

        uint32_t lowBits = 0xaaaaaaaa & mask;
        EXPECT_EQ(expected, SplitByMsb(pattern | lowBits, count));
        EXPECT_EQ(expectedCount, count);

        lowBits = 0x55555555 & mask;
        EXPECT_EQ(expected, SplitByMsb(pattern | lowBits, count));
        EXPECT_EQ(expectedCount, count);
    }

    EXPECT_FALSE(SplitByMsb(0, count));
    EXPECT_EQ(BitSetWidth, count);

    EXPECT_EQ(0xffffffffu, SplitByMsb(0x80000000u, count));
    EXPECT_EQ(0, count);

    EXPECT_EQ(0xffffffffu, SplitByMsb(0xc0000000u, count));
    EXPECT_EQ(0, count);
}

TEST_F(TestAsmInstructions, SetHighOnes) {
    FilledBitSet count = 0;
    constexpr FilledBitSet BitSetWidth = sizeof(FilledBitSet) * 8;
    static_assert(BitSetWidth == 32, "Unexpected uint size");

    for(FilledBitSet i=0; i < BitSetWidth; ++i) {
        const FilledBitSet expectedCount = 31 - i;
        FilledBitSet diff = 1;
        diff <<= i;
        FilledBitSet mask = diff - 1;
        FilledBitSet expected = 0;
        expected -= diff;

        uint32_t arg = diff;
        EXPECT_EQ(expected, SetOnesForLeadingZeros(arg, count));
        EXPECT_EQ(expectedCount, count);

        uint32_t highBits = ~mask;
        uint32_t lowBits = 0xaaaaaaaa & mask;
        expected = highBits | lowBits;
        EXPECT_EQ(expected, SetOnesForLeadingZeros(arg | lowBits, count));
        EXPECT_EQ(expectedCount, count);

        lowBits = 0x55555555 & mask;
        expected = highBits | lowBits;
        EXPECT_EQ(expected, SetOnesForLeadingZeros(arg | lowBits, count));
        EXPECT_EQ(expectedCount, count);
    }

    EXPECT_FALSE(SetOnesForLeadingZeros(0, count));
    EXPECT_EQ(BitSetWidth, count);

    EXPECT_EQ(0x80000000u, SetOnesForLeadingZeros(0x80000000u, count));
    EXPECT_EQ(0, count);

    EXPECT_EQ(0xc0000000u, SetOnesForLeadingZeros(0xc0000000u, count));
    EXPECT_EQ(0, count);
}

TEST_F(TestAsmInstructions, BitScanReverse) {
    constexpr FilledBitSet BitSetWidth = sizeof(FilledBitSet) * 8;
    static_assert(BitSetWidth == 32, "Unexpected uint size");

    for(FilledBitSet i=0; i < BitSetWidth; ++i) {
        const FilledBitSet expectedBsr = i;
        FilledBitSet arg = 1;
        arg <<= i;

        FilledBitSet actual = 0;
        asm volatile (
            "bsr %%eax, %%edx \n\t"
            :"=d"(actual):"a"(arg):);
        EXPECT_EQ(expectedBsr, actual);

        const FilledBitSet expectedLzcnt = BitSetWidth - 1 - i;
        actual = 0;
        asm volatile (
            "lzcnt %%eax, %%edx \n\t"
            :"=d"(actual):"a"(arg):);
        EXPECT_EQ(expectedLzcnt, actual);
    }

    constexpr FilledBitSet argZero = 0;
    FilledBitSet actual = 0;
    asm volatile (
        "tzcnt %%eax, %%edx \n\t"
        :"=d"(actual):"a"(argZero):);
    EXPECT_EQ(BitSetWidth, actual);

    actual = 0;
    asm volatile (
        "lzcnt %%eax, %%edx \n\t"
        :"=d"(actual):"a"(argZero):);
    EXPECT_EQ(BitSetWidth, actual);

    constexpr uint64_t argZero64 = 0;
    uint64_t actual64 = 0;
    asm volatile (
        "tzcnt %%rax, %%rdx \n\t"
        :"=d"(actual64):"a"(argZero64):);
    EXPECT_EQ(64, actual64);

    actual64 = 0;
    asm volatile (
        "lzcnt %%rax, %%rdx \n\t"
        :"=d"(actual64):"a"(argZero64):);
    EXPECT_EQ(64, actual64);
}

namespace {
    struct XmmRegValue {
        alignas(16) unsigned char values[16] {0};
    };

    void ReLU(XmmRegValue& xmmRegValue) {
        asm volatile (
            "xorps  %%xmm4, %%xmm4 \n\t"
            "movaps (%0),   %%xmm5 \n\t"
            "maxps  %%xmm5, %%xmm4 \n\t"
            "movaps %%xmm4, (%0)   \n\t"
            ::"r"(xmmRegValue.values):"xmm4", "xmm5", "memory");
        return;
    }

    int16_t SaturatedAddInt16(int16_t x, int16_t y) {
        XmmRegValue reg;
        ::memmove(reg.values, &x, sizeof(x));
        ::memmove(reg.values + 2, &y, sizeof(y));

        asm volatile (
            "movdqa  (%0), %%xmm4 \n\t"
            "phaddsw %%xmm4, %%xmm4 \n\t"
            "movdqa  %%xmm4, (%0) \n\t"
            ::"r"(reg.values):"xmm4", "memory");

        return *(std::launder(reinterpret_cast<int16_t*>(reg.values)));
    }

    uint16_t SaturatedAddUint16(uint16_t x, uint16_t y) {
        uint16_t sum = 0;
        asm volatile (
            "mov  %%bx, %%ax \n\t"
            "add  %%cx, %%ax \n\t"
            "sbb  %%r10w, %%r10w \n\t"
            "or   %%r10w, %%ax \n\t"
            :"=a"(sum):"b"(x),"c"(y):"r10");

        return sum;
    }

    uint16_t SaturatedAddUint16cmov(uint16_t x, uint16_t y) {
        uint16_t sum = 0;
        asm volatile (
            "xor   %%r10w, %%r10w \n\t"
            "not   %%r10w \n\t"
            "mov   %%bx, %%ax \n\t"
            "add   %%cx, %%ax \n\t"
            "cmovc %%r10w, %%ax \n\t"
            :"=a"(sum):"b"(x),"c"(y):"r10");

        return sum;
    }

    void threeBytesXchg(void) {
        // 0x48 REX.W 0100b (fixed) and 1000b (64 Bit Operand Size)
        // 0x87 XCHG
        // 0xc0
        //   2-bit ModR/M.mod (11b : direct register access)
        //   3-bit a destination register operand
        //   3-bit a source register operand
        asm volatile (
            ".byte  0x48, 0x87, 0xc0 \n\t"
            :::"rax", "memory");
        return;
    }
}

TEST_F(TestAsmInstructions, MaxDoubles) {
    using FloatingType = float;
    XmmRegValue actual;
    constexpr size_t n_float = sizeof(actual.values) / sizeof(FloatingType);
    FloatingType values[n_float] {2.5, 10, -7.5, 1.0};
    const FloatingType expected[n_float] {2.5, 10, 0, 1.0};
    static_assert(sizeof(actual.values) == sizeof(values), "Must have a same size");
    static_assert(sizeof(actual.values) == sizeof(expected), "Must have a same size");

    ::memmove(actual.values, values, sizeof(values));
    ReLU(actual);
    EXPECT_EQ(0, ::memcmp(expected, &actual.values, sizeof(expected)));
}

TEST_F(TestAsmInstructions, SaturatedAddInt16) {
    using IntType = int16_t;
    using BigIntType = int64_t;
    constexpr auto minValue = std::numeric_limits<IntType>::min();
    constexpr auto maxValue = std::numeric_limits<IntType>::max();

    const std::vector<int16_t> valueSet {
        minValue, minValue + 1,
        minValue/2 - 1, minValue/2, minValue/2 + 1,
        -1, 0, 1,
        maxValue/2 - 1, maxValue/2, maxValue/2 + 1,
        maxValue - 1, maxValue
    };

    constexpr BigIntType lowerLimit = minValue;
    constexpr BigIntType upperLimit = maxValue;
    for (const auto& x : valueSet) {
        for (const auto& y : valueSet) {
            BigIntType intX = x;
            BigIntType intY = y;
            BigIntType expected = std::clamp(intX + intY, lowerLimit, upperLimit);
            BigIntType actual = SaturatedAddInt16(x, y);
            EXPECT_EQ(expected, actual);
        }
    }
}

TEST_F(TestAsmInstructions, SaturatedAddUint16) {
    using UintType = uint16_t;
    using BigUintType = uint64_t;
    constexpr auto maxValue = std::numeric_limits<UintType>::max();
    const std::vector<uint16_t> valueSet {
        0, 1, maxValue/2 - 1, maxValue/2, maxValue/2 + 1,
        maxValue - 1, maxValue
    };

    constexpr BigUintType upperLimit = maxValue;
    for (const auto& x : valueSet) {
        for (const auto& y : valueSet) {
            BigUintType uintX = x;
            BigUintType uintY = y;
            BigUintType expected = std::min(uintX + uintY, upperLimit);
            BigUintType actual = SaturatedAddUint16(x, y);
            EXPECT_EQ(expected, actual);
            actual = SaturatedAddUint16cmov(x, y);
            EXPECT_EQ(expected, actual);
        }
    }
}

#if 1
TEST_F(TestAsmInstructions, SaturatedAddInt16Full) {
    using IntType = int16_t;
    using BigIntType = int64_t;
    constexpr auto minValue = std::numeric_limits<IntType>::min();
    constexpr auto maxValue = std::numeric_limits<IntType>::max();
    constexpr BigIntType lowerLimit = minValue;
    constexpr BigIntType upperLimit = maxValue;
    for (IntType x = minValue; x < maxValue; ++x) {
        for (IntType y = minValue; y < maxValue; ++y) {
            BigIntType intX = x;
            BigIntType intY = y;
            BigIntType expected = std::clamp(intX + intY, lowerLimit, upperLimit);
            BigIntType actual = SaturatedAddInt16(x, y);
            EXPECT_EQ(expected, actual);
        }
    }
}

TEST_F(TestAsmInstructions, SaturatedAddUint16Full) {
    using UintType = uint16_t;
    using BigUintType = uint64_t;
    constexpr auto maxValue = std::numeric_limits<UintType>::max();

    constexpr BigUintType upperLimit = maxValue;
    for (UintType x = 0; x < maxValue; ++x) {
        for (UintType y = 0; y < maxValue; ++y) {
            BigUintType uintX = x;
            BigUintType uintY = y;
            BigUintType expected = std::min(uintX + uintY, upperLimit);
            BigUintType actual = SaturatedAddUint16(x, y);
            EXPECT_EQ(expected, actual);
            actual = SaturatedAddUint16cmov(x, y);
            EXPECT_EQ(expected, actual);
        }
    }
}
#endif

TEST_F(TestAsmInstructions, ThreeBytesXchg) {
    threeBytesXchg();
}

/*
  Local Variables:
  mode: c++
  coding: utf-8-dos
  tab-width: nil
  c-file-style: "stroustrup"
  End:
*/


/** @file
 *****************************************************************************

 Implementation of interfaces for components for Miller loops.

 See weierstrass_miller_loop.hpp .

 *****************************************************************************
 * @author     This file is part of libsnark, developed by SCIPR Lab
 *             and contributors (see AUTHORS).
 * @copyright  MIT license (see LICENSE file)
 *****************************************************************************/

#ifndef CRYPTO3_ZK_BLUEPRINT_WEIERSTRASS_MILLER_LOOP_TEST_HPP
#define CRYPTO3_ZK_BLUEPRINT_WEIERSTRASS_MILLER_LOOP_TEST_HPP

#include <boost/test/unit_test.hpp>

#include <nil/crypto3/algebra/algorithms/pair.hpp>

#include <nil/crypto3/zk/components/algebra/curves/weierstrass/element_g1.hpp>
#include <nil/crypto3/zk/components/algebra/curves/weierstrass/element_g2.hpp>

#include <nil/crypto3/zk/components/algebra/pairing/weierstrass/precomputation.hpp>
#include <nil/crypto3/zk/components/algebra/pairing/weierstrass/miller_loop.hpp>

#include <nil/crypto3/algebra/random_element.hpp>
#include <nil/crypto3/algebra/algorithms/pair.hpp>

using namespace nil::crypto3::zk;
using namespace nil::crypto3::algebra;

template<typename CurveType>
void test_mnt_miller_loop() {

    using curve_type = CurveType;
    using pair_curve_type = typename curve_type::pairing::pair_curve_type;
    using curve_pairing_policy = typename curve_type::pairing;
    using other_curve_pairing_policy = typename pair_curve_type::pairing;

    using component_policy = components::detail::basic_pairing_component_policy<CurveType>;

    components::blueprint<typename curve_type::scalar_field_type> bp;
    typename pair_curve_type::g1_type::value_type P_val =
        random_element<typename pair_curve_type::scalar_field_type>() * pair_curve_type::g1_type::value_type::one();
    typename pair_curve_type::g2_type::value_type Q_val =
        random_element<typename pair_curve_type::scalar_field_type>() * pair_curve_type::g2_type::value_type::one();

    components::element_g1<curve_type> P(bp);
    components::element_g2<curve_type> Q(bp);

    components::g1_precomputation<curve_type> prec_P;
    components::g2_precomputation<curve_type> prec_Q;

    components::precompute_G1_component<curve_type> compute_prec_P(bp, P, prec_P);
    components::precompute_G2_component<curve_type> compute_prec_Q(bp, Q, prec_Q);

    typename component_policy::Fqk_variable_type result(bp);
    components::mnt_miller_loop_component<curve_type> miller(bp, prec_P, prec_Q, result);

    compute_prec_P.generate_r1cs_constraints();

    compute_prec_Q.generate_r1cs_constraints();

    miller.generate_r1cs_constraints();

    P.generate_r1cs_witness(P_val);
    compute_prec_P.generate_r1cs_witness();
    Q.generate_r1cs_witness(Q_val);
    compute_prec_Q.generate_r1cs_witness();
    miller.generate_r1cs_witness();
    BOOST_CHECK(bp.is_satisfied());

    typename other_curve_pairing_policy::affine_ate_g1_precomp native_prec_P =
        affine_ate_precompute_g1<pair_curve_type>(P_val);
    typename other_curve_pairing_policy::affine_ate_g2_precomp native_prec_Q =
        affine_ate_precompute_g2<pair_curve_type>(Q_val);
    typename other_curve_pairing_policy::fqk_type::value_type native_result =
        affine_ate_miller_loop<pair_curve_type>(native_prec_P, native_prec_Q);

    BOOST_CHECK(result.get_element() == native_result);
    std::cout << "number of constraints for Miller loop" << bp.num_constraints() << std::endl;
}

template<typename CurveType>
void test_mnt_e_over_e_miller_loop() {

    using curve_type = CurveType;
    using pair_curve_type = typename curve_type::pairing::pair_curve_type;
    using curve_pairing_policy = typename curve_type::pairing;
    using other_curve_pairing_policy = typename pair_curve_type::pairing;

    using component_policy = components::detail::basic_pairing_component_policy<CurveType>;

    components::blueprint<typename curve_type::scalar_field_type> bp;
    typename pair_curve_type::g1_type::value_type P1_val =
        random_element<typename pair_curve_type::scalar_field_type>() * pair_curve_type::g1_type::value_type::one();
    typename pair_curve_type::g2_type::value_type Q1_val =
        random_element<typename pair_curve_type::scalar_field_type>() * pair_curve_type::g2_type::value_type::one();

    typename pair_curve_type::g1_type::value_type P2_val =
        random_element<typename pair_curve_type::scalar_field_type>() * pair_curve_type::g1_type::value_type::one();
    typename pair_curve_type::g2_type::value_type Q2_val =
        random_element<typename pair_curve_type::scalar_field_type>() * pair_curve_type::g2_type::value_type::one();

    components::element_g1<curve_type> P1(bp);
    components::element_g2<curve_type> Q1(bp);
    components::element_g1<curve_type> P2(bp);
    components::element_g2<curve_type> Q2(bp);

    components::g1_precomputation<curve_type> prec_P1;
    components::precompute_G1_component<curve_type> compute_prec_P1(bp, P1, prec_P1);
    components::g1_precomputation<curve_type> prec_P2;
    components::precompute_G1_component<curve_type> compute_prec_P2(bp, P2, prec_P2);
    components::g2_precomputation<curve_type> prec_Q1;
    components::precompute_G2_component<curve_type> compute_prec_Q1(bp, Q1, prec_Q1);
    components::g2_precomputation<curve_type> prec_Q2;
    components::precompute_G2_component<curve_type> compute_prec_Q2(bp, Q2, prec_Q2);

    typename component_policy::Fqk_variable_type result(bp);
    components::mnt_e_over_e_miller_loop_component<curve_type> miller(bp, prec_P1, prec_Q1, prec_P2, prec_Q2, result);

    compute_prec_P1.generate_r1cs_constraints();
    compute_prec_P2.generate_r1cs_constraints();

    compute_prec_Q1.generate_r1cs_constraints();
    compute_prec_Q2.generate_r1cs_constraints();

    miller.generate_r1cs_constraints();

    P1.generate_r1cs_witness(P1_val);
    compute_prec_P1.generate_r1cs_witness();
    Q1.generate_r1cs_witness(Q1_val);
    compute_prec_Q1.generate_r1cs_witness();
    P2.generate_r1cs_witness(P2_val);
    compute_prec_P2.generate_r1cs_witness();
    Q2.generate_r1cs_witness(Q2_val);
    compute_prec_Q2.generate_r1cs_witness();
    miller.generate_r1cs_witness();
    BOOST_CHECK(bp.is_satisfied());

    typename other_curve_pairing_policy::affine_ate_g1_precomp native_prec_P1 =
        affine_ate_precompute_g1<pair_curve_type>(P1_val);
    typename other_curve_pairing_policy::affine_ate_g2_precomp native_prec_Q1 =
        affine_ate_precompute_g2<pair_curve_type>(Q1_val);
    typename other_curve_pairing_policy::affine_ate_g1_precomp native_prec_P2 =
        affine_ate_precompute_g1<pair_curve_type>(P2_val);
    typename other_curve_pairing_policy::affine_ate_g2_precomp native_prec_Q2 =
        affine_ate_precompute_g2<pair_curve_type>(Q2_val);
    typename other_curve_pairing_policy::fqk_type::value_type native_result =
        (affine_ate_miller_loop<pair_curve_type>(native_prec_P1, native_prec_Q1) *
         affine_ate_miller_loop<pair_curve_type>(native_prec_P2, native_prec_Q2).inversed());

    BOOST_CHECK(result.get_element() == native_result);
    std::cout << "number of constraints for e over e Miller loop " << bp.num_constraints() << std::endl;
}

template<typename CurveType>
void test_mnt_e_times_e_over_e_miller_loop() {

    using curve_type = CurveType;
    using pair_curve_type = typename curve_type::pairing::pair_curve_type;
    using curve_pairing_policy = typename curve_type::pairing;
    using other_curve_pairing_policy = typename pair_curve_type::pairing;

    using component_policy = components::detail::basic_pairing_component_policy<CurveType>;

    components::blueprint<typename curve_type::scalar_field_type> bp;
    typename pair_curve_type::g1_type::value_type P1_val =
        random_element<typename pair_curve_type::scalar_field_type>() * pair_curve_type::g1_type::value_type::one();
    typename pair_curve_type::g2_type::value_type Q1_val =
        random_element<typename pair_curve_type::scalar_field_type>() * pair_curve_type::g2_type::value_type::one();

    typename pair_curve_type::g1_type::value_type P2_val =
        random_element<typename pair_curve_type::scalar_field_type>() * pair_curve_type::g1_type::value_type::one();
    typename pair_curve_type::g2_type::value_type Q2_val =
        random_element<typename pair_curve_type::scalar_field_type>() * pair_curve_type::g2_type::value_type::one();

    typename pair_curve_type::g1_type::value_type P3_val =
        random_element<typename pair_curve_type::scalar_field_type>() * pair_curve_type::g1_type::value_type::one();
    typename pair_curve_type::g2_type::value_type Q3_val =
        random_element<typename pair_curve_type::scalar_field_type>() * pair_curve_type::g2_type::value_type::one();

    components::element_g1<curve_type> P1(bp);
    components::element_g2<curve_type> Q1(bp);
    components::element_g1<curve_type> P2(bp);
    components::element_g2<curve_type> Q2(bp);
    components::element_g1<curve_type> P3(bp);
    components::element_g2<curve_type> Q3(bp);

    components::g1_precomputation<curve_type> prec_P1;
    components::precompute_G1_component<curve_type> compute_prec_P1(bp, P1, prec_P1);
    components::g1_precomputation<curve_type> prec_P2;
    components::precompute_G1_component<curve_type> compute_prec_P2(bp, P2, prec_P2);
    components::g1_precomputation<curve_type> prec_P3;
    components::precompute_G1_component<curve_type> compute_prec_P3(bp, P3, prec_P3);
    components::g2_precomputation<curve_type> prec_Q1;
    components::precompute_G2_component<curve_type> compute_prec_Q1(bp, Q1, prec_Q1);
    components::g2_precomputation<curve_type> prec_Q2;
    components::precompute_G2_component<curve_type> compute_prec_Q2(bp, Q2, prec_Q2);
    components::g2_precomputation<curve_type> prec_Q3;
    components::precompute_G2_component<curve_type> compute_prec_Q3(bp, Q3, prec_Q3);

    typename component_policy::Fqk_variable_type result(bp);
    components::mnt_e_times_e_over_e_miller_loop_component<curve_type> miller(bp, prec_P1, prec_Q1, prec_P2, prec_Q2,
                                                                              prec_P3, prec_Q3, result);

    compute_prec_P1.generate_r1cs_constraints();
    compute_prec_P2.generate_r1cs_constraints();
    compute_prec_P3.generate_r1cs_constraints();

    compute_prec_Q1.generate_r1cs_constraints();
    compute_prec_Q2.generate_r1cs_constraints();
    compute_prec_Q3.generate_r1cs_constraints();

    miller.generate_r1cs_constraints();

    P1.generate_r1cs_witness(P1_val);
    compute_prec_P1.generate_r1cs_witness();
    Q1.generate_r1cs_witness(Q1_val);
    compute_prec_Q1.generate_r1cs_witness();
    P2.generate_r1cs_witness(P2_val);
    compute_prec_P2.generate_r1cs_witness();
    Q2.generate_r1cs_witness(Q2_val);
    compute_prec_Q2.generate_r1cs_witness();
    P3.generate_r1cs_witness(P3_val);
    compute_prec_P3.generate_r1cs_witness();
    Q3.generate_r1cs_witness(Q3_val);
    compute_prec_Q3.generate_r1cs_witness();
    miller.generate_r1cs_witness();
    BOOST_CHECK(bp.is_satisfied());

    typename other_curve_pairing_policy::affine_ate_g1_precomp native_prec_P1 =
        affine_ate_precompute_g1<pair_curve_type>(P1_val);
    typename other_curve_pairing_policy::affine_ate_g2_precomp native_prec_Q1 =
        affine_ate_precompute_g2<pair_curve_type>(Q1_val);
    typename other_curve_pairing_policy::affine_ate_g1_precomp native_prec_P2 =
        affine_ate_precompute_g1<pair_curve_type>(P2_val);
    typename other_curve_pairing_policy::affine_ate_g2_precomp native_prec_Q2 =
        affine_ate_precompute_g2<pair_curve_type>(Q2_val);
    typename other_curve_pairing_policy::affine_ate_g1_precomp native_prec_P3 =
        affine_ate_precompute_g1<pair_curve_type>(P3_val);
    typename other_curve_pairing_policy::affine_ate_g2_precomp native_prec_Q3 =
        affine_ate_precompute_g2<pair_curve_type>(Q3_val);
    typename other_curve_pairing_policy::fqk_type::value_type native_result =
        (affine_ate_miller_loop<pair_curve_type>(native_prec_P1, native_prec_Q1) *
         affine_ate_miller_loop<pair_curve_type>(native_prec_P2, native_prec_Q2) *
         affine_ate_miller_loop<pair_curve_type>(native_prec_P3, native_prec_Q3).inversed());

    BOOST_CHECK(result.get_element() == native_result);
    std::cout << "number of constraints for e times e over e Miller loop " << bp.num_constraints() << std::endl;
}

#endif    // CRYPTO3_ZK_BLUEPRINT_WEIERSTRASS_MILLER_LOOP_TEST_HPP


#include <string>
#include <vector>
#include <stdexcept>
#include <math.h>
#include <map>

#include <Eigen/Dense>
#include <autodiff/forward.hpp>
#include <autodiff/forward/eigen.hpp>

#include "uom.h"
#include "thermodynamics.h"

using namespace std;

namespace Thermodynamics
{

    namespace Types
    {

        Equilibrium3PropertiesExt export_properties3(Equilibrium3Properties props)
        {
            auto NC= props.z.size();

            Equilibrium3PropertiesExt result;
            result.T = props.T.val;
            result.P = props.P.val;
            result.vf = props.vf.val;
            result.l1f = props.l1f.val;
            result.l2f = props.l2f.val;
            result.z = std::vector<double>(NC);
            result.x1 = std::vector<double>(NC);
            result.x2 = std::vector<double>(NC);
            result.y = std::vector<double>(NC);
            result.phase = props.phase;
            for (auto i = 0; i < props.z.size(); i++)
            {
                result.z[i] = props.z[i].val;
                result.x1[i] = props.x1[i].val;
                result.x2[i] = props.x2[i].val;
                result.y[i] = props.y[i].val;
            }
            return result;
        }

        EquilibriumPropertiesExt export_properties(EquilibriumProperties props)
        {
            auto NC= props.z.size();

            EquilibriumPropertiesExt result;
            result.T = props.T.val;
            result.P = props.P.val;
            result.vf = props.vf.val;
            result.z = std::vector<double>(NC);
            result.x = std::vector<double>(NC);
            result.y = std::vector<double>(NC);
            result.phase = props.phase;
            for (auto i = 0; i < props.z.size(); i++)
            {
                result.z[i] = props.z[i].val;
                result.x[i] = props.x[i].val;
                result.y[i] = props.y[i].val;
            }
            return result;
        }
    }
}


// Copyright 2016. All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//
// * Redistributions of source code must retain the above copyright notice,
//   this list of conditions and the following disclaimer.
// * Redistributions in binary form must reproduce the above copyright notice,
//   this list of conditions and the following disclaimer in the documentation
//   and/or other materials provided with the distribution.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
//
// Author: ryan.latture@gmail.com (Ryan Latture)

#include "mesh.h"
#include <Eigen/Dense>
#include <iostream>

namespace explicit_fea {
    Mesh::Mesh(const std::vector<Node> &nodes,
               const std::vector<std::unique_ptr<BeamElement>>& elems,
               BCList bcs) :
        bcs(std::move(bcs))
    {
        assemble_matrices(nodes, elems);
        apply_bcs();
    }

    SparseMatrix const& Mesh::get_global_stiffness_matrix() const {
        return global_stiffness_matrix;
    }

    SparseMatrix const& Mesh::get_inv_mass_matrix() const {
        return inverse_mass_matrix;
    }

    SparseMatrix const& Mesh::get_mass_matrix() const {
        return mass_matrix;
    }

    BCList const& Mesh::get_bcs() const {
        return bcs;
    }

    void Mesh::assemble_matrices(const std::vector<Node> &nodes, const std::vector<std::unique_ptr<BeamElement>>& elems) {
        int n = DOF::NUM_DOFS * nodes.size();
        global_stiffness_matrix.resize(n, n);
        mass_matrix.resize(n, n);
        inverse_mass_matrix.resize(n, n);

        std::vector<Eigen::Triplet<double> > stiffness_triplets, mass_triplets, inv_mass_triplets;
        stiffness_triplets.reserve(40 * elems.size());
        mass_triplets.reserve(40 * elems.size());
        inv_mass_triplets.reserve(40 * elems.size());
        LocalMatrix inv_mass, mass;

        for (size_t i = 0; i < elems.size(); ++i) {
            inv_mass = elems[i]->calculate_inv_mass_matrix(nodes);
            mass = inv_mass.inverse();
            append_triplets(*elems[i], stiffness_triplets, elems[i]->calculate_stiffness_matrix(nodes));
            append_triplets(*elems[i], mass_triplets, mass);
            append_triplets(*elems[i], inv_mass_triplets, inv_mass);
        }
        global_stiffness_matrix.setFromTriplets(stiffness_triplets.begin(), stiffness_triplets.end());
        inverse_mass_matrix.setFromTriplets(inv_mass_triplets.begin(), inv_mass_triplets.end());
        mass_matrix.setFromTriplets(mass_triplets.begin(), mass_triplets.end());

        global_stiffness_matrix.prune(1.e-14);
        mass_matrix.prune(1.e-14);
        inverse_mass_matrix.prune(1.e-14);

        global_stiffness_matrix.makeCompressed();
        mass_matrix.makeCompressed();
        inverse_mass_matrix.makeCompressed();
    }

    void Mesh::append_triplets(const BeamElement& elem, std::vector<Eigen::Triplet<double> > &triplets, const LocalMatrix& mat) {
        const int nn1 = elem.get_node_numbers()[0];
        const int nn2 = elem.get_node_numbers()[1];
        size_t row, col;
        const unsigned int dofs_per_elem = DOF::NUM_DOFS;

        SparseMatrix sparse_mat = mat.sparseView();

        for (size_t j = 0; j < sparse_mat.outerSize(); ++j) {
            for (SparseMatrix::InnerIterator it(sparse_mat, j); it; ++it) {
                row = it.row();
                col = it.col();

                // check position in local matrix and update corresponding global position
                if (row < dofs_per_elem) {
                    // top left
                    if (col < dofs_per_elem) {
                        triplets.push_back(Eigen::Triplet<double>(dofs_per_elem * nn1 + row,
                                                                  dofs_per_elem * nn1 + col,
                                                                  it.value()));
                    }
                        // top right
                    else {
                        triplets.push_back(Eigen::Triplet<double>(dofs_per_elem * nn1 + row,
                                                                  dofs_per_elem * (nn2 - 1) + col,
                                                                  it.value()));
                    }
                }
                else {
                    // bottom left
                    if (col < dofs_per_elem) {
                        triplets.push_back(Eigen::Triplet<double>(dofs_per_elem * (nn2 - 1) + row,
                                                                  dofs_per_elem * nn1 + col,
                                                                  it.value()));
                    }
                        // bottom right
                    else {
                        triplets.push_back(Eigen::Triplet<double>(dofs_per_elem * (nn2 - 1) + row,
                                                                  dofs_per_elem * (nn2 - 1) + col,
                                                                  it.value()));
                    }
                }
            }
        }
    }

    void Mesh::apply_bcs() {
        BcsPruneFunctor prune_functor(bcs);
        inverse_mass_matrix.prune(prune_functor);
        for (std::set<int>::const_iterator it = prune_functor.bcs_ind_set.begin();
             it != prune_functor.bcs_ind_set.end(); ++it) {
            inverse_mass_matrix.insert(*it, *it) = 1.0;
        }
    }

    Mesh::BcsPruneFunctor::BcsPruneFunctor(const BCList &bcs){
        for (BCList::const_iterator it = bcs.begin(); it != bcs.end(); ++it) {
            bcs_ind_set.insert((*it)->global_index);
        }
    }

    bool Mesh::BcsPruneFunctor::operator()(int row, int col, double value) const {
        bool row_exists = bcs_ind_set.find(row) != bcs_ind_set.end();
        if (row_exists)
            return false;

        bool col_exists = bcs_ind_set.find(col) != bcs_ind_set.end();
        return !col_exists;
    }

} // namespace explicit_fea