#pragma once

#include <iostream>
#include <memory>
#include <valarray>
#include <functional>
#include <utility>
#include <list>
#include <cstdint>
#include <vector>

#include <boost/optional.hpp>

typedef char Alphabet;
typedef uint32_t State;
typedef State TAState;
typedef State RAState;
typedef State ZAState;

//! @brief The return values of comparison of two values. Similar to strcmp.
enum class Order {
  LT,EQ,GT
};

inline bool toBool (Order odr) {
  return odr == Order::EQ;
}

struct Constraint {
  using Variables = char;
  enum class Order {
    lt,le,ge,gt
  };

  Variables x;
  Order odr;
  int c;

  bool satisfy (double d) const {
    switch (odr) {
    case Order::lt:
      return d < c;
    case Order::le:
      return d <= c;
    case Order::gt:
      return d > c;
    case Order::ge:
      return d >= c;
    }
  }
  using Interpretation = std::vector<double>;
  ::Order operator() (Interpretation val) const {
    if (satisfy (val.at (x))) {
      return ::Order::EQ;
    } else if (odr == Order::lt || odr == Order::le) {
      return ::Order::GT;
    } else {
      return ::Order::LT;
    }
  }
};

class ConstraintMaker {
  using Variables = char;
  Variables x;
public:
  ConstraintMaker(Variables x) : x(x) {}
  Constraint operator<(int c) {
    return Constraint {x,Constraint::Order::lt,c};
  }
  Constraint operator<=(int c) {
    return Constraint {x,Constraint::Order::le,c};
  }
  Constraint operator>(int c) {
    return Constraint {x,Constraint::Order::gt,c};
  }
  Constraint operator>=(int c) {
    return Constraint {x,Constraint::Order::ge,c};
  }
};

template <int NumOfVariables>
struct TimedAutomaton {
  using Variables = char;
  // arrayとどっちがいいかしらない
  using Interpretation = std::array<double,NumOfVariables>;

  struct Edge {
    State source;
    State target;
    Alphabet c;
    std::vector<Alphabet> resetVars;
    std::vector<Constraint> guard;
  };

  // This must be sorted.
  std::vector<State> initialStates;
  // Keys are source nodes.
  std::vector<std::vector<Edge> > edges;
  // This must be sorted.
  std::vector<State> acceptingStates;

  std::vector<int> max_constraints;
  inline size_t stateSize() const {return edges.size ();}
};

struct Region {
  using Interpretation = std::vector<double>;
  using Tuple = std::tuple<std::vector< std::pair<int,int> >, std::list<std::list<int> >, std::vector<int> >;
  // (2,3) -> (2,3), (2,2) -> [2,2], (c,c>) -> (c,inf)
  std::vector< std::pair<int,int> > integer_parts;
  // front() is the least.
  // each vector is sorted
  std::list<std::list<int> > frac_order;

  std::vector<int> max_constraints;

  /*!
    @brief generate next (least time-successor) region

    @note  next region cannot be now itself.

    @retval true  next region does exist
    @retval false next region does not exist
  */
  bool nextRegion (Region &next,bool & ss) const;

  inline bool self_successor () const;

  inline bool operator == (const Region &r) const {
    return integer_parts == std::move(r.integer_parts) && frac_order == std::move(r.frac_order);
  }
  inline bool operator == (const Region &&r) const {
    return integer_parts == std::move(r.integer_parts) && frac_order == std::move(r.frac_order);
  }

  operator Interpretation() const {
    Interpretation ret;
    ret.resize (integer_parts.size());
    for (std::size_t i = 0;i < ret.size();++i) {
      ret[i] = (integer_parts[i].first + integer_parts[i].second) * 0.5;
    }
    return ret;
  }

  inline bool operator> (const Region &r) const {
    return integer_parts > r.integer_parts;
  }
  inline bool operator> (const Region &&r) const {
    return integer_parts > r.integer_parts;
  }

  inline void cutVars (std::shared_ptr<Region> &out,std::size_t from,std::size_t to) {
    if (max_constraints.empty()) {
      out = std::shared_ptr<Region>(this);
      return;
    }
    // memoization
    if (from == 0 && to == integer_parts.size() / 2 - 1 && firstHalf) {
      // std::cout << "memoized" << std::endl;
      out = firstHalf;
      return;
    } else if (from == integer_parts.size() / 2 && to == integer_parts.size() - 1 && secondHalf) {
      // std::cout << "memoized" << std::endl;
      out = secondHalf;
      return;
    }
    // std::cout << "not memoized" << std::endl;

    out = std::make_shared<Region>();
    out->integer_parts = {integer_parts.begin () + from,integer_parts.begin() + to+1};
    out->max_constraints = {max_constraints.begin () + from,max_constraints.begin() + to+1};

    out->frac_order.clear();
    for (const auto &vec:frac_order) {
      std::list<int> newList;
      auto newBegin = std::lower_bound (vec.begin(), vec.end (),from);
      if (newBegin == vec.end ()) {
        break;
      }

      auto newEnd = std::lower_bound (vec.begin(), vec.end (),to);
      if (newEnd != vec.end ()) {
        newEnd++;
      }

      newList = {newBegin,newEnd};
      for_each (newList.begin (),newList.end (),[=] (int &v){v -= from;});
      out->frac_order.push_back (newList);
    }

    // memoization
    if (from == 0 && to == integer_parts.size() / 2 - 1) {
      firstHalf = out;
    } else if (from == integer_parts.size() / 2 && to == integer_parts.size() - 1) {
      secondHalf = out;
    }
  }
  inline Tuple toTuple () {
    return std::make_tuple(integer_parts,frac_order,max_constraints);
  }
  inline std::size_t getNumOfVar() const {
    return max_constraints.size();
  }
private:
  std::shared_ptr<Region> firstHalf;
  std::shared_ptr<Region> secondHalf;
};

struct NFA {
  struct Edge {
    State source;
    State target;
    Alphabet c;
    inline bool operator == (const Edge e) const {
      return source == e.source && target == e.target && c == e.c;
    }
    std::tuple<State, State, Alphabet> toTuple() const {
      return std::tuple<State, State, Alphabet>(source, target, c);
    }
  };

  // This must be sorted.
  std::vector<State> initialStates;
  // Keys are source nodes.
  std::vector<std::vector<Edge> > edges;
  // This must be sorted.
  std::vector<State> acceptingStates;
  inline bool operator == (const NFA A) const {
    return initialStates == A.initialStates &&
      edges == A.edges &&
      acceptingStates == A.acceptingStates;
  }
};

template <typename Abstraction>
struct AbstractionAutomaton : public NFA {
  std::vector<std::pair<State, Abstraction> > abstractedStates;
};

struct RegionAutomaton : public AbstractionAutomaton<Region> {
  bool isPartialRun (std::vector<State>) const;
  bool isPartialRun2 (const RegionAutomaton &,const std::vector<State>&,const std::vector<State>&,const TAState);
};

std::ostream& operator << (std::ostream& os, const Region& r);


// (C) Copyright David James Pearce 2003. Permission to copy, use,
// modify, sell and distribute this software is granted provided this
// copyright notice appears in all copies. This software is provided
// "as is" without express or implied warranty, and with no claim as
// to its suitability for any purpose.
//
// Email: david.pearce@mcs.vuw.ac.nz
//
// Notes
// =====
// This is another solution to the online topological
// order problem.  It was first described in the
// following:
//
// [1] B. Alpern, R. Hoover, B. K. Rosen, P. F. Sweeney and F. K.
//     Zadeck, "Incremental Evaluation of Computational Circuits",
//     In Proc. of the First Annual ACM-SIAM Symposium on Discrete
//     Algorithms, pages 32-42, 1990.

#ifndef AHRSZ_ONLINE_TOPOLOGICAL_ORDER_HPP
#define AHRSZ_ONLINE_TOPOLOGICAL_ORDER_HPP

#include <queue>
#include <vector>
#include <stdexcept>
#include <cassert>
#include <boost/property_map.hpp>
#include <boost/graph/topological_sort.hpp>
#include <boost/lexical_cast.hpp>

#include "ordered_slist.hpp"
#include "ordered_slist2.hpp"
#include "oto_tags.hpp"
#include "myless.hpp"
#include "mygreater.hpp"

#if __GNUC__ >= 3
#include <ext/functional>
using __gnu_cxx::select2nd;
using __gnu_cxx::identity;
#else
using std::select2nd;
using std::identity;
#endif

template<class T, class PSPACE, class N2I>
class ahrsz_online_topological_order;

// The arbitrary assignment algorithm
// is slightly simpler but does not
// minimise the number of new priorities
// created.

// #define AHRSZ_USE_SIMPLE_REASSIGNMENT

// These globals are not strictly needed
// they are used to generate the metrics
#ifdef AHRSZ_GENERATE_STATS
extern unsigned int ahrsz_K;
extern unsigned int ahrsz_dKfb;
extern unsigned int ahrsz_ninvalid;
#endif

// --------------------------------
// priority compare
//
// This gives a comparison based
// on the priority of a node
// --------------------------------

template<class T, class N2I, class Comp>
class ahrsz_priority_comp {
private:
  typedef typename boost::property_map<T, N2I>::type priority_map;
private:
  Comp _comp;
  priority_map _pmap; // efficiency
public:
  ahrsz_priority_comp(priority_map pmap)
    : _pmap(pmap) {
  }

  bool operator()(typename T::vertex_descriptor l,
		  typename T::vertex_descriptor r) const {
    return _comp(_pmap[l],_pmap[r]);
  }
};

template<class PSPACE = ordered_slist<void> >
class ahrsz_priority_value {
private:
  typename PSPACE::iterator _iter;
  PSPACE  const *_ptr;
public:
  ahrsz_priority_value(void)
    : _iter(), _ptr(NULL) {
  }

  ahrsz_priority_value(typename PSPACE::iterator const &iter,
		       PSPACE const &pspace)
  : _iter(iter), _ptr(&pspace) {
  }

  bool operator<(ahrsz_priority_value const &src) const {
    assert(_ptr != NULL);
    return _ptr->order_lt(_iter,src._iter);
  }

  bool operator>(ahrsz_priority_value const &src) const {
    assert(_ptr != NULL);
    return _iter != src._iter && !(*this < src);
  }

  bool operator!=(ahrsz_priority_value const &src) const {
    return _iter != src._iter;
  }

  bool operator==(ahrsz_priority_value const &src) const {
    return _iter == src._iter;
  }

  typename PSPACE::iterator &base(void) {
    return _iter;
  }

  ahrsz_priority_value const &operator++(int) {
    ++_iter;
    return *this;
  }

  ahrsz_priority_value operator++(void) {
    ahrsz_priority_value r(*this);
    ++_iter;
    return r;
  }
};

typedef enum {minus_infinity,plus_infinity} ahrsz_ext_priority_value_inf_t;

template<class PSPACE>
class ahrsz_ext_priority_value {
private:
  ahrsz_priority_value<PSPACE> _val;
  bool _minus_inf;
  bool _plus_inf;
public:
  ahrsz_ext_priority_value(ahrsz_ext_priority_value_inf_t f = minus_infinity)
    : _val(), _minus_inf(f == minus_infinity), _plus_inf(f == plus_infinity) {
  }

  ahrsz_ext_priority_value(ahrsz_priority_value<PSPACE> &e)
    : _val(e), _minus_inf(false), _plus_inf(false) {
  }

  ahrsz_ext_priority_value(typename PSPACE::iterator const &iter,
			   PSPACE const &space)
    : _val(iter,space), _minus_inf(false), _plus_inf(false) {
  }

  bool minus_inf(void) const { return _minus_inf; }
  bool plus_inf(void) const { return _plus_inf; }

  bool operator<(ahrsz_ext_priority_value const &src) const {
    if(_minus_inf || src._minus_inf) {
      return _minus_inf && !src._minus_inf;
    } else if(_plus_inf || src._plus_inf) {
      return src._plus_inf && !_plus_inf;
    }
    return _val < src._val;
  }

  bool operator>=(ahrsz_ext_priority_value const &src) const {
    return !(*this < src);
  }

  bool operator==(ahrsz_ext_priority_value const &src) const {
    if(_minus_inf || src._minus_inf) {
      return _minus_inf == src._minus_inf;
    } else if(_plus_inf || src._plus_inf) {
      return _plus_inf == src._plus_inf;
    }
    return _val == src._val;
  }

  typename PSPACE::iterator &base(void) {
    return _val.base();
  }

  ahrsz_ext_priority_value const &operator++(int) {
    ++_val;
    return *this;
  }

  ahrsz_ext_priority_value operator++(void) {
    ahrsz_ext_priority_value r(*this);
    ++_val;
    return r;
  }
};

// --------------
// the main class
// --------------

template<class T, class PSPACE, class N2I>
std::pair<typename T::edge_descriptor, bool>
add_edge(typename ahrsz_online_topological_order<T,PSPACE,N2I>::vertex_descriptor t,
	 typename ahrsz_online_topological_order<T,PSPACE,N2I>::vertex_descriptor h,
	 ahrsz_online_topological_order<T,PSPACE,N2I> &g) {

  std::pair<typename T::edge_descriptor, bool> r;
  r = add_edge(static_cast<typename T::vertex_descriptor>(t),
	       static_cast<typename T::vertex_descriptor>(h),
	       static_cast<T&>(g));
  if(r.second) {
    typename boost::property_map<T, N2I>::type _pmap;
    _pmap = get(N2I(),g);
    if(!(_pmap[t] < _pmap[h])) {
      std::vector<typename T::vertex_descriptor> K;
      g.discovery(t,h,K);
      g.reassignment(K);
#ifdef AHRSZ_GENERATE_STATS
      ++ahrsz_ninvalid;
      ahrsz_K += K.size();
#endif
    }
  }
}

template<class InputIter, class T, class P, class N2I>
void add_edges(InputIter b, InputIter e,
	       ahrsz_online_topological_order<T,P,N2I> &g) {
  for(;b!=e;++b) {
    add_edge(b->first,b->second,g);
  }
}

template<class T, class PSPACE = ordered_slist<void>, class N2I = n2i_t>
class ahrsz_online_topological_order : public T {
public:
  typedef ahrsz_online_topological_order<T,PSPACE,N2I> self;
  friend std::pair<typename T::edge_descriptor, bool> add_edge<T>(typename self::vertex_descriptor,
								  typename self::vertex_descriptor,
								  self &);
private:
  typedef typename self::vertex_descriptor vd_t;
  typedef typename T::out_edge_iterator out_iterator;
  typedef typename T::in_edge_iterator in_iterator;
  typedef std::priority_queue<vd_t,std::vector<vd_t>,ahrsz_priority_comp<
    T,N2I,std::less<ahrsz_priority_value<PSPACE> > > > max_priority_queue;
  typedef std::priority_queue<vd_t,std::vector<vd_t>,ahrsz_priority_comp<
    T,N2I,std::greater<ahrsz_priority_value<PSPACE> > > > min_priority_queue;
  typedef typename boost::property_map<T, N2I>::type n2i_t;

  PSPACE _pspace; // the priority space.
  // temporary storage needed by the reassignment stage
  std::vector<ahrsz_ext_priority_value<PSPACE> > _ceiling;
  std::vector<bool> _visited;
  std::vector<bool> _inK;
#ifndef AHRSZ_USE_SIMPLE_REASSIGNMENT
  std::vector<unsigned int> _indegree;
#endif
public:
  ahrsz_online_topological_order(T const &g, unsigned int a = 1)
    : T(g),
      _pspace(1),
      _ceiling(num_vertices(g),minus_infinity),
      _visited(num_vertices(g),false),
      _inK(num_vertices(g),false)
#ifndef AHRSZ_USE_SIMPLE_REASSIGNMENT
      ,_indegree(num_vertices(g),0)
#endif
  {
    typename boost::property_map<T, N2I>::type _pmap = get(N2I(),*this);

    std::vector<typename T::vertex_descriptor> tmp;

    typename T::vertex_iterator i,iend;
    for(tie(i,iend) = vertices(*this);i!=iend;++i) {
      _pmap[*i] = ahrsz_priority_value<PSPACE>(_pspace.begin(),_pspace);
      tmp.push_back(*i);
    }

    reassignment(tmp);
  }

  ahrsz_online_topological_order(typename T::vertices_size_type n, unsigned int a = 1)
    : T(n), _pspace(1), _ceiling(n,minus_infinity),
      _visited(n,false), _inK(n,false)
#ifndef AHRSZ_USE_SIMPLE_REASSIGNMENT
      ,_indegree(n,0)
#endif
{
    typename boost::property_map<T, N2I>::type _pmap = get(N2I(),*this);

    typename T::vertex_iterator i,iend;
    for(tie(i,iend) = vertices(*this);i!=iend;++i) {
      _pmap[*i] = ahrsz_priority_value<PSPACE>(_pspace.begin(),_pspace);
      assert(!(_pmap[*i] < _pmap[*i]));
    }
  }

  ahrsz_online_topological_order(ahrsz_online_topological_order const &src) {
    copy(src);
  }

  void operator=(ahrsz_online_topological_order const &src) {
    if(&src != this) {
      copy(src);
    }
  }
protected:

  // -----------------------
  // the discovery algorithm
  // -----------------------

  void discovery(vd_t tail, vd_t head,
		 std::vector<vd_t> &K)
  {
    n2i_t n2i(get(N2I(),*this));

    // -------------
    // Function Code
    // -------------

    vd_t f(head);
    vd_t b(tail);
    min_priority_queue ForwFron(get(N2I(),*this));  // the forward frontier
    max_priority_queue BackFron(get(N2I(),*this));  // the backward frontier
    unsigned int ForwEdges(out_degree(head,*this));
    unsigned int BackEdges(in_degree(tail,*this));
    ForwFron.push(head);
    BackFron.push(tail);

    // notice, I uses visited to indicate when
    // a node is on one of the queues.
    _visited[head]=true;
    _visited[tail]=true;

    while(!(n2i[f] > n2i[b]) && !ForwFron.empty() && !BackFron.empty()) {
      unsigned int u=std::min(ForwEdges,BackEdges);
      ForwEdges -= u;
      BackEdges -= u;

      if(ForwEdges == 0) {
	K.push_back(f);
	ForwFron.pop();
#ifdef AHRSZ_GENERATE_STATS
	++ahrsz_dKfb;
#endif
	_visited[f]=false;
	out_iterator i,iend;
	for(tie(i,iend) = out_edges(f,*this);i!=iend;++i) {
	  vd_t w(target(*i,*this));
	  if(!_visited[w]) {
	    ForwFron.push(w);
	    _visited[w]=true;
	  }
#ifdef AHRSZ_GENERATE_STATS
	  ++ahrsz_dKfb;
#endif
	}
	if(ForwFron.empty()) {
	  f = tail;
	} else {
	  f = ForwFron.top();
	}
	ForwEdges = out_degree(f,*this);
      }
      if(BackEdges == 0) {
	K.push_back(b);
	BackFron.pop();
#ifdef AHRSZ_GENERATE_STATS
	++ahrsz_dKfb;
#endif
	_visited[b]=false;
	in_iterator i,iend;
	for(tie(i,iend) = in_edges(b,*this);i!=iend;++i) {
	  vd_t w(source(*i,*this));
	  if(!_visited[w]) {
	    BackFron.push(w);
	    _visited[w]=true;
	  }
#ifdef AHRSZ_GENERATE_STATS
	  ++ahrsz_dKfb;
#endif
	}
	if(BackFron.empty()) {
	  b = head;
	} else {
	  b = BackFron.top();
	}
	BackEdges = in_degree(b,*this);
      }
    }
    // Finally, unmark all remaining on queues
    while(!ForwFron.empty()) {
      _visited[ForwFron.top()]=false;
      ForwFron.pop();
    }
    while(!BackFron.empty()) {
      _visited[BackFron.top()]=false;
      BackFron.pop();
    }
  }

  std::string a2str(ahrsz_ext_priority_value<PSPACE> x) {
    if(x.minus_inf()) {
      return "-oo";
    } else if(x.plus_inf()){
      return "+oo";
    }

    uint64_t r(_pspace.order(x.base()));
    return boost::lexical_cast<std::string>(r);
  }

  std::string a2str(ahrsz_priority_value<PSPACE> x) {
    uint64_t r(_pspace.order(x.base()));
    return boost::lexical_cast<std::string>(r);
  }

  // --------------------------
  // the reassignment algorithm
  // --------------------------

  void reassignment(std::vector<vd_t> &K) {
    n2i_t n2i(get(N2I(),*this));
    // Initialise temporary data
    for(typename std::vector<vd_t>::iterator i(K.begin());
	i!=K.end();++i) {
      _ceiling[*i] = plus_infinity;
      _inK[*i] = true;
    }
    // first pass - compute ceiling Information
#ifdef AHRSZ_USE_SIMPLE_REASSIGNMENT
    std::vector<vd_t> rto; // reverse topological order
    for(typename std::vector<vd_t>::iterator i(K.begin());
	i!=K.end();++i) {
      if(!_visited[*i]) { compute_ceiling(*i,rto,n2i); }
    }

    // second pass - perform the reassignment
    for(typename std::vector<vd_t>::reverse_iterator i(rto.rbegin());i!=rto.rend();++i) {
      ahrsz_ext_priority_value<PSPACE> ep(compute_priority(compute_floor(*i,n2i),_ceiling[*i]));
      n2i[*i] = ahrsz_priority_value<PSPACE>(ep.base(),_pspace);
    }
#else
    // first pass - compute ceiling Information
    for(typename std::vector<vd_t>::iterator i(K.begin());
	i!=K.end();++i) {
      if(!_visited[*i]) { compute_ceiling(*i,n2i); }
    }

    // construct the min-priority queue
    typedef std::pair<vd_t,ahrsz_ext_priority_value<PSPACE> > Q_e;
    typedef my_greater<Q_e,select2nd<Q_e> > Q_c;
    typedef std::priority_queue<Q_e,std::vector<Q_e>,Q_c> Q_t;
    Q_t Q;

    // initialise Q
    for(typename std::vector<vd_t>::iterator i(K.begin());i!=K.end();++i) {
      ahrsz_ext_priority_value<PSPACE> floor(minus_infinity);
      unsigned int kid = 0;
      typename T::in_edge_iterator j,jend;
      for(tie(j,jend) = in_edges(*i,*this);j!=jend;++j) {
	if(_inK[source(*j,*this)]) { ++kid; }
	floor = std::max(floor,ahrsz_ext_priority_value<PSPACE>(n2i[source(*j,*this)]));
      }
      _indegree[*i] = kid;
      if(kid == 0) { Q.push(Q_e(*i,floor)); }
    }

    // now perform the reassignment
    std::vector<vd_t> Z;
    while(!Q.empty()) {
      Z.clear();
      ahrsz_ext_priority_value<PSPACE> Z_floor = Q.top().second;
      ahrsz_ext_priority_value<PSPACE> Z_ceil = plus_infinity;
      while(!Q.empty() && Q.top().second == Z_floor) {
	vd_t x = Q.top().first;
	Z_ceil = std::min(Z_ceil,_ceiling[x]);
	Z.push_back(x);
	Q.pop();
      }

      assert(Q.empty() || Z_floor < Q.top().second);

      // all of Z must get same priority
      ahrsz_ext_priority_value<PSPACE> Z_p = compute_priority(Z_floor,Z_ceil);

      for(typename std::vector<vd_t>::const_iterator i(Z.begin());i!=Z.end();++i) {
	n2i[*i] = ahrsz_priority_value<PSPACE>(Z_p.base(),_pspace);
	typename T::out_edge_iterator j,jend;
	for(tie(j,jend) = out_edges(*i,*this);j!=jend;++j) {
	  vd_t y(target(*j,*this));
	  if(_inK[y] && --_indegree[y] == 0) {
	    Q.push(std::make_pair(y,compute_floor(y,n2i)));
	  }
	}
      }
    }
#endif

    // reset visited information
    for(typename std::vector<vd_t>::iterator i(K.begin());i!=K.end();++i) {
      _visited[*i]=false;
      _inK[*i]=false;
    }
  }

  void compute_ceiling(vd_t n,
#ifdef AHRSZ_USE_SIMPLE_REASSIGNMENT
		       std::vector<vd_t> &rto,
#endif
		       n2i_t n2i) {
    _visited[n]=true;
    typename T::out_edge_iterator i,iend;
    for(tie(i,iend) = out_edges(n,*this);i!=iend;++i) {
      vd_t j(target(*i,*this));

      if(_inK[j]) {
	// successor is in K
#ifdef AHRSZ_USE_SIMPLE_REASSIGNMENT
	if(!_visited[j]) { compute_ceiling(j,rto,n2i); }
#else
	if(!_visited[j]) { compute_ceiling(j,n2i); }
#endif
	_ceiling[n] = std::min(_ceiling[j],_ceiling[n]);
      } else {
	// successor is not in K
	_ceiling[n] = std::min(_ceiling[n],ahrsz_ext_priority_value<PSPACE>(n2i[j]));
      }
    }
#ifdef AHRSZ_USE_SIMPLE_REASSIGNMENT
    rto.push_back(n);
#endif
  }

  ahrsz_ext_priority_value<PSPACE>
  compute_priority(ahrsz_ext_priority_value<PSPACE> floor,
		   ahrsz_ext_priority_value<PSPACE> ceiling) {
    assert(floor < ceiling);
    ahrsz_ext_priority_value<PSPACE> candidate;

    if(floor.minus_inf()) {
      candidate = ahrsz_ext_priority_value<PSPACE>(_pspace.begin(),_pspace);
      if(candidate >= ceiling) {
	// we need a new priority
	_pspace.push_front();
	candidate = ahrsz_ext_priority_value<PSPACE>(_pspace.begin(),_pspace);
      }
    } else {
      assert(!floor.plus_inf());
      candidate = floor;
      ++candidate;
      if(candidate.base() == _pspace.end() || candidate >= ceiling) {
	// we need a new priority
	candidate = ahrsz_ext_priority_value<PSPACE>(_pspace.insert_after(floor.base()),_pspace);
      }
    }
    assert(floor < candidate && candidate < ceiling);
    return candidate;
  }

  ahrsz_ext_priority_value<PSPACE> compute_floor(vd_t v, n2i_t n2i) {
    ahrsz_ext_priority_value<PSPACE> floor(minus_infinity);

    typename T::in_edge_iterator j,jend;
    for(tie(j,jend) = in_edges(v,*this);j!=jend;++j) {
      floor = std::max(floor,ahrsz_ext_priority_value<PSPACE>(n2i[source(*j,*this)]));
    }

    return floor;
  }

private:
  void copy(ahrsz_online_topological_order const &src) {
    // invoke super assignment operator
    T::operator=(src);

    // direct copy
    _pspace=PSPACE(1);
    typename boost::property_map<T, N2I>::type _pmap = get(N2I(),*this);
    std::vector<typename T::vertex_descriptor> tmp;

    typename T::vertex_iterator i,iend;
    for(tie(i,iend) = vertices(*this);i!=iend;++i) {
      _pmap[*i] = ahrsz_priority_value<PSPACE>(_pspace.begin(),_pspace);
      tmp.push_back(*i);
    }

    reassignment(tmp);
  }
};

#endif


#include <boost/mpl/int.hpp>
#include <boost/simd/sdk/simd/native.hpp>
#include <boost/mpl/assert.hpp>
#include <boost/simd/sdk/memory/is_aligned.hpp>

using boost::mpl::int_;
using boost::simd::meta::is_aligned;

int main()
{
  BOOST_MPL_ASSERT(( is_aligned< int_<0>,int_<2> >::type ));
  BOOST_MPL_ASSERT(( is_aligned< int_<2>,int_<2> >::type ));
  BOOST_MPL_ASSERT(( is_aligned< int_<4>,int_<2> >::type ));
  BOOST_MPL_ASSERT(( is_aligned< int_<8>,int_<2> >::type ));
  BOOST_MPL_ASSERT_NOT(( is_aligned< int_<17>,int_<2> >::type ));
}


/*
  uniform_smooth_noise64.hh

  Copyright (c) 2018 Ryan P. Nicholl, "Exaeta", <exaeta@protonmail.com>

    https://github.com/Exaeta/det_noise64

    uniform_smooth_noise64 is a N-dimensional uniform smooth noise
    function, but I'm going to change the hash function into something
    faster/more well known in a later version (and/or allow templating
    the hash used). The next version or version with templated hash will
    be cross platform deterministic.

    It doesn't rely on any third party libraries except boost::multiprecision,
    which can be substituited for any library that provides a 128-bit
    unsigned integer.

    Any donations would be appreciated. :)

  Permission is hereby granted, free of charge, to any person obtaining a copy
  of this software and associated documentation files (the "Software"), to deal
  in the Software without restriction, including without limitation the rights
  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
  copies of the Software, and to permit persons to whom the Software is
  furnished to do so, subject to the following conditions:

  The above copyright notice and this permission notice shall be included in all
  copies or substantial portions of the Software.

  THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
  SOFTWARE.

*/

#ifndef RPNX_G_NOISE_HH
#define RPNX_G_NOISE_HH

#include <array>
#include <cinttypes>
#include <random>
#include <boost/multiprecision/cpp_int.hpp>

namespace rpnx
{

  using uint64_t = std::uint64_t;
  using uint128_t = boost::multiprecision::uint128_t;

  /*
    This function is probably not cryptographically secure.
    For insecure functions only.
  */
  template <std::size_t N>
  std::uint64_t det_point_noise64(std::array<std::uint64_t, N> inputs, std::size_t c = 5, std::uint64_t seed = 0)
  {
    std::uint64_t output = seed + 0xFAFAFAFAFAFAFAFAull;
    for (std::size_t k = 0; k < c; k++)
    {
      for (std::size_t i = 0; i < N; i++)
      {
        output = (output >> 17) | (output  << (64 - 17));
        output += inputs[i];
        output = (output >> ((k+inputs[i]) &0x0F)) | (output  << (64 - ((k+inputs[i]) &0x0F)));
        output ^= 7;
        output ^= ((output & (0xbeull << 31)) >> 21);
        output += 1;

      }
    }
    output = (output >> 17) | (output  << (64 - 17));

    return output;
  }

  template <std::size_t N>
  bool det_point_noise1(std::array<std::uint64_t, N> inputs)
  {

    std::uint64_t output = 0xFAFAFAFAFAFAFAFAull;
    for (std::size_t k = 0; k < 4; k++)
    {
      for (std::size_t i = 0; i < N; i++)
      {
        output = (output >> 17) | (output  << (64 - 17));
        output += inputs[i];
        output ^= 7;
        output ^= ((output & (0xbeull << 31)) >> 21);
        output += k;
      }
    }
    output = (output >> 19) | (output  << (64 - 19));

    return 1 & ( (output >> 7) ^ (output >> 3) ^ (output >> 5) ^
    (output >> 2) ^ (output >> 11) ^ (output >> 17) ^ (output >> 13) );
  }

  /** A deterministic pseudo-random N-dimensional smooth noise function,
      which, assuming the values of the input and hash function are
      uniformly distributed, provides uniformly distributed output.

      The algorithm used by this function might be changed in a future
      version of this library.

      @param C C is the template parameter that represents the number of
      bits used to interpolate between coordinates. A higher value of C
      will make the noise smooth over a larger range.

      @param N N is the number of elements in the array. Generally leave
      this to automatic deduction.

      @param inputs A std::array of N 64-bit integers

      @time O(N 2^N), O(1) as compiled

      @space O(2^N),  O(1) as compiled
  */
  template <std::size_t C = 32, std::size_t N = 1>
  std::uint64_t wave_noise64(std::array<std::uint64_t, N> inputs, std::uint64_t seed = 0)
  {
    using u64 = std::uint64_t;
    using u128 = uint128_t;
    using std::uint8_t;

    u64 out=0;

    std::array<uint64_t, (1 << N)> field_corners;
    // 2 ^ N dimensions
    // gathers the values in each corner.

    std::mt19937 rg;

    std::uint8_t r = 0; // used later;

    std::array<std::uint64_t, N> field_inputs = inputs;
    // Each dimension has a set of inputs

    for (std::size_t k = 0; k < field_inputs.size(); k++)
    {
      field_inputs[k] >>= C;

      // only the first N-C bits are significant to the field

    }

    for (std::size_t i = 0; i < field_corners.size(); i++)
    {

      std::array<std::uint64_t, N> field_inputs2 = field_inputs;

      for (std::size_t k = 0; k < N; k++)
      {
        if (i & (1 << k)) field_inputs2[k]++;
        // We must adjust the field inputs depending on which "corners"
        // we are in.
      }

      field_corners[i] = det_point_noise64(field_inputs2, 8, seed);
      // calculate the corner values

      r += field_corners[i] & 0xFF;
      // replace lost randomness with this value;

    }

   // std::cout << "intial corner[0]=" << field_corners[0] << std::endl;

    std::array<std::uint64_t, N> sigvals;
    std::array<bool, N> bvals;
    for (std::size_t i = 0; i < N; i++)
    {
      sigvals[i] = inputs[i] & ((std::uint64_t(1) << C) - 1);
      bvals[i] = 1 & inputs[i] >> C;
    }
    // We need to know the dimensional significance of dimension in
    // order to interpolate the values correctly.

    // The following code interpolates the N-dimensional structure
    // in N log N time by collapsing each dimension one at a time
    // until the structure is 0-dimensional and has only 1 value

    std::size_t q = N;
    // q here is to avoid taking logs later

    out = 0;

    for (size_t z = 0; z < N; z++)
    {
      u64 a = det_point_noise64(std::array<u64, 1>{field_inputs[z]}, 5, z+seed);
      u64 b = det_point_noise64(std::array<u64, 1>{field_inputs[z]+1}, 5, z+seed);

      u64 v1 = sigvals[z];
      u64 v2 = (u64(1) << C) - v1;

      //std::swap(a, b);
      if (bvals[z])
      {
        std::swap(a, b);
        std::swap(v1, v2);
      }

      u64 m1 = static_cast<u64>( ((u128(b) * v1) + (u128(a) * v2)) >> C );

      out += 0;
      out += m1;

    }

     if (out & (std::uint64_t(1) << 63))
    {
      out = ((~out ^ 1) << 1) | 1;
    }
    else out = out << 1;

    /* The transformation above is supposed to redistribute the "out"
    value such that values near 0 are "similar" so there aren't abrupt
    transitions due to modular arithmetic wrapping from 1 to 0.
    */

    out ^= 1 & (r ^ (r >> 3) ^ (r >> 5));
    /*
      An unfortunate result of the above transformation is that although
      it's uniform over the 64-bit space, the last bit only changes when
      the sign flips and thus effectively only 63 bits are adequately
      random. To fix that some "random" bits are used.
    */

    return out;

  }

    template <typename R>
  void wrap_correction(R & r, uint64_t & val)
  {
    if (val & (std::uint64_t(1) << 63))
    {
      val = ((~val ^ 1) << 1) | 1;
    }
    else val = val << 1;

    val ^= 1 & r();
  }

  template <std::size_t C = 32, std::size_t D = 2, std::size_t N = 1>
  std::uint64_t crystal_noise64(std::array<std::uint64_t, N> inputs)
  {
    using u64 = std::uint64_t;

    static_assert(64 - C - D >= 0 && 64 - C - D <= 63, "Invalid parameters");

    u64 out = 0;
    std::array<u64, N+1> a;

    for (int i = 0; i < N; i++)
    {
      a[i] = inputs[i];
    }
    a[N] = 0;

    std::array<u64, N> field_input = inputs;
    for (int i = 0; i < N; i++)
    {
      field_input[i] = inputs[i];
    }

    if (true) for (u64 i = 0; i < N; i++)
    {
      std::array<u64, N> b = inputs;

      u64 k = det_point_noise64(std::array<u64, N>{i}, 5, i);
      for (size_t i2 = 0; i2 < N; i2++)
      {
        b[i2] += k;
      }

      field_input[i] += (wave_noise64<C>(b)) >> (64 - C - D) ;

    }

    out = wave_noise64<C>(field_input);

    return out;
  }

  template <std::size_t C = 32, std::size_t D = 2, std::size_t N = 1>
  std::uint64_t bicrystal_noise64(std::array<std::uint64_t, N> inputs)
  {
    using u64 = std::uint64_t;

    static_assert(64 - C - D >= 0 && 64 - C - D <= 63, "Invalid parameters");

    std::array<u64, N+1> a;
    for (int i = 0; i < N; i++)
    {
      a[i] = inputs[i];
    }
    a[N] = 0;

    std::array<u64, N> field_input = inputs;
    for (int i = 0; i < N; i++)
    {
      field_input[i] = inputs[i];
    }

    if (true) for (u64 i = 0; i < N; i++)
    {
      std::array<u64, N> b = inputs;

      u64 k = det_point_noise64(std::array<u64, N>{i});
      for (size_t i2 = 0; i2 < N; i2++)
      {
        b[i2] += k;
      }

      field_input[i] += (crystal_noise64<C>(b)) >> (64 - C - D) ;

    }

    return wave_noise64<C>(field_input);
  }

  template <std::size_t C = 12, size_t N = 1>
  uint64_t weave_noise2d_64(std::array<uint64_t, N> inputs, bool correct = true, size_t d = 1)
  {
    std::mt19937_64 r{0};
    std::uint64_t output = 0;

    const constexpr std::size_t K = 3;
    const constexpr std::size_t k = (1 << K);

    std::array<uint64_t, N + 2> k_inputs;

    for (size_t i = 0; i < N; i++)
    {
      k_inputs[i] = inputs[i];
    }

    k_inputs[N] = r();
     k_inputs[N+1] = r();

    std::uint64_t q = (1 << (C));

    for (std::size_t i = 0; i < k; i++) for (std::size_t j = 0; j < k; j++)
    {

      std::array<std::uint64_t, N+2> y_inputs = k_inputs;
      y_inputs[N] = r();
      y_inputs[N+1] = r();
      y_inputs[0] += q*i;
      y_inputs[1] += q*j;

      uint64_t km = crystal_noise64<C+K, 1>(y_inputs);
      km = km/d;
      output += km;
    }

    if (correct) wrap_correction(r, output);

    return output;

  }

}

#endif


// Copyright John Maddock 2022.

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

#ifdef _MSC_VER
#define _SCL_SECURE_NO_WARNINGS
#endif
#define BOOST_CHRONO_HEADER_ONLY

#ifdef TEST_BIN_FLOAT
#include <boost/multiprecision/cpp_bin_float.hpp>
#endif
#ifdef TEST_DEC_FLOAT
#include <boost/multiprecision/cpp_dec_float.hpp>
#endif
#ifdef TEST_MPFR
#include <boost/multiprecision/mpfr.hpp>
#endif
#ifdef TEST_MPF
#include <boost/multiprecision/gmp.hpp>
#endif
#ifdef TEST_FLOAT128
#include <boost/multiprecision/float128.hpp>
#endif

#include <boost/random/mersenne_twister.hpp>
#include <boost/random/uniform_int.hpp>
#include <boost/chrono.hpp>
#include "test.hpp"
#include <boost/array.hpp>
#include <iostream>
#include <iomanip>

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

#if defined(TEST_MPF)
template <unsigned N, boost::multiprecision::expression_template_option ET>
bool has_bad_bankers_rounding(const boost::multiprecision::number<boost::multiprecision::gmp_float<N>, ET>&)
{
   return true;
}
#endif
template <class T>
bool has_bad_bankers_rounding(const T&)
{
   return false;
}

bool is_bankers_rounding_error(const std::string& s, const std::string& expect)
{
   // This check isn't foolproof: that would require *much* more sophisticated code!!!
   std::string::size_type l = expect.size();
   if (l != s.size())
      return false;
   std::string::size_type len = s.find('e');
   if (len == std::string::npos)
      len = l - 1;
   else
      --len;
   for (unsigned i = 0; i < len; ++i)
   {
      char c1 = s[i];
      char c2 = expect[i];
      if (c1 != c2)
         return false;
   }
   if (s[len] != expect[len] + 1)
      return false;
   return true;
}

template <class Clock>
struct stopwatch
{
   typedef typename Clock::duration duration;
   stopwatch()
   {
      m_start = Clock::now();
   }
   duration elapsed()
   {
      return Clock::now() - m_start;
   }
   void reset()
   {
      m_start = Clock::now();
   }

 private:
   typename Clock::time_point m_start;
};

template <class T>
struct exponent_type
{
   typedef int type;
};
template <class T, boost::multiprecision::expression_template_option ET>
struct exponent_type<boost::multiprecision::number<T, ET> >
{
   typedef typename T::exponent_type type;
};

template <class T>
T generate_random_float()
{
   BOOST_MATH_STD_USING
   typedef typename exponent_type<T>::type e_type;
   static boost::random::mt19937           gen;
   T                                       val      = gen();
   T                                       prev_val = -1;
   while (val != prev_val)
   {
      val *= (gen.max)();
      prev_val = val;
      val += gen();
   }
   e_type e;
   val = frexp(val, &e);

   static const int                                       max_exponent_value = (std::min)(static_cast<int>(std::numeric_limits<T>::max_exponent - std::numeric_limits<T>::digits - 20), 40);
   static boost::random::uniform_int_distribution<e_type> ui(0, max_exponent_value);
   return ldexp(val, ui(gen));
}

template <class Float>
void test_fixed_io()
{
   std::cout << "Testing type " << typeid(Float).name() << std::endl;
   std::cout << "digits = " << std::numeric_limits<Float>::digits << std::endl;
   std::cout << "digits10 = " << std::numeric_limits<Float>::digits10 << std::endl;
   std::cout << "max_digits10 = " << std::numeric_limits<Float>::max_digits10 << std::endl;

   stopwatch<boost::chrono::high_resolution_clock> w;

   int count = 0;

#ifndef CI_SUPPRESS_KNOWN_ISSUES
   while (boost::chrono::duration_cast<boost::chrono::duration<double> >(w.elapsed()).count() < 200)
#else
   while (boost::chrono::duration_cast<boost::chrono::duration<double> >(w.elapsed()).count() < 50)
#endif
   {
      double val = generate_random_float<double>();
      Float  f(val);
#ifdef BOOST_MSVC
      if (val > 0.5 && val < 1)
         val = 1;
#endif
      std::stringstream ss1, ss2;
      ss1 << std::fixed << std::setprecision(0) << val;
      ss2 << std::fixed << std::setprecision(0) << f;
      if (ss1.str() != ss2.str())
      {
         if (has_bad_bankers_rounding(Float()) && is_bankers_rounding_error(ss2.str(), ss1.str()))
         {
            std::cout << "Ignoring bankers-rounding error with GMP mp_f.\n";
         }
         else
         {
            std::cout << std::setprecision(20) << "Testing value " << val << std::endl;
            std::cout << "Got:      " << ss2.str() << std::endl;
            std::cout << "Expected: " << ss1.str() << std::endl;
            ++boost::detail::test_errors();
            Float(val).str(0, std::ios_base::fixed);
         }
      }
      ++count;
      if (boost::detail::test_errors() > 100)
         break;
   }

   std::cout << "Execution time = " << boost::chrono::duration_cast<boost::chrono::duration<double> >(w.elapsed()).count() << "s" << std::endl;
   std::cout << "Total values tested: " << count << std::endl;
}

int main()
{
#ifdef BOOST_MSVC && (BOOST_MSVC < 1920)
   std::cout << "MSVC prior to 14.2 does not perform bankers rounding for double IO, as a result all our test cases are incorrect, so there's nothing we can productively test here." << std::endl;
#else
   using namespace boost::multiprecision;
#ifdef TEST_BIN_FLOAT
   test_fixed_io<number<cpp_bin_float<113, digit_base_2, void, std::int16_t> > >();
#endif
#ifdef TEST_DEC_FLOAT
   test_fixed_io<cpp_dec_float_50>();
#endif
#ifdef TEST_MPFR
   test_fixed_io<boost::multiprecision::mpfr_float_50>();
#endif
#ifdef TEST_MPF
   test_fixed_io<boost::multiprecision::mpf_float_50>();
#endif
#ifdef TEST_FLOAT128
   test_fixed_io<boost::multiprecision::float128>();
#endif
#endif
   return boost::report_errors();
}


/*
 * MultiValueConstraint.cpp
 * @brief domain constraint
 * @date Mar 20, 2022
 * @author Yoonwoo Kim
 */

#include <gtsam/base/Testable.h>
#include <gtsam/discrete/DecisionTreeFactor.h>
#include <cpp/planning/MultiValueConstraint.h>
#include <cpp/planning/SingleValueConstraint.h>

#include <boost/make_shared.hpp>
using namespace gtsam;
using namespace std;

namespace gtsam_planner {

/* ************************************************************************* */
MultiValueConstraint::MultiValueConstraint(const DiscreteKeys& dkeys,
  const vector<size_t>& values) : DiscreteFactor(dkeys.indices()) {
  dkeys_ = dkeys;
  for (const DiscreteKey& dkey : dkeys) cardinalities_.insert(dkey);
  for (const size_t& value : values) values_.push_back(value);
}

/* ************************************************************************* */
void MultiValueConstraint::print(const string& s,
  const KeyFormatter& formatter) const {
  cout << s << "MultiValueConstraint on ";
  for (Key dkey : keys_) cout << formatter(dkey) << " ";
  cout << ::endl;
}

/* ************************************************************************* */
DiscreteKeys MultiValueConstraint::discreteKeys() const {
  return dkeys_;
}

/* ************************************************************************* */
double MultiValueConstraint::operator()(const DiscreteValues& values) const {
  for (size_t i=0; i < values_.size(); i++) {
    if (values.count(keys_[i]) != 0) {
      size_t value = values.at(keys_[i]);
      if (value != values_[i]) return 0.0;
    }
  }
  return 1.0;
}

/* ************************************************************************* */
DecisionTreeFactor MultiValueConstraint::toDecisionTreeFactor() const {
  DecisionTreeFactor converted;
  size_t nrKeys = keys_.size();
  for (size_t i = 0; i < nrKeys; i++) {
    SingleValueConstraint single(discreteKey(i), values_[i]);
    converted = converted * single.toDecisionTreeFactor();
  }
  return converted;
}

/* ************************************************************************* */
DecisionTreeFactor MultiValueConstraint::operator*(const DecisionTreeFactor& f) const {
  return toDecisionTreeFactor() * f;
}

/* ************************************************************************* */
}  // namespace gtsam


#include <boost/mpl/assert.hpp>
#include <nt2/sdk/meta/as_integer.hpp>
#include <nt2/sdk/simd/pack.hpp>
#include <boost/type_traits/is_same.hpp>

using boost::is_same;
using nt2::meta::as_integer;

using nt2::uint32_t;
using nt2::int32_t;
using nt2::simd::pack;

int main()
{
  BOOST_MPL_ASSERT(( is_same<int32_t,       as_integer<float>::type>            ));
  BOOST_MPL_ASSERT(( is_same<uint32_t,      as_integer<float, unsigned>::type>  ));
  BOOST_MPL_ASSERT(( is_same<int32_t,       as_integer<uint32_t, signed>::type> ));
  BOOST_MPL_ASSERT(( is_same<pack<int32_t>, as_integer< pack<float> >::type>    ));
}


//  Copyright (c) 2014 Hartmut Kaiser
//  Copyright (c) 2014 Patricia Grubel
//  Copyright (c) 2018 Adrian Serio
//
//  SPDX-License-Identifier: BSL-1.0
//  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)

// This is the fourth in a series of examples demonstrating the development of
// a fully distributed solver for a simple 1D heat distribution problem.
//
// This example builds on example three. It futurizes the code from that
// example. Compared to example two this code runs much more efficiently. It
// allows for changing the amount of work executed in one HPX thread which
// enables tuning the performance for the optimal grain size of the
// computation. This example is still fully local but demonstrates nice
// scalability on SMP machines.
//
// In this variation of stencil we use the save_checkpoint and
// revive_checkpint functions to back up the state of the application
// every n time steps.
//

#include <hpx/hpx.hpp>
#include <hpx/hpx_init.hpp>

#include <hpx/algorithm.hpp>
#include <hpx/modules/checkpoint.hpp>
#include <hpx/modules/serialization.hpp>

#include <boost/range/irange.hpp>

#include <cstddef>
#include <cstdint>
#include <fstream>
#include <iostream>
#include <memory>
#include <utility>
#include <vector>

#include "print_time_results.hpp"

///////////////////////////////////////////////////////////////////////////////
// Command-line variables
bool header = true;    // print csv heading
double k = 0.5;        // heat transfer coefficient
double dt = 1.;        // time step
double dx = 1.;        // grid spacing

inline std::size_t idx(std::size_t i, int dir, std::size_t size)
{
    if (i == 0 && dir == -1)
        return size - 1;
    if (i == size - 1 && dir == +1)
        return 0;

    HPX_ASSERT((i + dir) < size);

    return i + dir;
}

///////////////////////////////////////////////////////////////////////////////
// Our partition data type
struct partition_data
{
private:
    typedef hpx::serialization::serialize_buffer<double> buffer_type;

public:
    partition_data()
      : data_()
      , size_(0)
    {
    }

    partition_data(std::size_t size)
      : data_(std::allocator<double>().allocate(size), size, buffer_type::take)
      , size_(size)
    {
    }

    partition_data(std::size_t size, double initial_value)
      : data_(std::allocator<double>().allocate(size), size, buffer_type::take)
      , size_(size)
    {
        double base_value = double(initial_value * size);
        for (std::size_t i = 0; i != size; ++i)
            data_[i] = base_value + double(i);
    }

    partition_data(const partition_data& old_part)
      : data_(std::allocator<double>().allocate(old_part.size()),
            old_part.size(), buffer_type::take)
      , size_(old_part.size())
    {
        for (std::size_t i = 0; i < old_part.size(); i++)
        {
            data_[i] = old_part[i];
        }
    }

    double& operator[](std::size_t idx)
    {
        return data_[idx];
    }
    double operator[](std::size_t idx) const
    {
        return data_[idx];
    }

    std::size_t size() const
    {
        return size_;
    }

private:
    buffer_type data_;
    std::size_t size_;

    // Serialization Definitions
    friend class hpx::serialization::access;
    template <typename Volume>
    void serialize(Volume& vol, const unsigned int version)
    {
        vol& data_& size_;
    }
};

std::ostream& operator<<(std::ostream& os, partition_data const& c)
{
    os << "{";
    for (std::size_t i = 0; i != c.size(); ++i)
    {
        if (i != 0)
            os << ", ";
        os << c[i];
    }
    os << "}";
    return os;
}

///////////////////////////////////////////////////////////////////////////////
// Checkpoint Function

struct backup
{
    std::vector<hpx::util::checkpoint> bin;
    std::string file_name_;

    backup(std::string const& file_name, std::size_t np)
      : bin(np)
      , file_name_(file_name)
    {
    }
    backup(backup&& old)
      : bin(std::move(old.bin))
      , file_name_(std::move(old.file_name_))
    {
    }
    ~backup() {}

    void save(partition_data const& status, std::size_t index)
    {
        bin[index] = hpx::util::save_checkpoint(hpx::launch::sync, status);
    }

    void write()
    {
        hpx::util::checkpoint archive_data =
            hpx::util::save_checkpoint(hpx::launch::sync, bin);
        // Make sure file stream is bianary for Windows/Mac machines
        std::ofstream file_archive(
            file_name_, std::ios::binary | std::ios::out);
        if (file_archive.is_open())
        {
            file_archive << archive_data;
        }
        else
        {
            std::cout << "Error opening file!" << std::endl;
        }
        file_archive.close();
    }

    void revive(std::vector<std::vector<hpx::shared_future<partition_data>>>& U,
        std::size_t nx)
    {
        hpx::util::checkpoint temp_archive;
        // Make sure file stream is bianary for Windows/Mac machines
        std::ifstream ist(file_name_, std::ios::binary | std::ios::in);
        ist >> temp_archive;
        hpx::util::restore_checkpoint(temp_archive, bin);
        for (std::size_t i = 0; i < U[0].size(); i++)
        {
            partition_data temp(nx, double(i));
            hpx::util::restore_checkpoint(bin[i], temp);
            //Check
            for (std::size_t e = 0; e < temp.size(); e++)
            {
                std::cout << temp[e] << ", ";
            }
            std::cout << std::endl;
            U[0][i] = hpx::make_ready_future(temp);
        }
    }
};

void print(std::vector<std::vector<hpx::shared_future<partition_data>>> U)
{
    for (std::size_t out = 0; out < U[0].size(); out++)
    {
        partition_data print_buff(U[0][out].get());
        for (std::size_t inner = 0; inner < print_buff.size(); inner++)
        {
            std::cout << print_buff[inner] << ", ";
            if (inner % 9 == 0 && inner != 0)
                std::cout << std::endl;
        }
    }
    std::cout << std::endl;
}
void print_space(std::vector<hpx::shared_future<partition_data>> next)
{
    for (std::size_t out = 0; out < next.size(); out++)
    {
        partition_data print_buff(next[out].get());
        for (std::size_t inner = 0; inner < print_buff.size(); inner++)
        {
            std::cout << print_buff[inner] << ", ";
            if (inner % 9 == 0 && inner != 0)
                std::cout << std::endl;
        }
    }
    std::cout << std::endl;
}

///////////////////////////////////////////////////////////////////////////////
struct stepper
{
    // Our data for one time step
    typedef hpx::shared_future<partition_data> partition;
    typedef std::vector<partition> space;

    // Our operator
    static double heat(double left, double middle, double right)
    {
        return middle + (k * dt / (dx * dx)) * (left - 2 * middle + right);
    }

    // The partitioned operator, it invokes the heat operator above on all
    // elements of a partition.
    static partition_data heat_part(partition_data const& left,
        partition_data const& middle, partition_data const& right)
    {
        std::size_t size = middle.size();
        partition_data next(size);

        next[0] = heat(left[size - 1], middle[0], middle[1]);

        for (std::size_t i = 1; i != size - 1; ++i)
        {
            next[i] = heat(middle[i - 1], middle[i], middle[i + 1]);
        }

        next[size - 1] = heat(middle[size - 2], middle[size - 1], right[0]);

        return next;
    }

    // do all the work on 'np' partitions, 'nx' data points each, for 'nt'
    // time steps, limit depth of dependency tree to 'nd'
    hpx::future<space> do_work(std::size_t np, std::size_t nx, std::size_t nt,
        std::uint64_t nd, std::uint64_t cp, std::string rsf, std::string fn)
    {
        using hpx::dataflow;
        using hpx::util::unwrapping;

        // Set up Check-pointing
        std::size_t num_c = nt / cp;    // Number of checkpoints to be made
        std::cout << "Number of checkpoints to be made: " << num_c << std::endl;
        std::vector<std::string> v_file_names(num_c, fn);
        std::vector<backup> container;

        // Initialize checkpoint file names
        for (std::size_t i = 0; i < num_c; i++)
        {
            v_file_names[i] =
                v_file_names[i] + "_" + std::to_string((i + 1) * cp);
            container.push_back(backup(v_file_names[i], np));
        }

        // Container to wait on all held futures
        std::vector<hpx::future<void>> backup_complete;

        // U[t][i] is the state of position i at time t.
        std::vector<space> U(2);
        for (space& s : U)
            s.resize(np);

        // Initial conditions: f(0, i) = i
        std::size_t b = 0;
        auto range = boost::irange(b, np);
        using hpx::parallel::execution::par;
        hpx::ranges::for_each(par, range, [&U, nx](std::size_t i) {
            U[0][i] = hpx::make_ready_future(partition_data(nx, double(i)));
        });

        //Initialize from backup
        if (rsf != "")
        {
            backup restart(rsf, np);
            restart.revive(U, nx);
        }

        //Check
        std::cout << "Initialization Check" << std::endl;
        print(U);

        // limit depth of dependency tree
        hpx::lcos::local::sliding_semaphore sem(nd);

        auto Op = unwrapping(&stepper::heat_part);

        // Actual time step loop
        for (std::size_t t = 0; t != nt; ++t)
        {
            space const& current = U[t % 2];
            space& next = U[(t + 1) % 2];

            for (std::size_t i = 0; i != np; ++i)
            {
                next[i] =
                    dataflow(hpx::launch::async, Op, current[idx(i, -1, np)],
                        current[i], current[idx(i, +1, np)]);

                //Checkpoint
                if (t % cp == 0 && t != 0)
                {
                    next[i] =
                        next[i].then([&container, i, t, cp](partition&& p) {
                            partition_data value(p.get());
                            container[(t / cp) - 1].save(value, i);
                            partition f_value = hpx::make_ready_future(value);
                            return f_value;
                        });
                }
            }

            //Print Checkpoint to file
            if (t % cp == 0 && t != 0)
            {
                hpx::future<void> f_print = hpx::when_all(next).then(
                    [&container, t, cp](hpx::future<space>&& f_s) {
                        container[(t / cp) - 1].write();
                    });
                backup_complete.push_back(std::move(f_print));
            }

            //Check
            if (t % cp == 0 && t != 0)
            {
                std::cout << "Checkpoint Check:" << std::endl;
                print_space(next);
            }

            // every nd time steps, attach additional continuation which will
            // trigger the semaphore once computation has reached this point
            if ((t % nd) == 0)
            {
                next[0].then([&sem, t](partition&&) {
                    // inform semaphore about new lower limit
                    sem.signal(t);
                });
            }

            // suspend if the tree has become too deep, the continuation above
            // will resume this thread once the computation has caught up
            sem.wait(t);
        }

        // Wait on Checkpoint Printing
        hpx::wait_all(backup_complete);

        //Begin Test
        //Create a new test vector and resize it
        std::vector<space> Z(2);
        for (space& y : Z)
        {
            y.resize(np);
        }

        backup test(v_file_names[0], np);
        std::cout << std::endl;
        std::cout << "Revive Check:" << std::endl;
        test.revive(Z, nx);
        std::cout << std::endl;
        //End Test

        // Return the solution at time-step 'nt'.
        return hpx::when_all(U[nt % 2]);
    }
};

///////////////////////////////////////////////////////////////////////////////
int hpx_main(hpx::program_options::variables_map& vm)
{
    std::uint64_t np = vm["np"].as<std::uint64_t>();    // Number of partitions.
    std::uint64_t nx =
        vm["nx"].as<std::uint64_t>();    // Number of grid points.
    std::uint64_t nt = vm["nt"].as<std::uint64_t>();    // Number of steps.
    std::uint64_t nd =
        vm["nd"].as<std::uint64_t>();    // Max depth of dep tree.
    std::uint64_t cp =
        vm["cp"].as<std::uint64_t>();    // Num. steps to checkpoint
    std::string rsf = vm["restart-file"].as<std::string>();
    std::string fn = vm["output-file"].as<std::string>();

    if (vm.count("no-header"))
        header = false;

    // Create the stepper object
    stepper step;

    // Measure execution time.
    std::uint64_t t = hpx::util::high_resolution_clock::now();

    // Execute nt time steps on nx grid points and print the final solution.
    hpx::future<stepper::space> result =
        step.do_work(np, nx, nt, nd, cp, rsf, fn);

    stepper::space solution = result.get();
    hpx::wait_all(solution);

    std::uint64_t elapsed = hpx::util::high_resolution_clock::now() - t;

    // Print the final solution
    if (vm.count("results"))
    {
        for (std::size_t i = 0; i != np; ++i)
            std::cout << "U[" << i << "] = " << solution[i].get() << std::endl;
    }

    std::uint64_t const os_thread_count = hpx::get_os_thread_count();
    print_time_results(os_thread_count, elapsed, nx, np, nt, header);

    return hpx::finalize();
}

int main(int argc, char* argv[])
{
    using namespace hpx::program_options;

    // Configure application-specific options.
    options_description desc_commandline;

    desc_commandline.add_options()(
        "results", "print generated results (default: false)")("nx",
        value<std::uint64_t>()->default_value(10),
        "Local x dimension (of each partition)")("nt",
        value<std::uint64_t>()->default_value(45),
        "Number of time steps")("nd", value<std::uint64_t>()->default_value(10),
        "Number of time steps to allow the dependency tree to grow to")("np",
        value<std::uint64_t>()->default_value(10),
        "Number of partitions")("k", value<double>(&k)->default_value(0.5),
        "Heat transfer coefficient (default: 0.5)")("dt",
        value<double>(&dt)->default_value(1.0),
        "Timestep unit (default: 1.0[s])")(
        "dx", value<double>(&dx)->default_value(1.0), "Local x dimension")("cp",
        value<std::uint64_t>()->default_value(44),
        "Number of steps to checkpoint")(
        "no-header", "do not print out the csv header row")("restart-file",
        value<std::string>()->default_value(""),
        "Start application from restart file")("output-file",
        value<std::string>()->default_value("1d.archive"),
        "Base name of archive file");

    // Initialize and run HPX
    return hpx::init(desc_commandline, argc, argv);
}