#include "trajopt/collision_terms.hpp"
#include "trajopt/collision_checker.hpp"
#include "trajopt/rave_utils.hpp"
#include "trajopt/utils.hpp"
#include "sco/expr_vec_ops.hpp"
#include "sco/expr_ops.hpp"
#include "sco/sco_common.hpp"
#include <boost/foreach.hpp>
#include "utils/eigen_conversions.hpp"
#include "sco/modeling_utils.hpp"
#include "utils/stl_to_string.hpp"
#include "utils/logging.hpp"
#include <boost/functional/hash.hpp>
using namespace OpenRAVE;
using namespace sco;
using namespace util;
using namespace std;

namespace trajopt {

void CollisionsToDistances(const vector<Collision>& collisions, const Link2Int& m_link2ind,
    DblVec& dists) {
  // Note: this checking (that the links are in the list we care about) is probably unnecessary
  // since we're using LinksVsAll
  dists.clear();
  dists.reserve(collisions.size());
  BOOST_FOREACH(const Collision& col, collisions) {
    Link2Int::const_iterator itA = m_link2ind.find(col.linkA);
    Link2Int::const_iterator itB = m_link2ind.find(col.linkB);
    if (itA != m_link2ind.end() || itB != m_link2ind.end()) {
      dists.push_back(col.distance);
    }
  }
}

void CollisionsToDistanceExpressions(const vector<Collision>& collisions, Configuration& rad,
    const Link2Int& link2ind, const VarVector& vars, const DblVec& dofvals, vector<AffExpr>& exprs, bool isTimestep1) {

  exprs.clear();
  exprs.reserve(collisions.size());
  rad.SetDOFValues(dofvals); // since we'll be calculating jacobians
  BOOST_FOREACH(const Collision& col, collisions) {
    AffExpr dist(col.distance);
    Link2Int::const_iterator itA = link2ind.find(col.linkA);
    if (itA != link2ind.end()) {
      VectorXd dist_grad = toVector3d(col.normalB2A).transpose()*rad.PositionJacobian(itA->second, col.ptA);
      exprInc(dist, varDot(dist_grad, vars));
      exprInc(dist, -dist_grad.dot(toVectorXd(dofvals)));
    }
    Link2Int::const_iterator itB = link2ind.find(col.linkB);
    if (itB != link2ind.end()) {
      VectorXd dist_grad = -toVector3d(col.normalB2A).transpose()*rad.PositionJacobian(itB->second, (isTimestep1 && (col.cctype == CCType_Between)) ? col.ptB1 : col.ptB);
      exprInc(dist, varDot(dist_grad, vars));
      exprInc(dist, -dist_grad.dot(toVectorXd(dofvals)));
    }
    if (itA != link2ind.end() || itB != link2ind.end()) {
      exprs.push_back(dist);
    }
  }
  LOG_DEBUG("%ld distance expressions\n", exprs.size());
}

void CollisionsToDistanceExpressions(const vector<Collision>& collisions, Configuration& rad, const Link2Int& link2ind,
    const VarVector& vars0, const VarVector& vars1, const DblVec& vals0, const DblVec& vals1,
    vector<AffExpr>& exprs) {
  vector<AffExpr> exprs0, exprs1;
  CollisionsToDistanceExpressions(collisions, rad, link2ind, vars0, vals0, exprs0, false);
  CollisionsToDistanceExpressions(collisions, rad, link2ind, vars1, vals1, exprs1,true);

  exprs.resize(exprs0.size());
  for (int i=0; i < exprs0.size(); ++i) {
    exprScale(exprs0[i], (1-collisions[i].time));
    exprScale(exprs1[i], collisions[i].time);
    exprs[i] = AffExpr(0);
    exprInc(exprs[i], exprs0[i]);
    exprInc(exprs[i], exprs1[i]);
    cleanupAff(exprs[i]);
  }
}

inline size_t hash(const DblVec& x) {
  return boost::hash_range(x.begin(), x.end());
}

void CollisionEvaluator::GetCollisionsCached(const DblVec& x, vector<Collision>& collisions) {
  double key = hash(getDblVec(x, GetVars()));
  vector<Collision>* it = m_cache.get(key);
  if (it != NULL) {
    LOG_DEBUG("using cached collision check\n");
    collisions = *it;
  }
  else {
    LOG_DEBUG("not using cached collision check\n");
    CalcCollisions(x, collisions);
    m_cache.put(key, collisions);
  }
}

SingleTimestepCollisionEvaluator::SingleTimestepCollisionEvaluator(ConfigurationPtr rad, const VarVector& vars) :
  m_env(rad->GetEnv()),
  m_cc(CollisionChecker::GetOrCreate(*m_env)),
  m_rad(rad),
  m_vars(vars),
  m_link2ind(),
  m_links(),
  m_filterMask(-1) {
  vector<KinBody::LinkPtr> links;
  vector<int> inds;
  rad->GetAffectedLinks(m_links, true, inds);
  for (int i=0; i < m_links.size(); ++i) {
    m_link2ind[m_links[i].get()] = inds[i];
  }
  // TODO add argument
}

void SingleTimestepCollisionEvaluator::CalcCollisions(const DblVec& x, vector<Collision>& collisions) {
  DblVec dofvals = getDblVec(x, m_vars);
  m_rad->SetDOFValues(dofvals);
  m_cc->LinksVsAll(m_links, collisions, m_filterMask);
}

void SingleTimestepCollisionEvaluator::CalcDists(const DblVec& x, DblVec& dists) {
  vector<Collision> collisions;
  GetCollisionsCached(x, collisions);
  CollisionsToDistances(collisions, m_link2ind, dists);
}

void SingleTimestepCollisionEvaluator::CalcDistExpressions(const DblVec& x, vector<AffExpr>& exprs) {
  vector<Collision> collisions;
  GetCollisionsCached(x, collisions);
  DblVec dofvals = getDblVec(x, m_vars);
  CollisionsToDistanceExpressions(collisions, *m_rad, m_link2ind, m_vars, dofvals, exprs, false);
}

////////////////////////////////////////

CastCollisionEvaluator::CastCollisionEvaluator(ConfigurationPtr rad, const VarVector& vars0, const VarVector& vars1) :
  m_env(rad->GetEnv()),
  m_cc(CollisionChecker::GetOrCreate(*m_env)),
  m_rad(rad),
  m_vars0(vars0),
  m_vars1(vars1),
  m_link2ind(),
  m_links() {
  vector<KinBody::LinkPtr> links;
  vector<int> inds;
  rad->GetAffectedLinks(m_links, true, inds);
  for (int i=0; i < m_links.size(); ++i) {
    m_link2ind[m_links[i].get()] = inds[i];
  }
}

void CastCollisionEvaluator::CalcCollisions(const DblVec& x, vector<Collision>& collisions) {
  DblVec dofvals0 = getDblVec(x, m_vars0);
  DblVec dofvals1 = getDblVec(x, m_vars1);
  m_rad->SetDOFValues(dofvals0);
  m_cc->CastVsAll(*m_rad, m_links, dofvals0, dofvals1, collisions);
}
void CastCollisionEvaluator::CalcDistExpressions(const DblVec& x, vector<AffExpr>& exprs) {
  vector<Collision> collisions;
  GetCollisionsCached(x, collisions);
  DblVec dofvals0 = getDblVec(x, m_vars0);
  DblVec dofvals1 = getDblVec(x, m_vars1);
  CollisionsToDistanceExpressions(collisions, *m_rad, m_link2ind, m_vars0, m_vars1, dofvals0, dofvals1, exprs);
}
void CastCollisionEvaluator::CalcDists(const DblVec& x, DblVec& dists) {
  vector<Collision> collisions;
  GetCollisionsCached(x, collisions);
  CollisionsToDistances(collisions, m_link2ind, dists);
}

//////////////////////////////////////////

typedef OpenRAVE::RaveVector<float> RaveVectorf;

void PlotCollisions(const std::vector<Collision>& collisions, OR::EnvironmentBase& env, vector<OR::GraphHandlePtr>& handles, double safe_dist) {
  BOOST_FOREACH(const Collision& col, collisions) {
    RaveVectorf color;
    if (col.distance < 0) color = RaveVectorf(1,0,0,1);
    else if (col.distance < safe_dist) color = RaveVectorf(1,1,0,1);
    else color = RaveVectorf(0,1,0,1);
    if (col.cctype == CCType_Between) {
      handles.push_back(env.drawarrow(col.ptB, col.ptB1, .002, RaveVectorf(0,0,0,1)));
    }
    OR::Vector ptB = (col.cctype == CCType_Between)  ? ((1-col.time)* col.ptB +col.time*col.ptB1) : col.ptB;
    handles.push_back(env.drawarrow(col.ptA, ptB, .0025, color));
  }
}

CollisionCost::CollisionCost(double dist_pen, double coeff, ConfigurationPtr rad, const VarVector& vars) :
    Cost("collision"),
    m_calc(new SingleTimestepCollisionEvaluator(rad, vars)), m_dist_pen(dist_pen), m_coeff(coeff)
{}

CollisionCost::CollisionCost(double dist_pen, double coeff, ConfigurationPtr rad, const VarVector& vars0, const VarVector& vars1) :
    Cost("cast_collision"),
    m_calc(new CastCollisionEvaluator(rad, vars0, vars1)), m_dist_pen(dist_pen), m_coeff(coeff)
{}
ConvexObjectivePtr CollisionCost::convex(const vector<double>& x, Model* model) {
  ConvexObjectivePtr out(new ConvexObjective(model));
  vector<AffExpr> exprs;
  m_calc->CalcDistExpressions(x, exprs);
  for (int i=0; i < exprs.size(); ++i) {
    AffExpr viol = exprSub(AffExpr(m_dist_pen), exprs[i]);
    out->addHinge(viol, m_coeff);
  }
  return out;
}
double CollisionCost::value(const vector<double>& x) {
  DblVec dists;
  m_calc->CalcDists(x, dists);
  double out = 0;
  for (int i=0; i < dists.size(); ++i) {
    out += pospart(m_dist_pen - dists[i]) * m_coeff;
  }
  return out;
}

void CollisionCost::Plot(const DblVec& x, OR::EnvironmentBase& env, std::vector<OR::GraphHandlePtr>& handles) {
  vector<Collision> collisions;
  m_calc->GetCollisionsCached(x, collisions);
  PlotCollisions(collisions, env, handles, m_dist_pen);
}

// ALMOST EXACTLY COPIED FROM CollisionCost

CollisionConstraint::CollisionConstraint(double dist_pen, double coeff, ConfigurationPtr rad, const VarVector& vars) :
    m_calc(new SingleTimestepCollisionEvaluator(rad, vars)), m_dist_pen(dist_pen), m_coeff(coeff)
{
  name_="collision";
}

CollisionConstraint::CollisionConstraint(double dist_pen, double coeff, ConfigurationPtr rad, const VarVector& vars0, const VarVector& vars1) :
    m_calc(new CastCollisionEvaluator(rad, vars0, vars1)), m_dist_pen(dist_pen), m_coeff(coeff)
{
  name_="collision";
}
ConvexConstraintsPtr CollisionConstraint::convex(const vector<double>& x, Model* model) {
  ConvexConstraintsPtr out(new ConvexConstraints(model));
  vector<AffExpr> exprs;
  m_calc->CalcDistExpressions(x, exprs);
  for (int i=0; i < exprs.size(); ++i) {
    AffExpr viol = exprSub(AffExpr(m_dist_pen), exprs[i]);
    out->addIneqCnt(exprMult(viol,m_coeff));
  }
  return out;
}
DblVec CollisionConstraint::value(const vector<double>& x) {
  DblVec dists;
  m_calc->CalcDists(x, dists);
  DblVec out(dists.size());
  for (int i=0; i < dists.size(); ++i) {
    out[i] = pospart(m_dist_pen - dists[i]) * m_coeff;
  }
  return out;
}

}


// Edited by:
//  @authors:     Ahmad Humayun
//  @contact:     ahumayun@cc.gatech.edu
//  @affiliation: Georgia Institute of Technology
//  @date:        Fall 2013 - Summer 2014

/* maxflow.cpp */

#ifndef __MAXFLOWIMPL_HPP__
#define __MAXFLOWIMPL_HPP__

#include <stdio.h>
#include <iostream>
#include <boost/format.hpp>

#include "graph.h"

using boost::format;

/*
	special constants for node->parent. Duplicated in graph.cpp, both should match!
*/
#define TERMINAL ( (arc *) 1 )		/* to terminal */
#define ORPHAN   ( (arc *) 2 )		/* orphan */

#define INFINITE_D ((int)(((unsigned)-1)/2))		/* infinite distance to the terminal */

/*
	Returns the next active node.
	If it is connected to the sink, it stays in the list,
	otherwise it is removed from the list
*/
template <typename captype, typename tcaptype, typename flowtype>
	inline typename Graph<captype,tcaptype,flowtype>::node* Graph<captype,tcaptype,flowtype>::next_active()
{
	node *i;

	while ( 1 )
	{
	  // if the queue 0 is empty, swap with queue 1
		if (!(i=queue_first[0]))
		{
			queue_first[0] = i = queue_first[1];
			queue_last[0]  = queue_last[1];
			queue_first[1] = NULL;
			queue_last[1]  = NULL;
			if (!i) return NULL;
		}

		/* remove it from the active list */
		if (i->next == i) queue_first[0] = queue_last[0] = NULL;
		else              queue_first[0] = i -> next;
		i -> next = NULL;

		/* a node in the list is active iff it has a parent */
		if (i->parent) return i;
	}
}

/***********************************************************************/

template <typename captype, typename tcaptype, typename flowtype>
	inline void Graph<captype,tcaptype,flowtype>::set_orphan_front(node *i)
{
	nodeptr *np;
	i -> parent = ORPHAN;
	//i -> src_origin_idx = INVALID_SRC_ID;
	np = nodeptr_block -> New();
	np -> ptr = i;
	np -> next = orphan_first;
	orphan_first = np;
}

template <typename captype, typename tcaptype, typename flowtype>
	inline void Graph<captype,tcaptype,flowtype>::set_orphan_rear(node *i)
{
	nodeptr *np;
	i -> parent = ORPHAN;
	//i -> src_origin_idx = INVALID_SRC_ID;
	np = nodeptr_block -> New();
	np -> ptr = i;
	if (orphan_last) orphan_last -> next = np;
	else             orphan_first        = np;
	orphan_last = np;
	np -> next = NULL;
}

/***********************************************************************/

template <typename captype, typename tcaptype, typename flowtype>
	inline void Graph<captype,tcaptype,flowtype>::add_to_changed_list(node *i)
{
	if (changed_list && !i->is_in_changed_list)
	{
		node_id* ptr = changed_list->New();
		*ptr = (node_id)(i - nodes);
		i->is_in_changed_list = true;
	}
}

/************************************************process_sink_orphan***********************/

template <typename captype, typename tcaptype, typename flowtype>
	void Graph<captype,tcaptype,flowtype>::maxflow_init()
{
	node *i;

	queue_first[0] = queue_last[0] = NULL;
	queue_first[1] = queue_last[1] = NULL;
	orphan_first = NULL;

	TIME = 0;

	for (i=nodes; i<node_last; i++)
	{
		i -> next = NULL;
		i -> is_marked = 0;
		i -> is_in_changed_list = 0;
		i -> TS = TIME;
		if (i->tr_cap > 0)
		{
			/* i is connected to the source */
			i -> is_sink = 0;
			i -> parent = TERMINAL;
			set_active(i);
			i -> DIST = 1;
			if (i -> src_origin_idx == INVALID_SRC_ID)
			  i -> src_origin_idx = 0;
		}
		else if (i->tr_cap < 0)
		{
			/* i is connected to the sink */
			i -> is_sink = 1;
			i -> parent = TERMINAL;
			set_active(i);
			i -> DIST = 1;
			i -> src_origin_idx = INVALID_SRC_ID;
		}
		else
		{
			/* i is free */
			i -> parent = NULL;
			i -> src_origin_idx = INVALID_SRC_ID;
		}
	}
}

template <typename captype, typename tcaptype, typename flowtype>
	void Graph<captype,tcaptype,flowtype>::maxflow_reuse_trees_init()
{
	node* i;
	node* j;
	// the queue would be checked from the second list
	node* queue = queue_first[1];
	arc* a;
	nodeptr* np;

	// vacate all queues because they will rebuilt
	queue_first[0] = queue_last[0] = NULL;
	queue_first[1] = queue_last[1] = NULL;
	orphan_first = orphan_last = NULL;

	TIME ++;

	// iterate over the whole active list (coming from the second list)
	while ((i=queue))
	{
	  // de-queue the first node
		queue = i->next;
		// if node was last in queue, set queue to NULL
		if (queue == i) queue = NULL;
		// disconnect the current node from the queue
		i->next = NULL;
		i->is_marked = 0;
		// add node to the second queue
		set_active(i);

		// in case node has no residual capacity to src/sink
		if (i->tr_cap == 0)
		{
		  // if the node has a parent, then set the node to orphan
		  // - this is a simple thing to do because you don't need to go up on the
		  //   tree to see if the root still belongs to the same src/sink this node
		  //   belongs to
			if (i->parent) set_orphan_rear(i);
			continue;
		}

		if (i->tr_cap > 0)
		{
	    // if node could be connected to src

		  // if node has no parent (neither is terminal or orphan) OR is is in the
		  // sink side of the cut
			if (!i->parent || i->is_sink)
			{
			  // assign node to src (because have residual cap to src)
				i->is_sink = 0;
				if (curr_src_origin_idx != INVALID_SRC_ID)
				  i->src_origin_idx = curr_src_origin_idx;
				// iterate over all the edges connected to this node
				for (a=i->first; a; a=a->next)
				{
					j = a->head;
					// if not already marked (only marked by mark_node() - used to
					// indicate which parts of the tree have changed)
					if (!j->is_marked)
					{
					  // if neighboring node j is child of i, with edge going to i
					  //  (as a sink tree), then set to orphan
						if (j->parent == a->sister) set_orphan_rear(j);
						// if neighboring node is in sink and there is residual capacity to
						//  push flow, then set to active
						if (j->parent && j->is_sink && a->r_cap > 0) set_active(j);
					}
				}
				add_to_changed_list(i);
			}
		}
		else
		{
		  // if node could be connected to sink

      // if node has no parent (neither is terminal or orphan) OR is in the
      // src side of the cut
			if (!i->parent || !i->is_sink)
			{
			  // assign node to sink (because have residual cap to sink)
				i->is_sink = 1;
				i->src_origin_idx = INVALID_SRC_ID;
				// iterate over all the edges connected to this node
				for (a=i->first; a; a=a->next)
				{
					j = a->head;
					// if not already marked
					if (!j->is_marked)
					{
					  // if neighboring node j is child of i, with edge going to i
					  //  (as a sink tree), then set to orphan
						if (j->parent == a->sister) set_orphan_rear(j);
						// if neighboring node is in src and there is residual capacity to
            //  push flow, then set to active
						if (j->parent && !j->is_sink && a->sister->r_cap > 0) set_active(j);
					}
				}
				add_to_changed_list(i);
			}
		}
		// if set to src/sink - directly connect to terminal
		i -> parent = TERMINAL;
		i -> TS = TIME;
		i -> DIST = 1;
	}

	//test_consistency();

	/* adoption */
	// iterate over all orphans and try to adopt them into trees
	while ((np=orphan_first))
	{
		orphan_first = np -> next;
		i = np -> ptr;
		nodeptr_block -> Delete(np);
		if (!orphan_first) orphan_last = NULL;
		if (i->is_sink) process_sink_orphan(i);
		else            process_source_orphan(i);
	}
	/* adoption end */

	//test_consistency();
}

template <typename captype, typename tcaptype, typename flowtype>
	void Graph<captype,tcaptype,flowtype>::augment(arc *middle_arc)
{
	node *i;
	arc *a;
	tcaptype bottleneck;

	/* 1. Finding bottleneck capacity */
	/* 1a - the source tree */
	bottleneck = middle_arc -> r_cap;
	for (i=middle_arc->sister->head; ; i=a->head)
	{
		a = i -> parent;
		if (a == TERMINAL) break;
		if (bottleneck > a->sister->r_cap) bottleneck = a -> sister -> r_cap;
		//gassert(i->src_origin_idx != INVALID_SRC_ID, "src_origin_idx needs to be src");

		//if (i->parent->head->parent != TERMINAL && i->parent->head->parent != ORPHAN && i->parent->head->parent != NULL) {
		//  gassert(i->parent->head->parent->head != i, "circular");
		//}
	}
	if (bottleneck > i->tr_cap) bottleneck = i -> tr_cap;
	/* 1b - the sink tree */
	for (i=middle_arc->head; ; i=a->head)
	{
		a = i -> parent;
		if (a == TERMINAL) break;
		if (bottleneck > a->r_cap) bottleneck = a -> r_cap;

		//gassert(i->src_origin_idx == INVALID_SRC_ID, "src_origin_idx needs to be sink");
	}
	if (bottleneck > - i->tr_cap) bottleneck = - i -> tr_cap;
	//gassert(bottleneck > 0, "bottleneck should be positive");

	/* 2. Augmenting */
	/* 2a - the source tree */
	middle_arc -> sister -> r_cap += bottleneck;
	middle_arc -> r_cap -= bottleneck;
	for (i=middle_arc->sister->head; ; i=a->head)
	{
		a = i -> parent;
		if (a == TERMINAL) break;
		a -> r_cap += bottleneck;
		a -> sister -> r_cap -= bottleneck;
		if (!a->sister->r_cap)
		{
			set_orphan_front(i); // add i to the beginning of the adoption list
		}
	}
	i -> tr_cap -= bottleneck;
	if (!i->tr_cap)
	{
		set_orphan_front(i); // add i to the beginning of the adoption list
	}
	/* 2b - the sink tree */
	for (i=middle_arc->head; ; i=a->head)
	{
		a = i -> parent;
		if (a == TERMINAL) break;
		a -> sister -> r_cap += bottleneck;
		a -> r_cap -= bottleneck;
		if (!a->r_cap)
		{
			set_orphan_front(i); // add i to the beginning of the adoption list
		}
	}
	i -> tr_cap += bottleneck;
	if (!i->tr_cap)
	{
		set_orphan_front(i); // add i to the beginning of the adoption list
	}

	++num_augmentations;

	flow += bottleneck;
}

/***********************************************************************/

template <typename captype, typename tcaptype, typename flowtype>
	void Graph<captype,tcaptype,flowtype>::process_source_orphan(node *i)
{
	node *j;
	arc *a0, *a0_min = NULL, *a;
	int d, d_min = INFINITE_D;

	if (i->tr_cap > 0) {
	  // if there is direct residual capacity to the src
	  a0_min = TERMINAL;
	  d_min = 0;
	} else {
	  // set node free if it has some children (i.e. don't attempt to adopt)
	  bool has_children = false;
    for (a0=i->first; a0; a0=a0->next) {
      j = a0 -> head;
      if (j->parent && j->parent != TERMINAL && j->parent != ORPHAN) {
        if (j->parent->head == i) {
          has_children = true;
          break;
        }
      }
    }

    /* trying to find a new parent */
    for (a0=i->first; !has_children && a0; a0=a0->next) {
      // if the reverse arc has residual capacity, it can possibly adopt
      if (a0->sister->r_cap)
      {
        // j now is the node the arc points to
        j = a0 -> head;
        // if j is in src
        if (!j->is_sink && (a=j->parent) &&
            j->src_origin_idx == i->src_origin_idx)
        {
          /*
          // if the arc points to something that is a child
          if (j->parent != TERMINAL && j->parent != ORPHAN) {
            if (j->parent->head == i)
              break;
          }*/
          // checking the origin of j
          d = 0;
          while ( 1 )
          {
            // if the timing is up to date, find the distance terminal
            if (j->TS == TIME)
            {
              d += j -> DIST;
              break;
            }
            // if the time stamp was old, then keep moving up on the tree
            a = j -> parent;
            d ++;
            if (a==TERMINAL)
            {
              j -> TS = TIME;
              j -> DIST = 1;
              break;
            }
            if (a==ORPHAN) { d = INFINITE_D; break; }
            j = a -> head;
          }
          if (d<INFINITE_D) // if j is on a path at some distance from src
          {
            if (d<d_min)
            {
              a0_min = a0;
              d_min = d;
            }
            /* set marks along the path */
            for (j=a0->head; j->TS!=TIME; j=j->parent->head)
            {
              j -> TS = TIME;
              j -> DIST = d --;
            }
          }
        }
      }
    }
	}

	if (i->parent = a0_min)
	{
		i -> TS = TIME;
		i -> DIST = d_min + 1;

		++num_adoptions;
	}
	else
	{
	  i -> src_origin_idx = INVALID_SRC_ID;

		/* no parent is found */
		add_to_changed_list(i);

		/* make sure now its source id is set to default null value */
		i->src_origin_idx = INVALID_SRC_ID;

		/* process neighbors */
		for (a0=i->first; a0; a0=a0->next)
		{
			j = a0 -> head;
			if (!j->is_sink && (a=j->parent))
			{
				if (a0->sister->r_cap) set_active(j);
				if (a!=TERMINAL && a!=ORPHAN && a->head==i)
				{
					set_orphan_rear(j); // add j to the end of the adoption list
				}
			}
		}
	}
}

template <typename captype, typename tcaptype, typename flowtype>
	void Graph<captype,tcaptype,flowtype>::process_sink_orphan(node *i)
{
	node *j;
	arc *a0, *a0_min = NULL, *a;
	int d, d_min = INFINITE_D;

  if (i->tr_cap < 0) {
    // if there is direct residual capacity to the sink
    a0_min = TERMINAL;
    d_min = 0;
  } else {
    // set node free if it has some children (i.e. don't attempt to adopt)
    bool has_children = false;
    for (a0=i->first; a0; a0=a0->next) {
      j = a0 -> head;
      if (j->parent && j->parent != TERMINAL && j->parent != ORPHAN) {
        if (j->parent->head == i) {
          has_children = true;
          break;
        }
      }
    }

    /* trying to find a new parent */
    for (a0=i->first; !has_children && a0; a0=a0->next) {
      // if the arc has residual capacity, it can possibly adopt
      if (a0->r_cap)
      {
        // j now is the node the arc points to
        j = a0 -> head;
        if (j->is_sink && (a=j->parent))
        {
          /*
          // if the arc points to something that is a child
          if (j->parent != TERMINAL && j->parent != ORPHAN) {
            if (j->parent->head == i)
              break;
          }*/
          /* checking the origin of j */
          d = 0;
          while ( 1 )
          {
            // if the timing is up to date, find the distance terminal
            if (j->TS == TIME)
            {
              d += j -> DIST;
              break;
            }
            // if the time stamp was old, then keep moving up on the tree
            a = j -> parent;
            d ++;
            if (a==TERMINAL)
            {
              j -> TS = TIME;
              j -> DIST = 1;
              break;
            }
            if (a==ORPHAN) { d = INFINITE_D; break; }
            j = a -> head;
          }
          if (d<INFINITE_D) // if j is on a path at some distance from sink
          {
            if (d<d_min)
            {
              a0_min = a0;
              d_min = d;
            }
            /* set marks along the path */
            for (j=a0->head; j->TS!=TIME; j=j->parent->head)
            {
              j -> TS = TIME;
              j -> DIST = d --;
            }
          }
        }
      }
    }
	}

	if (i->parent = a0_min)
	{
		i -> TS = TIME;
		i -> DIST = d_min + 1;

		++num_adoptions;
	}
	else
	{
	  i -> src_origin_idx = INVALID_SRC_ID;

		/* no parent is found */
		add_to_changed_list(i);

		/* process neighbors */
		for (a0=i->first; a0; a0=a0->next)
		{
			j = a0 -> head;
			if (j->is_sink && (a=j->parent))
			{
				if (a0->r_cap) set_active(j);
				if (a!=TERMINAL && a!=ORPHAN && a->head==i)
				{
					set_orphan_rear(j); // add j to the end of the adoption list
				}
			}
		}
	}
}

/***********************************************************************/

template <typename captype, typename tcaptype, typename flowtype>
	flowtype Graph<captype,tcaptype,flowtype>::maxflow(bool reuse_trees, Block<node_id>* _changed_list, bool run_init_func)
{
	node *i, *j, *current_node = NULL;
	arc *a;
	nodeptr *np, *np_next;

	if (!nodeptr_block)
	{
		nodeptr_block = new DBlock<nodeptr>(NODEPTR_BLOCK_SIZE, error_function);
	}

	// initialize the counters
	num_growths = 0;
	num_augmentations = 0;
	num_adoptions = 0;

	changed_list = _changed_list;
	if (maxflow_iteration == 0 && reuse_trees) { if (error_function) (*error_function)("reuse_trees cannot be used in the first call to maxflow()!"); exit(1); }
	if (changed_list && !reuse_trees) { if (error_function) (*error_function)("changed_list cannot be used without reuse_trees!"); exit(1); }

	if (run_init_func) {
	    if (reuse_trees) maxflow_reuse_trees_init();
	    else             maxflow_init();
	}

	// main loop
	while ( 1 )
	{
		// test_consistency(current_node);

		if ((i=current_node))
		{
			i -> next = NULL; /* remove active flag */
			if (!i->parent) i = NULL;
		}
		if (!i)
		{
		  /* get the next active node (and remove from the active list)
		   * - when no active nodes remaining, the algorithm ends
		   */
			if (!(i = next_active())) break;
		}

		/* growth */
		if (!i->is_sink)
		{
			/* grow source tree */
		  /* iterate over all the edges coming out of this active node */
			for (a=i->first; a; a=a->next) {
			  /* if the edge has a residual capacity, it can potentially become part of the S tree */
        if (a->r_cap)
        {
          j = a -> head;
          /* in case the terminus node of the arc is free, add to S tree (and
           * set newly added node to active) */
          if (!j->parent)
          {
            j -> is_sink = 0;
            j -> parent = a -> sister;
            j -> TS = i -> TS;
            j -> DIST = i -> DIST + 1;
            j -> src_origin_idx = i ->src_origin_idx;
            set_active(j);
            add_to_changed_list(j);
            ++num_growths;
          }
          /* if arc connects S tree to T tree, an augmenting path has been found */
          else if (j->is_sink)
            break;
          else if (j->TS <= i->TS &&
                   j->DIST > i->DIST &&
                   j -> src_origin_idx == i -> src_origin_idx)
          {
            /* heuristic - trying to make the distance from j to the source shorter */
            j -> parent = a -> sister;
            j -> TS = i -> TS;
            j -> DIST = i -> DIST + 1;
          }
        }
			}
		}
		else
		{
			/* grow sink tree */
		  /* iterate over all the edges coming out of this active node */
			for (a=i->first; a; a=a->next) {
			  /* if the reverse edge has a residual capacity, it can potentially become part of the T tree */
        if (a->sister->r_cap)
        {
          j = a -> head;
          /* in case the terminus node of the arc is free, add to T tree (and
           * set newly added node to active) */
          if (!j->parent)
          {
            j -> is_sink = 1;
            j -> parent = a -> sister;
            j -> TS = i -> TS;
            j -> DIST = i -> DIST + 1;
            set_active(j);
            add_to_changed_list(j);
            ++num_growths;
          }
          /* if arc connects T tree to S tree, an augmenting path has been found */
          else if (!j->is_sink) {
            /* set arc to the reverse edge (which we checked has residual capacity) */
            a = a -> sister;
            break;
          }
          else if (j->TS <= i->TS &&
                   j->DIST > i->DIST)
          {
            /* heuristic - trying to make the distance from j to the sink shorter */
            j -> parent = a -> sister;
            j -> TS = i -> TS;
            j -> DIST = i -> DIST + 1;
          }
        }
			}
		}

		TIME ++;

		/* if we found an augmenting path */
		if (a)
		{
			i -> next = i; /* set active flag */
			current_node = i;

			/* augmentation */
			augment(a);
			/* augmentation end */

			/* adoption - adopt all orphans or set them free */
			while ((np=orphan_first))
			{
				np_next = np -> next;
				np -> next = NULL;

				while ((np=orphan_first))
				{
					orphan_first = np -> next;
					i = np -> ptr;
					nodeptr_block -> Delete(np);
					if (!orphan_first) orphan_last = NULL;
					if (i->is_sink) process_sink_orphan(i);
					else            process_source_orphan(i);
				}

				orphan_first = np_next;
			}
			/* adoption end */
		}
		else current_node = NULL;
	}

	// test_consistency();

	if (!reuse_trees || (maxflow_iteration % 64) == 0)
	{
		delete nodeptr_block;
		nodeptr_block = NULL;
	}

	maxflow_iteration ++;
	return flow;
}

/***********************************************************************/

template <typename captype, typename tcaptype, typename flowtype>
	void Graph<captype,tcaptype,flowtype>::test_consistency(node* current_node) const
{
  std::cout << "Checking tree consistency" << std::endl;

	node *i;
	arc *a;
	int r;
	int num1 = 0, num2 = 0;

	// test whether all nodes i with i->next!=NULL are indeed in the queue
	for (i=nodes; i<node_last; i++)
	{
		if (i->next || i==current_node) num1 ++;
	}
	for (r=0; r<3; r++)
	{
		i = (r == 2) ? current_node : queue_first[r];
		if (i)
		for ( ; ; i=i->next)
		{
			num2 ++;
			if (i->next == i)
			{
			  // check if the last node is also indicated by queue_last
				if (r<2) gassert(i == queue_last[r], "i != queue_last[r]");
				else gassert(i == current_node, "i != current_node");
				break;
			}
		}
	}
	gassert(num1 == num2, "num1 != num2");

	for (i=nodes; i<node_last; i++)
	{
    node_id nid = (node_id)(i - nodes);

		// test whether all edges in search trees are non-saturated
    if (i->parent == NULL) {
      // if node is free
		  gassert(i->tr_cap == 0,
		          (format("free node %d cannot have any tr cap") % nid).str());
		  gassert(i->src_origin_idx == INVALID_SRC_ID,
		          (format("free node %d cannot have src id") % nid).str());
		}
		else if (i->parent == ORPHAN) {}
		else if (i->parent == TERMINAL)
		{
		  // if node is terminal, it should have appropriate residual
		  if (!i->is_sink) {
			  gassert(i->tr_cap > 0,
			          (format("src node %d has <= 0 tr cap") % nid).str());
			  gassert(i->src_origin_idx != INVALID_SRC_ID,
			          (format("src node %d INVALID_SRC_ID") % nid).str());
		  } else {
			  gassert(i->tr_cap < 0,
			          (format("sink node %d has >= 0 tr cap") % nid).str());
        gassert(i->src_origin_idx == INVALID_SRC_ID,
                (format("sink node %d not INVALID_SRC_ID") % nid).str());
		  }
		}
		else
		{
		  node_id pid = (node_id)(i - nodes);

		  // if the node has a parent
			if (!i->is_sink) {
        gassert(i->tr_cap >= 0,
                (format("src node %d has < 0 tr cap") % nid).str());
        gassert(i->parent->head->src_origin_idx == i->src_origin_idx,
                (format("src node %d doesn't have same src id as parent %d (%d %d)") % nid % (node_id)(i->parent->head-nodes) % i->src_origin_idx % i->parent->head->src_origin_idx).str());
			  gassert(i->parent->sister->r_cap > 0,
			          (format("src node %d connected to the parent %d with <= 0 residual cap arc") % nid % pid).str());
			} else {
			  gassert(i->tr_cap <= 0,
                (format("sink node %d has > 0 tr cap") % nid).str());
        gassert(i->src_origin_idx == INVALID_SRC_ID,
                (format("sink node %d doesn't have INVALID_SRC_ID") % nid).str());
			  gassert(i->parent->r_cap > 0,
			          (format("sink node %d connected to the parent %d with <= 0 residual cap arc") % nid % pid).str());
			}
		}

		// test whether passive nodes in search trees have neighbors in
		// a different tree through non-saturated edges
		if (i->parent && !i->next)
		{
			if (!i->is_sink)
			{
				for (a=i->first; a; a=a->next)
				{
					if (a->r_cap > 0) {
					  gassert(a->head->parent != NULL && a->head->parent != ORPHAN,
					          "src passive node cannot have a remaining residual "
					          "connection to a null or an orphan node");
            gassert(!a->head->is_sink, "src passive node cannot have a "
                    "remaining residual connection to a sink node");
					}
				}
			}
			else
			{
				for (a=i->first; a; a=a->next)
				{
					if (a->sister->r_cap > 0) {
					  gassert(a->head->parent != NULL && a->head->parent != ORPHAN,
					          "sink passive node cannot have a remaining residual "
					          "connection to a null or an orphan node");
            gassert(a->head->is_sink, "sink passive node cannot have a "
                    "remaining residual connection to a src node");
					}
				}
			}
		}

		// test marking invariants
		if (i->parent && i->parent!=ORPHAN && i->parent!=TERMINAL)
		{
		  gassert(i->TS <= i->parent->head->TS, "i->TS > i->parent->head->TS");

			if (i->TS == i->parent->head->TS)
			  gassert(i->DIST > i->parent->head->DIST,
			          (format("%d->DIST <= (PARENT)%d->DIST")
			              % (node_id)(i-nodes) % (node_id)(i->parent->head-nodes)).str());
		}
	}
}

#endif


 /*!
  * @file   swp_birth_process.cpp
  * @author Matthew Celnik, William Menz
  * @brief  Implementation of a birth process
  *
  *   Licence:
  *      mops is free software; you can redistribute it and/or
  *      modify it under the terms of the GNU Lesser General Public License
  *      as published by the Free Software Foundation; either version 2
  *      of the License, or (at your option) any later version.
  *
  *      This program is distributed in the hope that it will be useful,
  *      but WITHOUT ANY WARRANTY; without even the implied warranty of
  *      MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  *      GNU Lesser General Public License for more details.
  *
  *      You should have received a copy of the GNU Lesser General Public
  *      License along with this program; if not, write to the Free Software
  *      Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA
  *      02111-1307, USA.
  *
  *   Contact:
  *      Prof Markus Kraft
  *      Dept of Chemical Engineering
  *      University of Cambridge
  *      New Museums Site
  *      Pembroke Street
  *      Cambridge
  *      CB2 3RA, UK
  *
  *      Email:       mk306@cam.ac.uk
  *      Website:     http://como.cheng.cam.ac.uk
  */

#include <boost/random/bernoulli_distribution.hpp>
#include <boost/random/poisson_distribution.hpp>
#include "swp_birth_process.h"
#include "swp_mechanism.h"
#include <stdexcept>

using namespace Sweep;
using namespace Sweep::Processes;
using namespace std;

// CONSTRUCTORS AND DESTRUCTORS.

// Default constructor (protected).
BirthProcess::BirthProcess(void)
: m_cell(NULL),
  m_btype(BirthProcess::iStochastic),
  m_on(true)
{
    m_name = "Birth Process";
}

/*!
 * Initialising constructor
 *
 * @param mech  The mechanism defining the process
 * @return      A new BirthProcess
 */
BirthProcess::BirthProcess(const Sweep::Mechanism &mech)
: Process(mech),
  m_cell(NULL),
  m_btype(BirthProcess::iStochastic),
  m_on(true)
{
    m_name = "Birth Process";
}

/*!
 * Copy constructor
 *
 * @param copy  Process to copy
 * @return      A new BirthProcess, copy of the original
 */
BirthProcess::BirthProcess(const BirthProcess &copy)
: m_cell(copy.m_cell)
{
    *this = copy;
}

// OPERATOR OVERLOADS.

// Assignment operator.
BirthProcess &BirthProcess::operator =(const BirthProcess &rhs)
{
    if (this != &rhs) {
        Process::operator =(rhs);
        // Copy pointer to sampling ensemble.
        m_cell = rhs.m_cell;
        m_btype = rhs.m_btype;
        m_on = rhs.m_on;
    }
    return *this;
}

//! Set the Cell from which this process samples.
void BirthProcess::SetCell(Cell* c) {
    m_cell = c;
}

//! Sets the birth process type
void BirthProcess::SetBirthType(const BirthType t) {
    m_btype = t;
}

//! Turn the process on or off
void BirthProcess::SetProcessSwitch(const bool s) {
    m_on = s;
}

// INFORMATION FOR THE SOLVER
// Does the Cell inflow have particles present?
bool BirthProcess::HasParticlesInCell() const {
    if ((m_cell->Particles().Count() + m_cell->Particles().GetTotalParticleNumber())>0u) return true;
    else return false;
}

// TOTAL RATE CALCULATIONS.

/*!
 * Weights are scaled by this quantity when particles move from cell to cell.
 *
 * @param sys   System to evaluate scaling factor for
 * @return      The cell-transfer scaling factor
 */
double BirthProcess::F(const Cell &sys) const {
    return sys.SampleVolume() / m_cell->SampleVolume();
}

/*!
 *@param[in]            t           Time at which rate is being calculated
 *@param[in]            sys         System for which rate is to be calculated
 *@param[in]            local_geom  Spatial configuration information (ignored)
 *
 *@return   Process rate
 */
double BirthProcess::Rate(double t, const Cell &sys,
                        const Geometry::LocalGeometry1d &local_geom) const

{
    if (m_btype == BirthProcess::iStochastic && m_on) {
        return InternalRate(t, sys, local_geom);
    }
    return 0.0;
}

/*!
 *@param[in]            t           Time at which rate is being calculated
 *@param[in]            sys         System for which rate is to be calculated
 *@param[in]            local_geom  Spatial configuration information (ignored)
 *
 *@return   Process rate
 */
double BirthProcess::InternalRate(
        double t,
        const Cell &sys,
        const Geometry::LocalGeometry1d &local_geom) const {
    if (m_cell == NULL) throw runtime_error("No cell specified for sampling."
                " (Sweep, BirthProcess::InternalRate)");
    unsigned int n_total = m_cell->Particles().Count() + m_cell->Particles().GetTotalParticleNumber();
    return A() * (double)n_total;
}

// RATE TERM CALCULATIONS.

// Returns the number of rate terms for this process (one).
unsigned int BirthProcess::TermCount(void) const {return 1;}

/*!
 *
 * @param t             Time at which rate is being calculated
 * @param sys           System for which rate is to be calculated
 * @param local_geom    Spatial configuration information (ignored)
 * @param iterm         Iterator on rates vector
 * @return              Rate of birth process
 */
double BirthProcess::RateTerms(const double t, const Cell &sys,
                             const Geometry::LocalGeometry1d &local_geom,
                             fvector::iterator &iterm) const
{
    *iterm = Rate(t, sys, local_geom);
    return *(iterm++);
}

// PERFORMING THE PROCESS.

/*!
 * \param[in]       t           Time
 * \param[in,out]   sys         System to update
 * \param[in]       local_geom  Details of local phsyical layout
 * \param[in]       iterm       Process term responsible for this event
 * \param[in,out]   rng         Random number generator
 *
 * \return      0 on success, otherwise negative.
 */
int BirthProcess::Perform(double t, Sweep::Cell &sys,
                          const Geometry::LocalGeometry1d& local_geom,
                          unsigned int iterm,
                          rng_type &rng) const
{
    if (m_cell == NULL)
        throw runtime_error("No cell specified for sampling."
            " (Sweep, BirthProcess::Perform)");

    int i = 0;
    // If all particles in ensemble, select a particle at random
    if (!(m_mech->IsHybrid()))
        i = m_cell->Particles().Select(rng);
    else
    {
        // Select a particle from the ensemble or particle-number list
        // ===========================================================
        // Get totals
        double ntotal_pn = (double)(m_cell->Particles().GetTotalParticleNumber());
        double ntotal_ens = (double)(m_cell->ParticleCount());

        // Here there should be a check that the index chosen is smaller than the threshold size
        // because nothing stops the stream having a larger threshold size than current system.
        // In that instance, particles could be added to the ensemble like with surface growth.
        // However this cannot be done here easily because it requires construction of a new particles.
        // Print warning message.
        if (m_cell->Particles().GetHybridThreshold() > sys.Particles().GetHybridThreshold())
            printf("sweep: Mixture PN threshold > reactor PN threshold; "
	           "could inflow particle that cannot be stored\n");

        // Select the particle
        boost::uniform_01<rng_type&, double> unifDistrib(rng);
        double test = unifDistrib() * (ntotal_pn + ntotal_ens);
        if (ntotal_pn >= test)
        {
            // Particle is chosen from the number list
            double repeats = F(sys);
            if (repeats != floor(repeats))
            {
                boost::random::bernoulli_distribution<double> decider(repeats);
                repeats = floor(repeats);
                if (decider(rng))
                    repeats += 1.0;
            }
            if (repeats > 0.0)
            {
                unsigned int index = m_mech->SetRandomParticle(m_cell->Particles(), t, test, iUniform, rng);
		// Check we found a valid index
                if (index > 0)
		{
		    sys.Particles().UpdateTotalsWithIndex(index, repeats);
		    sys.Particles().UpdateNumberAtIndex(index, (int)repeats);
		    sys.Particles().UpdateTotalParticleNumber((int)repeats);
		}
            }
            i = -1;
        }
        else
        {
            // Particle is chosen from the ensemble
            i = m_cell->Particles().Select_usingGivenRand(iUniform, test - ntotal_pn, rng);
        }

    }
    if (i >= 0)
    {
        DoParticleBirth(t, i, sys,
        m_cell->Particles().At(i)->getStatisticalWeight() * F(sys),
        rng);
    }
    // ===========================================================

    return 0;
}

/*!
 * Create particles over time dt.
 *
 * @param t     Current time of the system
 * @param dt    Time to remove particles over
 * @param sys   The system to do transport for
 * @param local_geom    Geometry of the system
 * @param rng   Random number generator
 */
void BirthProcess::PerformDT (
        double t,
        double dt,
        Sweep::Cell &sys,
        const Geometry::LocalGeometry1d& local_geom,
        rng_type &rng) const {

    if (m_btype == BirthProcess::iContinuous) {

        Process::PerformDT(dt, t, sys, local_geom, rng);

        // Get the rate, and do n times like a LPDA process
        double rate = InternalRate(t, sys, local_geom) * dt;
        if (rate > 0.0) {
            boost::random::poisson_distribution<unsigned, double> rpt(rate);
            unsigned num = rpt(rng);
            while (num > 0) {
                // Do the process to the particle.
                Perform(t, sys, local_geom, 0, rng);
                num--;
            }
        }

    }
    // Don't do anything for the iStochastic case, as the Perform() will be called
    // in the usual manner for jump processes.

}

/*!
 * Create the particle in this system's cell.
 *
 * @param t     Time to create particle at
 * @param isp   Index of particle to clone
 * @param sys   System to put particle into
 * @param wt    Weight of the particle
 * @param rng   Random number generator
 */
void BirthProcess::DoParticleBirth(
        const double t,
        const int isp,
        Sweep::Cell &sys,
        const double wt,
        rng_type &rng) const {

    // Get reference to a particle.
    Sweep::Particle *sp = m_cell->Particles().At(isp)->Clone();

    // Reset some properties
    sp->resetCoagCount();
    sp->resetFragCount();
    sp->SetTime(t);     // Set LPDA update time.

    // Note that we could just adjust the weight of isp and add it (SWAs only),
    // but this makes the ensemble more sensitive to depletion for very
    // downstream reactors.

    double repeats = F(sys);
    int counter(-1);
    while (repeats > 0.0) {
        if (repeats >= 1.0) sys.Particles().Add(*(sp->Clone()), rng);
        else {
            boost::random::bernoulli_distribution<double> decider(repeats);
            if (decider(rng))
                counter = sys.Particles().Add(*sp, rng);
        }
        repeats -= 1.0;
    }

    // Delete the particle copy if it wasn't added by the decider
    if (counter < 0) delete sp;

}

// READ/WRITE/COPY.

// Creates a copy of the inception.
BirthProcess *const BirthProcess::Clone(void) const {return new BirthProcess(*this);}

// Returns the process type.  Used to identify different
// processes and for serialisation.
ProcessType BirthProcess::ID(void) const {return Birth_ID;}


/*
 * Copyright (c) 2019 Opticks Team. All Rights Reserved.
 *
 * This file is part of Opticks
 * (see https://bitbucket.org/simoncblyth/opticks).
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *   http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#include <cstdio>
#include <cmath>

//#include <boost/algorithm/string.hpp>
#include <boost/lexical_cast.hpp>
#include <boost/math/constants/constants.hpp>
#include "BStr.hh"

#include "NGLM.hpp"

#include "PLOG.hh"

// npy-
#include "GLMPrint.hpp"
#include "GLMFormat.hpp"

#include "Trackball.hh"

const plog::Severity Trackball::LEVEL = debug ;

const char* Trackball::PREFIX = "trackball" ;
const char* Trackball::getPrefix()
{
   return PREFIX ;
}

const char* Trackball::RADIUS = "radius" ;
const char* Trackball::ORIENTATION = "orientation" ;
const char* Trackball::TRANSLATE = "translate" ;
const char* Trackball::TRANSLATEFACTOR = "translatefactor" ;

std::vector<std::string> Trackball::getTags()
{
    std::vector<std::string> tags ;
    tags.push_back(RADIUS);
    tags.push_back(ORIENTATION);
    tags.push_back(TRANSLATE);
    tags.push_back(TRANSLATEFACTOR);
    return tags ;
}

float Trackball::getTranslationMin()
{
    return -m_translate_max ;
}

float Trackball::getTranslationMax()
{
    return m_translate_max ;
}

float* Trackball::getRadiusPtr()
{
    return &m_radius ;
}
float Trackball::getRadiusMin()
{
    return m_radius_clip[0] ;
}
float Trackball::getRadiusMax()
{
    return m_radius_clip[1] ;
}

float* Trackball::getTFactorPtr()
{
    return &m_translatefactor ;
}
float Trackball::getTFactorMin()
{
    return m_translatefactor_clip[0] ;
}
float Trackball::getTFactorMax()
{
    return m_translatefactor_clip[1] ;
}

Trackball::Trackball() :
          m_radius(1.f),
          m_translatefactor(1000.f),
          m_drag_renorm(97),
          m_drag_count(0),
          m_changed(true)
{
    home();

    setTranslateMax(1e5);
    setRadiusClip(0.1f, 10.f);
    setTranslateFactorClip(10.f, 1e6);
}

void Trackball::setTranslateMax(float _max)
{
    m_translate_max = _max ;
}

void Trackball::setRadiusClip(float _min, float _max)
{
    m_radius_clip[0] = _min ;
    m_radius_clip[1] = _max ;
}

void Trackball::setTranslateFactorClip(float _min, float _max)
{
    m_translatefactor_clip[0] = _min ;
    m_translatefactor_clip[1] = _max ;
}

float Trackball::getRadius()
{
    return m_radius ;
}
float Trackball::getTranslateFactor()
{
    return m_translatefactor  ;
}

void Trackball::setRadius(float r)
{
    m_radius = r ;
    m_changed = true ;
}
void Trackball::setTranslateFactor(float tf)
{
    m_translatefactor = tf ;
    m_changed = true ;
}
void Trackball::setTranslate(float x, float y, float z)
{
    m_translate.x = x;
    m_translate.y = y;
    m_translate.z = z;
    m_changed = true ;
}

void Trackball::home()
{
    m_translate.x = 0.f ;
    m_translate.y = 0.f ;
    m_translate.z = 0.f ;
    setOrientation(0.f, 0.f);
    m_changed = true ;
}
void Trackball::zoom_to(float , float , float , float dy)
{
    m_translate.z += dy*m_translatefactor ;
    m_changed = true ;
}
void Trackball::pan_to(float , float , float dx, float dy)
{
    m_translate.x += dx*m_translatefactor ;
    m_translate.y += dy*m_translatefactor ;
    m_changed = true ;
}

bool Trackball::hasChanged()
{
    return m_changed ;
}
void Trackball::setChanged(bool changed)
{
    m_changed = changed ;
}

glm::vec3 Trackball::getTranslation()
{
    return m_translate;
}

float* Trackball::getTranslationPtr()
{
    return glm::value_ptr(m_translate);
}

float* Trackball::getOrientationPtr()
{
    return glm::value_ptr(m_orientation);
}

std::string Trackball::getOrientationString()
{
    return gformat(m_orientation) ;
}

glm::mat4 Trackball::getTranslationMatrix()
{
    return glm::translate(  glm::mat4(1.0f), m_translate);
}

glm::mat4 Trackball::getCombinedMatrix()
{
    // http://stackoverflow.com/questions/9920624/glm-combine-rotation-and-translation
    //
    // glm::translate,  translates before rotates
    //
    //        rot * trans;
    //

    return glm::translate( getOrientationMatrix(), m_translate);
}

void Trackball::getCombinedMatrices(glm::mat4& rt, glm::mat4& rti)
{
    glm::mat4 rot  = getOrientationMatrix();
    glm::mat4 irot = glm::transpose(rot);

    glm::mat4 tra(glm::translate(m_translate));
    glm::mat4 itra(glm::translate(-m_translate));

    rt = rot * tra  ;    // translates then rotate

    rti = itra * irot ;  //

    //  rti * rt = itra * irot * rot * tra = identity
    //

}

void Trackball::getOrientationMatrices(glm::mat4& rot, glm::mat4& irot)
{
    rot  = getOrientationMatrix();
    irot = glm::transpose(rot);
}

void Trackball::getTranslationMatrices(glm::mat4& tra, glm::mat4& itra)
{
    tra = glm::translate(m_translate);
    itra = glm::translate(-m_translate);
}

void Trackball::configureI(const char* , std::vector<int> )
{
}

void Trackball::configureF(const char* name, std::vector<float> values)
{
     if(values.empty())
     {
         printf("Trackball::configureF %s no values \n", name);
     }
     else
     {
         float vlast = values.back() ;

#ifdef VERBOSE
         LOG(info) << "Trackball::configureF"
                   << " name " << name
                   << " vals " << values.size()
                   ;

         for(size_t i=0 ; i < values.size() ; i++ ) printf("%10.3f ", values[i]);
         printf(" : vlast %10.3f \n", vlast );
#endif

         if(      strcmp(name, RADIUS) ==  0)          setRadius(vlast);
         else if( strcmp(name, TRANSLATEFACTOR) ==  0) setTranslateFactor(vlast);
         else
              printf("Trackball::configureF ignoring unknown parameter %s : %10.3f \n", name, vlast);
     }
}

void Trackball::configureS(const char* name, std::vector<std::string> values)
{
     if(values.empty())
     {
         printf("Trackball::configureS %s no values \n", name);
     }
     else
     {
         std::string  vlast = values.back() ;
#ifdef VERBOSE
         LOG(info) << "Trackball::configureS"
                   << " name " << name
                   << " vals " << values.size()
                   ;

         for(size_t i=0 ; i < values.size() ; i++ ) printf("%20s ", values[i].c_str());
         printf(" : vlast %20s \n", vlast.c_str() );
#endif
         set(name, vlast);
    }
}

bool Trackball::accepts(const char* name)
{
    return
         strcmp(name,TRANSLATE)==0  ||
         strcmp(name,TRANSLATEFACTOR)==0  ||
         strcmp(name,ORIENTATION)==0 ||
         strcmp(name,RADIUS)==0 ;
}

void Trackball::configure(const char* name, const char* value_)
{
    std::string value(value_);
    set(name, value);
}

void Trackball::set(const char* name, std::string& s)
{
    if(      strcmp(name, TRANSLATE)   ==  0)     setTranslate(s);
    else if( strcmp(name, ORIENTATION) ==  0)     setOrientation(s);
    else if( strcmp(name, RADIUS) ==  0)          setRadius(s);
    else if( strcmp(name, TRANSLATEFACTOR) ==  0) setTranslateFactor(s);
    else
        printf("Trackball::set ignoring unknown parameter %s : %s \n", name, s.c_str());
}

std::string Trackball::get(const char* name)
{
    std::string s ;
    if(strcmp(name,TRANSLATE)==0)
    {
         glm::vec3 v = getTranslation();
         s = gformat(v);
    }
    else if( strcmp(name, ORIENTATION) ==  0)
    {
         glm::quat q = getOrientation();
         s = gformat(q);
    }
    else if( strcmp(name, RADIUS) ==  0)
    {
         float r = getRadius();
         s = gformat(r);
    }
    else if( strcmp(name, TRANSLATEFACTOR) ==  0)
    {
         float f = getTranslateFactor();
         s = gformat(f);
    }
    else
         printf("Trackball::get bad name %s\n", name);

    return s ;
}

void Trackball::setRadius(std::string s)
{
    setRadius(gfloat_(s));
}

void Trackball::setTranslateFactor(std::string s)
{
    setTranslateFactor(gfloat_(s));
}

void Trackball::setOrientation(std::string _tp)
{
    std::vector<std::string> tp;
    //boost::split(tp, _tp, boost::is_any_of(","));
    BStr::split(tp, _tp.c_str(), ',');

    if(tp.size() == 2 )
    {
        float theta = boost::lexical_cast<float>(tp[0]);
        float phi   = boost::lexical_cast<float>(tp[1]);
        setOrientation(theta,phi);
    }
    else if(tp.size() == 4 )
    {
        glm::quat q = gquat(_tp);
        setOrientation(q);
    }
    else
    {
        printf("Trackball::setOrientation malformed _tp : %s \n", _tp.c_str() );
    }
}

void Trackball::setTranslate(std::string _xyz)
{
    glm::vec3 v = gvec3(_xyz);
    setTranslate(v.x,v.y,v.z);
}

void Trackball::setOrientation(float _theta, float _phi)
{
    m_theta_deg = _theta ;
    m_phi_deg  = _phi ;

    setOrientation();
}

void Trackball::setOrientation()
{
    float pi = boost::math::constants::pi<float>() ;
    float theta = m_theta_deg*pi/180.f ;
    float phi   = m_phi_deg*pi/180.f ;

    glm::quat xrot(cos(0.5f*theta),sin(0.5f*theta),0.f,0.f);
    glm::quat zrot(cos(0.5f*phi),  0.f,0.f,sin(0.5f*phi));
    glm::quat q = xrot * zrot ;
    setOrientation(q);
}

glm::quat Trackball::getOrientation()
{
   return m_orientation ;
}

glm::mat4 Trackball::getOrientationMatrix()
{
   return glm::toMat4(m_orientation) ;
}

void Trackball::setOrientation(glm::quat& q)
{
    m_orientation = q  ;
    m_changed = true ;
}

void Trackball::drag_to(float x, float y, float dx, float dy)
{
    //printf("Trackball::drag_to %10.3f %10.3f %10.3f %10.3f \n", x, y, dx, dy);

    m_drag_count += 1 ;

    glm::quat drag = rotate(x,y,dx,dy);

    bool bad =
                std::isnan(drag.x) ||
                std::isnan(drag.y) ||
                std::isnan(drag.z) ||
                std::isnan(drag.w) ||
                std::isinf(drag.x) ||
                std::isinf(drag.y) ||
                std::isinf(drag.z) ||
                std::isinf(drag.w)  ;

    if(bad)
    {
        LOG(LEVEL) << "IGNORING bad drag " ;
        return ;
    }

    glm::quat qrot = m_orientation * drag ;   // perturb orientation by drag rotation

    if(m_drag_count > m_drag_renorm)
    {
        //print(qrot, "Trackball::drag_to before renorm");
        qrot = glm::normalize(qrot);
        //print(qrot, "Trackball::drag_to after renorm");
        m_drag_count = 0 ;
    }

    setOrientation(qrot);
}

void Trackball::Summary(const char* msg)
{
    printf(" trackballradius  %10.3f \n", m_radius );
    print(getOrientation(), msg);
    //print(getOrientationMatrix(), msg);
}

glm::quat Trackball::rotate(float x, float y, float dx, float dy)
{
    // p0, p1 are positions of screen before and after coordinates
    // projected onto a deformed virtual trackball

    glm::vec3 p0(x   ,    y, project(m_radius,x,y));
    glm::vec3 p1(x+dx, y+dy, project(m_radius,x+dx,y+dy));

    // axis of rotation
    glm::vec3 axis = glm::cross(p1, p0);

    // angle of rotation
    float t = glm::clamp(glm::length(p1-p0)/(2.f*m_radius), -1.f, 1.f ) ;
    float phi = 2.f * asin(t) ;

    glm::quat q = glm::angleAxis( phi, glm::normalize(axis) );

#ifdef DEBUG
    print(p0,  "Trackball::rotate p0");
    print(p1,  "Trackball::rotate p1");
    print(axis,"Trackball::rotate axis");
    printf("Trackball::rotate t %15.5f phi %15.5f \n", t, phi );
    print(q,   "Trackball::rotate q");
#endif

    return q ;
}

float Trackball::project(float r, float x, float y)
{
  /*
        Project an x,y pair onto a sphere of radius r OR a hyperbolic sheet
        if we are away from the center of the sphere.

        For points inside xy circle::

                  d^2 = x^2 + y^2

                    d < r / sqrt(2)

                  d^2 < r^2 / 2

            x^2 + y^2 < r^2 / 2

        determine z from::

                z^2 + d^2 = r^2

        So are projecting onto sphere the center of which is on the screen plane.
  */

  float z, t, d ;
  d = sqrt(x*x + y*y);
  if (d < r * 0.70710678118654752440f)
  {
      z = sqrt(r*r - d*d);
  }
  else
  {
     t = r / 1.41421356237309504880f ;
     z = t*t / d  ;
  }
  return z;
}


/**
 * @file path.hpp
 * @author Leonardo Arcari (leonardo1.arcari@gmail.com)
 * @version 1.0.0
 * @date 2018-10-28
 *
 * @copyright Copyright (c) 2018 Leonardo Arcari
 *
 * MIT Licence
 *
 * 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 ALTERNATIVE_ROUTING_LIB_PATH_HPP
#define ALTERNATIVE_ROUTING_LIB_PATH_HPP

#include <arlib/details/path_impl.hpp>
#include <arlib/type_traits.hpp>

#include <boost/graph/filtered_graph.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/properties.hpp>

#include <utility>

/**
 * An Alternative-Routing library for Boost.Graph
 */
namespace arlib {
/**
 * A *view* of a graph to a simple path. A (simple) Path from `s` to `t` is a
 * cycle-free connected sequence of edges \f$p_{st} = \langle	(s, v_1), (v_1,
 * v_x), \dots , (v_y, t) \rangle\f$.
 *
 * This class wraps a `boost::filtered_graph<Graph>` to expose only vertices and
 * edges that make up a simple path. Moreover it provides the `length` of the
 * path, which is defined as the sum of the weights of all the edges. In
 * formula: \f$\ell(p) = \sum_{\forall (u, v) \in p} w_{uv}\f$.
 *
 * Just like `boost::filtered_graph`, Path models the same concepts as the
 * underlying graph type. If the underlying `Graph` type models
 * `VertexAndEdgeListGraph` and `PropertyGraph` then so does the Path.
 * If the underlying `Graph` type models fewer or smaller concepts than these,
 * then so does the Path.
 *
 * @tparam Graph The graph type
 */
template <typename Graph, typename Vertex = vertex_of_t<Graph>,
          typename Edge = edge_of_t<Graph>,
          typename Length = length_of_t<Graph>>
class Path {
public:
  /**
   * The inner boost::filtered_graph type
   */
  using FilteredGraph =
      boost::filtered_graph<Graph, details::alternative_path_edges<Edge>,
                            details::alternative_path_vertices<Vertex>>;
  /**
   * Construct a new Path object from a FilteredGraph and the Path length.
   *
   * @param g The FilteredGraph to wrap.
   * @param length The path length.
   */
  Path(std::shared_ptr<FilteredGraph> g, Length length)
      : graph_{std::move(g)}, length_{length} {};
  /**
   * Copy-construct a new Path.
   *
   * Copy operations on Path are always **shallow**. That means
   * that multiple copies always refer to the **same memory location**.
   *
   * @param other The Path to copy from.
   */
  Path(Path const &other) = default;
  /**
   * Move-construct a new Path.
   *
   * @param other The Path to move.
   */
  Path(Path &&other) noexcept = default;
  /**
   * Copy-assign a Path to `this`.
   *
   * Copy operations on Path are always **shallow**. That means
   * that multiple copies always refer to the **same memory location**.
   *
   * @param other The Path to copy from.
   * @return `this`.
   */
  Path &operator=(Path const &other) = default;
  /**
   * Move-assign a Path to `this`.
   *
   * @param other The Path to move.
   * @return `this`.
   */
  Path &operator=(Path &&other) noexcept = default;
  /**
   * Swaps `this` with another Path.
   *
   * @param other The Path to swap with.
   */
  void swap(Path &other) {
    using std::swap;
    swap(graph_, other.graph_);
    swap(length_, length_);
  }
  /**
   * @return the length of the path.
   */
  Length length() const { return length_; }

  //===-------------------------------------------------------------------===//
  //                           Graph concept
  //===-------------------------------------------------------------------===//
  /**
   * The type for the vertex descriptors associated with the Path,
   * which is the same type as the `vertex_descriptor` for the original `Graph`.
   */
  using vertex_descriptor =
      typename boost::graph_traits<FilteredGraph>::vertex_descriptor;
  /**
   * The type for the edge descriptors associated with the Path,
   * which is the same type as the `edge_descriptor` for the original `Graph`.
   */
  using edge_descriptor =
      typename boost::graph_traits<FilteredGraph>::edge_descriptor;
  /**
   * Provides information about whether the graph is directed (`directed_tag`)
   * or undirected (`undirected_tag`).
   */
  using directed_category =
      typename boost::graph_traits<FilteredGraph>::directed_category;
  /**
   * This describes whether the graph class allows the insertion of parallel
   * edges (edges with the same source and target). The two tags are
   * `allow_parallel_edge_tag` and `disallow_parallel_edge_tag`.
   */
  using edge_parallel_category =
      typename boost::graph_traits<FilteredGraph>::edge_parallel_category;
  /**
   * The ways in which the vertices and edges of the graph can be visited
   */
  using traversal_category =
      typename boost::graph_traits<FilteredGraph>::traversal_category;
  /**
   * @return A special `boost::graph_traits<Path<Graph>>::vertex_descriptor`
   * object which does not refer to any vertex of graph object which type is
   * `Graph`.
   */
  static vertex_descriptor null_vertex() {
    return boost::graph_traits<FilteredGraph>::null_vertex();
  }

  //===-------------------------------------------------------------------===//
  //                         IncidenceGraph concept
  //===-------------------------------------------------------------------===//
  /**
   * The type for the iterators returned by out_edges().
   * The iterator is a model of `MultiPassInputIterator`.
   */
  using out_edge_iterator =
      typename boost::graph_traits<FilteredGraph>::out_edge_iterator;
  /**
   * The type used for dealing with the number of edges incident to a vertex in
   * the graph
   */
  using degree_size_type =
      typename boost::graph_traits<FilteredGraph>::degree_size_type;

  template <typename G>
  friend std::pair<typename Path<G>::out_edge_iterator,
                   typename Path<G>::out_edge_iterator>
  out_edges(typename Path<G>::vertex_descriptor u, Path<G> const &g);

  template <typename G>
  friend typename Path<G>::vertex_descriptor
  source(typename Path<G>::edge_descriptor e, Path<G> const &g);

  template <typename G>
  friend typename Path<G>::vertex_descriptor
  target(typename Path<G>::edge_descriptor e, Path<G> const &g);

  template <typename G>
  friend typename Path<G>::degree_size_type
  out_degree(typename Path<G>::vertex_descriptor v, Path<G> const &g);

  //===-------------------------------------------------------------------===//
  //                       BidirectionalGraph concept
  //===-------------------------------------------------------------------===//
  /**
   * The type for the iterators returned by in_edges().
   * The iterator is a model of `MultiPassInputIterator`.
   */
  using in_edge_iterator =
      typename boost::graph_traits<FilteredGraph>::in_edge_iterator;

  template <typename G>
  friend std::pair<typename Path<G>::in_edge_iterator,
                   typename Path<G>::in_edge_iterator>
  in_edges(typename Path<G>::vertex_descriptor v, Path<G> const &g);

  template <typename G>
  friend typename Path<G>::degree_size_type
  in_degree(typename Path<G>::vertex_descriptor v, Path<G> const &g);

  template <typename G>
  friend typename Path<G>::degree_size_type
  degree(typename Path<G>::vertex_descriptor v, Path<G> const &g);

  //===-------------------------------------------------------------------===//
  //                        AdjacencyGraph concept
  //===-------------------------------------------------------------------===//
  /**
   * The type for the iterators returned by adjacent_vertices().
   * The iterator is a model of `MultiPassInputIterator`.
   */
  using adjacency_iterator =
      typename boost::graph_traits<FilteredGraph>::adjacency_iterator;

  template <typename G>
  friend std::pair<typename Path<G>::adjacency_iterator,
                   typename Path<G>::adjacency_iterator>
  adjacent_vertices(typename Path<G>::vertex_descriptor v, Path<G> const &g);

  //===-------------------------------------------------------------------===//
  //                        VertexListGraph concept
  //===-------------------------------------------------------------------===//
  /**
   * The type for the iterators returned by vertices().
   * The iterator is a model of `MultiPassInputIterator`.
   */
  using vertex_iterator =
      typename boost::graph_traits<FilteredGraph>::vertex_iterator;
  /**
   * The type used for dealing with the number of vertices in the graph.
   */
  using vertices_size_type =
      typename boost::graph_traits<FilteredGraph>::vertices_size_type;

  template <typename G>
  friend std::pair<typename Path<G>::vertex_iterator,
                   typename Path<G>::vertex_iterator>
  vertices(Path<G> const &g);

  template <typename G>
  friend typename Path<G>::vertices_size_type num_vertices(Path<G> const &g);

  //===-------------------------------------------------------------------===//
  //                        VertexListGraph concept
  //===-------------------------------------------------------------------===//
  /**
   * The type for the iterators returned by edges().
   * The iterator is a model of `MultiPassInputIterator`.
   */
  using edge_iterator =
      typename boost::graph_traits<FilteredGraph>::edge_iterator;
  /**
   * The type used for dealing with the number of edges in the graph.
   */
  using edges_size_type =
      typename boost::graph_traits<FilteredGraph>::edges_size_type;

  template <typename G>
  friend std::pair<typename Path<G>::edge_iterator,
                   typename Path<G>::edge_iterator>
  edges(Path<G> const &g);

  template <typename G>
  friend typename Path<G>::edges_size_type num_edges(Path<G> const &g);

  template <typename G>
  friend std::pair<typename Path<G>::edge_descriptor, bool>
  edge(typename Path<G>::vertex_descriptor u,
       typename Path<G>::vertex_descriptor v, Path<G> const &g);

  //===-------------------------------------------------------------------===//
  //                          PropertyMap concept
  //===-------------------------------------------------------------------===//

  template <typename G, typename Property>
  friend typename boost::property_map<typename Path<G>::FilteredGraph,
                                      Property>::type
  get(Property p, Path<G> &g);

  template <typename G, typename Property>
  friend typename boost::property_map<typename Path<G>::FilteredGraph,
                                      Property>::const_type
  get(Property p, const Path<G> &g);

  template <typename G, typename Property, typename Key>
  friend typename boost::property_map_value<typename Path<G>::FilteredGraph,
                                            Property>::type
  get(Property p, const Path<G> &g, const Key &k);

  template <typename G, typename Property, typename Key, typename Value>
  friend void put(Property p, const Path<G> &g, const Key &k, const Value &val);

private:
  std::shared_ptr<FilteredGraph> graph_ = {};
  Length length_ = {};
};
/**
 * Swap between two Path.
 *
 * @tparam Graph The graph type.
 * @param v1 The first Path.
 * @param v2 The second Path.
 */
template <typename Graph> void swap(Path<Graph> &v1, Path<Graph> &v2) {
  v1.swap(v2);
}

//===----------------------------------------------------------------------===//
//                Non-member functions for the Path Incidence Graph
//===----------------------------------------------------------------------===//
/**
 * @return An iterator-range providing access to the out-edges of vertex `u` in
 * graph `g`. If the graph is undirected, this iterator-range provides access to
 * all edges incident on vertex `u`. For both directed and undirected graphs,
 * for an out-edge `e`, `source(e, g) == u` and `target(e, g) == v` where `v` is
 * a vertex adjacent to `u`.
 */
template <typename G>
std::pair<typename Path<G>::out_edge_iterator,
          typename Path<G>::out_edge_iterator>
out_edges(typename Path<G>::vertex_descriptor u, Path<G> const &g) {
  using boost::out_edges;
  return out_edges(u, *g.graph_);
}
/**
 * @return The source vertex of edge `e`.
 */
template <typename G>
typename Path<G>::vertex_descriptor source(typename Path<G>::edge_descriptor e,
                                           Path<G> const &g) {
  using boost::source;
  return source(e, *g.graph_);
}
/**
 * @return The target vertex of edge `e`.
 */
template <typename G>
typename Path<G>::vertex_descriptor target(typename Path<G>::edge_descriptor e,
                                           Path<G> const &g) {
  using boost::target;
  return target(e, *g.graph_);
}
/**
 * @return The number of edges leaving vertex `u`.
 */
template <typename G>
typename Path<G>::degree_size_type
out_degree(typename Path<G>::vertex_descriptor u, Path<G> const &g) {
  using boost::out_degree;
  return out_degree(u, *g.graph_);
}

//===----------------------------------------------------------------------===//
//              Non-member functions for the Path Bidirectional Graph
//===----------------------------------------------------------------------===//
/**
 * @return An iterator-range providing access to the in-edges of vertex
 * `v` in graph `g`. For an in-edge `e`, `target(e, g) == v` and `source(e, g)
 * == u` for some vertex `u` that is adjacent to `v`, whether the graph is
 * directed or undirected.
 */
template <typename G>
std::pair<typename Path<G>::in_edge_iterator,
          typename Path<G>::in_edge_iterator>
in_edges(typename Path<G>::vertex_descriptor v, Path<G> const &g) {
  using boost::in_edges;
  return in_edges(v, *g.graph_);
}
/**
 * @return The number of edges entering vertex `v`.
 */
template <typename G>
typename Path<G>::degree_size_type
in_degree(typename Path<G>::vertex_descriptor v, Path<G> const &g) {
  using boost::in_degree;
  return in_degree(v, *g.graph_);
}
/**
 * @return The number of in-edges plus out-edges (for directed graphs)
 * or the number of incident edges (for undirected graphs) of vertex `v` in
 * graph `g`.
 */
template <typename G>
typename Path<G>::degree_size_type degree(typename Path<G>::vertex_descriptor v,
                                          Path<G> const &g) {
  using boost::degree;
  return degree(v, *g.graph_);
}

//===----------------------------------------------------------------------===//
//              Non-member function for the Path Adjacency Graph
//===----------------------------------------------------------------------===//
/**
 * @return An iterator-range providing access to the vertices adjacent
 * to vertex `v` in graph `g`.
 */
template <typename G>
std::pair<typename Path<G>::adjacency_iterator,
          typename Path<G>::adjacency_iterator>
adjacent_vertices(typename Path<G>::vertex_descriptor v, Path<G> const &g) {
  using boost::adjacent_vertices;
  return adjacent_vertices(v, *g.graph_);
}

//===----------------------------------------------------------------------===//
//             Non-member function for the Path Vertex List Graph
//===----------------------------------------------------------------------===//
/**
 * @return An iterator-range providing access to the vertex set of graph `g`.
 */
template <typename G>
std::pair<typename Path<G>::vertex_iterator, typename Path<G>::vertex_iterator>
vertices(Path<G> const &g) {
  using boost::vertices;
  return vertices(*g.graph_);
}
/**
 * @return The number of vertices in the underlying graph.
 */
template <typename G>
typename Path<G>::vertices_size_type num_vertices(Path<G> const &g) {
  using boost::num_vertices;
  return num_vertices(*g.graph_);
}

//===----------------------------------------------------------------------===//
//              Non-member functions for the Edge List Graph
//===----------------------------------------------------------------------===//
/**
 * @return An iterator-range providing access to the edge set of graph `g`.
 */
template <typename G>
std::pair<typename Path<G>::edge_iterator, typename Path<G>::edge_iterator>
edges(Path<G> const &g) {
  using boost::edges;
  return edges(*g.graph_);
}
/**
 * @return The number of edges in the underlying graph.
 */
template <typename G>
typename Path<G>::edges_size_type num_edges(Path<G> const &g) {
  using boost::num_edges;
  return num_edges(*g.graph_);
}
/**
 * @return The edge connecting vertex `u` to vertex `v` in graph `g`.
 */
template <typename G>
std::pair<typename Path<G>::edge_descriptor, bool>
edge(typename Path<G>::vertex_descriptor u,
     typename Path<G>::vertex_descriptor v, Path<G> const &g) {
  using boost::edge;
  return edge(u, v, *g.graph_);
}

//===----------------------------------------------------------------------===//
//                              Property Map
//===----------------------------------------------------------------------===//
/**
 * @return The property map object for the vertex property specified by
 * `Property`. The `Property` must match one of the properties specified in
 * the graph's `VertexProperty` template argument.
 */
template <typename G, typename Property>
typename boost::property_map<typename Path<G>::FilteredGraph, Property>::type
get(Property p, Path<G> &g) {
  return get(p, const_cast<typename Path<G>::FilteredGraph &>(*g.graph_));
}
/**
 * @return The property map object for the vertex property specified by
 * `Property`. The `Property` must match one of the properties specified in
 * the graph's `VertexProperty` template argument.
 */
template <typename G, typename Property>
typename boost::property_map<typename Path<G>::FilteredGraph,
                             Property>::const_type
get(Property p, const Path<G> &g) {
  return get(p, (const typename Path<G>::FilteredGraph &)*(g.graph_));
}
/**
 * @return The property value for `k`, where `k` is either a vertex or
 * edge descriptor
 */
template <typename G, typename Property, typename Key>
typename boost::property_map_value<typename Path<G>::FilteredGraph,
                                   Property>::type
get(Property p, const Path<G> &g, const Key &k) {
  return get(p, (const typename Path<G>::FilteredGraph &)*(g.graph_), k);
}
/**
 * This sets the property value for `k` to `val`. `k` is either a vertex or edge
 * descriptor. `Value` must be convertible to `typename
 * property_traits<property_map<Path<G>, Property>::type>::value_type`
 */
template <typename G, typename Property, typename Key, typename Value>
void put(Property p, const Path<G> &g, const Key &k, const Value &val) {
  put(p, const_cast<typename Path<G>::FilteredGraph &>(*g.graph_), k, val);
}

//===----------------------------------------------------------------------===//
//              Non-member functions for the Path instantiation
//===----------------------------------------------------------------------===//

template <typename Graph, typename Vertex = vertex_of_t<Graph>,
          typename Edge = edge_of_t<Graph>>
std::shared_ptr<
    boost::filtered_graph<Graph, details::alternative_path_edges<Edge>,
                          details::alternative_path_vertices<Vertex>>>
make_path_filtered_graph(
    Graph const &G, std::unordered_set<Edge, boost::hash<Edge>> const &edges,
    std::unordered_set<Vertex, boost::hash<Vertex>> const &vertices) {
  using FilteredGraph =
      boost::filtered_graph<Graph, details::alternative_path_edges<Edge>,
                            details::alternative_path_vertices<Vertex>>;
  auto ap_edges = details::alternative_path_edges{edges};
  auto ap_vertices = details::alternative_path_vertices{vertices};
  return std::make_shared<FilteredGraph>(G, ap_edges, ap_vertices);
}

template <typename Graph, typename Vertex = vertex_of_t<Graph>,
          typename Edge = edge_of_t<Graph>>
std::shared_ptr<
    boost::filtered_graph<Graph, details::alternative_path_edges<Edge>,
                          details::alternative_path_vertices<Vertex>>>
make_path_filtered_graph(
    Graph const &G, std::unordered_set<Edge, boost::hash<Edge>> &&edges,
    std::unordered_set<Vertex, boost::hash<Vertex>> &&vertices) {
  using FilteredGraph =
      boost::filtered_graph<Graph, details::alternative_path_edges<Edge>,
                            details::alternative_path_vertices<Vertex>>;
  auto ap_edges = details::alternative_path_edges{std::move(edges)};
  auto ap_vertices = details::alternative_path_vertices{std::move(vertices)};
  return std::make_shared<FilteredGraph>(G, ap_edges, ap_vertices);
}
} // namespace arlib

/**
 * A collection of free, peer-reviewed C++ libraries.
 */
namespace boost {
/**
 * The property map type for vertex or edge properties in the graph. The same
 * property maps from the wrapped `filtered_graph` graph are available in the
 * Path.
 */
template <typename G, typename Property>
struct property_map<arlib::Path<G>, Property>
    : property_map<typename arlib::Path<G>::FilteredGraph, Property> {};
} // namespace boost

#endif // ALTERNATIVE_ROUTING_LIB_PATH_HPP


/**
 * GeoDa TM, Copyright (C) 2011-2015 by Luc Anselin - all rights reserved
 *
 * This file is part of GeoDa.
 *
 * GeoDa is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * GeoDa is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 */

#include <algorithm>
#include <iomanip>
#include <fstream>
#include <set>
#include <map>
#include <utility>
#include <boost/uuid/uuid.hpp>
#include <wx/filename.h>

#include "../GenUtils.h"
#include "../Project.h"
#include "../VarCalc/WeightsManInterface.h"
#include "../DataViewer/TableInterface.h"
#include "GalWeight.h"

////////////////////////////////////////////////////////////////////////////////
//
// GalElement
//
////////////////////////////////////////////////////////////////////////////////
GalElement::GalElement()
{
    is_nbrAvgW_empty = true;
}

bool GalElement::Check(long nbrIdx)
{
    if (nbrLookup.find(nbrIdx) != nbrLookup.end())
        return true;
    return false;
}

// return row standardized weights value
double GalElement::GetRW(int idx)
{
    if (is_nbrAvgW_empty) {
        size_t sz = nbr.size();
        nbrAvgW.resize(sz);
        double sumW = 0.0;

        for (size_t i=0; i<sz; i++)
            sumW += nbrWeight[i];

        for (size_t i=0; i<sz; i++) {
            nbrAvgW[i] = nbrWeight[i] / sumW;
        }
        is_nbrAvgW_empty = false;
    }

    if (nbrLookup.find(idx) != nbrLookup.end())
        return nbrAvgW[nbrLookup[idx]];
    return 0;
}

void GalElement::SetSizeNbrs(size_t	sz)
{
	nbr.resize(sz);
    nbrWeight.resize(sz);
    for(size_t i=0; i<sz; i++) {
        nbrWeight[i] = 1.0;
    }
}

// (which neighbor, what ID)
void GalElement::SetNbr(size_t pos, long n)
{
    if (pos < nbr.size()) {
        nbr[pos] = n;
        nbrLookup[n] = pos;
    }
    // this should be called by GAL created only
    if (pos < nbrWeight.size()) {
        nbrWeight[pos] = 1.0;
    }
}

// (which neighbor, what ID, what value)
void GalElement::SetNbr(size_t pos, long n, double w)
{
    if (pos < nbr.size()) {
        nbr[pos] = n;
        nbrLookup[n] = pos;
    } else {
        nbr.push_back(n);
        nbrLookup[n] = pos;
    }

    // this should be called by GWT-GAL
    if (pos < nbrWeight.size()) {
        nbrWeight[pos] = w;
    } else {
        nbrWeight.push_back(w);
    }
}

// Update neighbor information on the fly using undefs information
// NOTE: this has to be used with a copy of weights (keep the original weights!)
void GalElement::Update(const std::vector<bool>& undefs)
{
    std::vector<int> undef_obj_positions;

    for (int i=0; i<nbr.size(); i++) {
        int obj_id = nbr[i];
        if (undefs[obj_id]) {
            int pos = nbrLookup[obj_id];
            undef_obj_positions.push_back(pos);
        }
    }

    if (undef_obj_positions.empty())
        return;

    // sort the positions in descending order, for removing from std::vector
	std::sort(undef_obj_positions.begin(),
              undef_obj_positions.end(), std::greater<int>());

    for (int i=0; i<undef_obj_positions.size(); i++) {
        int pos = undef_obj_positions[i];
        if (pos < nbr.size()) {
            nbrLookup.erase( nbr[pos] );
            nbr.erase( nbr.begin() + pos);
        }
        if (pos < nbrWeight.size()) {
            nbrWeight.erase( nbrWeight.begin() + pos);
        }
    }
}

/*
void GalElement::SetNbrs(const std::vector<long>& nbrs)
{
	nbr = nbrs;
    if (nbrWeight.empty()) {
        size_t sz = nbr.size();
        nbrWeight.resize(sz);
        for(size_t i=0; i<sz; i++) {
            nbrLookup[nbrs[i]] = i;
            nbrWeight[i] = 1.0;
        }
    }
}
 */

void GalElement::SetNbrs(const GalElement& gal)
{
    size_t sz = gal.Size();
    nbr.resize(sz);
    nbrWeight.resize(sz);

    nbr = gal.GetNbrs();
    nbrLookup = gal.nbrLookup;
    nbrWeight = gal.GetNbrWeights();
    nbrLookup = gal.nbrLookup;
    nbrAvgW = gal.nbrAvgW;
}

const std::vector<long> & GalElement::GetNbrs() const
{
	return nbr;
}

const std::vector<double> & GalElement::GetNbrWeights() const
{
	return nbrWeight;
}

void GalElement::SortNbrs()
{
	std::sort(nbr.begin(), nbr.end(), std::greater<long>());
}

/** Compute spatial lag for a contiguity weights matrix.
 Automatically performs standardization of the result. */
double GalElement::SpatialLag(const std::vector<double>& x) const
{
	double lag = 0;
	size_t sz = Size();

    double sumW = 0;
	for (size_t i=0; i<sz; ++i) {
        sumW += nbrWeight[i];
    }

    if (sumW == 0)
        lag = 0;
    else {
        for (size_t i=0; i<sz; ++i) {

            lag += x[nbr[i]] * nbrWeight[i] / sumW;
        }
    }

	return lag;
}

/** Compute spatial lag for a contiguity weights matrix.
 Automatically performs standardization of the result. */
double GalElement::SpatialLag(const double *x) const
{
	double lag = 0;
	size_t sz = Size();

    double sumW = 0;
	for (size_t i=0; i<sz; ++i) {
        sumW += nbrWeight[i];
    }

    if (sumW == 0)
        lag = 0;
    else {
        for (size_t i=0; i<sz; ++i) {
            lag += x[nbr[i]] * nbrWeight[i] / sumW;
        }
    }

	//for (size_t i=0; i<sz; ++i) lag += x[nbr[i]];
	//if (sz>1) lag /= (double) sz;
	return lag;
}

double GalElement::SpatialLag(const std::vector<double>& x,
							  const int* perm) const
{
    // todo: this should also handle ReadGWtAsGAL like previous 2 functions
	double lag = 0;
	size_t sz = Size();
	for (size_t i=0; i<sz; ++i) lag += x[perm[nbr[i]]];
	if (sz>1) lag /= (double) sz;
	return lag;
}

////////////////////////////////////////////////////////////////////////////////
//
// GalWeight
//
////////////////////////////////////////////////////////////////////////////////
GalWeight::GalWeight(const GalWeight& gw)
: GeoDaWeight(gw)
{
	GalWeight::operator=(gw);
}

GalWeight& GalWeight::operator=(const GalWeight& gw)
{
	GeoDaWeight::operator=(gw);
	gal = new GalElement[num_obs];

    for (int i=0; i<num_obs; ++i) {
        gal[i].SetNbrs(gw.gal[i]);
    }

    this->num_obs = gw.num_obs;
    this->wflnm = gw.wflnm;
    this->id_field = gw.id_field;

	return *this;
}

void GalWeight::Update(const std::vector<bool>& undefs)
{
    for (int i=0; i<num_obs; ++i) {
        gal[i].Update(undefs);
    }

}

bool GalWeight::HasIsolates(GalElement *gal, int num_obs)
{
    if (!gal) {
        return false;
    }
	for (int i=0; i<num_obs; i++) {
        if (gal[i].Size() <= 0) {
            return true;
        }
    }
	return false;
}

bool GalWeight::SaveDIDWeights(Project* project, int num_obs,
                               std::vector<wxInt64>& newids,
                               std::vector<wxInt64>& stack_ids,
                               const wxString& ofname)
{
    using namespace std;
    if (!project || ofname.empty()) return false;

    WeightsManInterface* wmi = project->GetWManInt();
    if (!wmi) return false;

    wxString layer_name = GenUtils::GetFileNameNoExt(ofname);

    GalElement* gal = this->gal;
    if (!gal) return false;

    int n = newids.size();

    ofstream out;
    out.open(GET_ENCODED_FILENAME(ofname));
    if (!(out.is_open() && out.good())) return false;

    // if layer_name contains an empty space, the layer name should be
    // braced with quotes "layer name"
    if (layer_name.Contains(" ")) {
        layer_name = "\"" + layer_name + "\"";
    }

    wxString id_var_name("STID");
    out << "0 " << n << " " << layer_name;
    out << " " << id_var_name << endl;

    int offset = 0;

    for (size_t i=0; i<n; ++i) {
        int orig_id = stack_ids[i];
        if (i == num_obs) {
            offset = num_obs;
            num_obs += num_obs;
        }

        out << newids[i];
        out << " " << gal[orig_id].Size() << endl;

        for (int cp=gal[orig_id].Size(); --cp >= 0;) {
			int n_id = gal[orig_id][cp];
            out << n_id + offset + 1; // n_id starts from 0, so add 1
            if (cp > 0) out << " ";
        }
        out << endl;
    }
    return true;
}

bool GalWeight::SaveSpaceTimeWeights(const wxString& ofname, WeightsManInterface* wmi, TableInterface* table_int)
{
    using namespace std;

    if (ofname.empty() || !wmi || !table_int)
        return false;

    wxString layer_name = GenUtils::GetFileNameNoExt(ofname);
    GalElement* gal = this->gal;
    if (!gal) return false;

    vector<wxString> id_vec;
    int c_id = table_int->FindColId(this->id_field);
    if (c_id < 0) return false;

    table_int->GetColData(c_id, 1, id_vec);

    std::vector<wxString> time_ids;
    table_int->GetTimeStrings(time_ids);

    size_t num_obs = id_vec.size();
    size_t num_t = time_ids.size();
    size_t n = num_obs * num_t;

    typedef std::pair<wxString, wxString> STID_KEY;
    std::map<STID_KEY, int> stid_dict;

    int id=1;
    for (size_t i=0; i<num_t; ++i) {
        for (size_t j=0; j<num_obs; ++j) {
            STID_KEY k(id_vec[j], time_ids[i]);
            stid_dict[k] = id++;
        }
    }

    ofstream out;
    out.open(GET_ENCODED_FILENAME(ofname));
    if (!(out.is_open() && out.good())) return false;

    // if layer_name contains an empty space, the layer name should be
    // braced with quotes "layer name"
    if (layer_name.Contains(" ")) {
        layer_name = "\"" + layer_name + "\"";
    }

    wxString id_var_name("STID");
    out << "0 " << n << " " << layer_name;
    out << " " << id_var_name << endl;

    for (size_t i=0; i<num_t; ++i) {
        for (size_t j=0; j<num_obs; ++j) {
            STID_KEY k(id_vec[j], time_ids[i]);
            int m_id = stid_dict[k];
            out << m_id;
            out << " " << gal[j].Size() << endl;

            for (int cp=gal[j].Size(); --cp >= 0;) {
                STID_KEY k(id_vec[gal[j][cp]], time_ids[i]);
                int n_id = stid_dict[k];
                out << n_id;
                if (cp > 0) out << " ";
            }
            out << endl;
        }
    }

    return true;
}

///////////////////////////////////////////////////////////////////////////////
// TODO: following old style functions should be moved into GalWeight class
bool Gda::SaveGal(const GalElement* g,
                  const wxString& _layer_name,
                  const wxString& ofname,
                  const wxString& id_var_name,
                  const std::vector<wxInt64>& id_vec)
{
	using namespace std;
	if (g == NULL || ofname.empty() ||
			id_var_name.empty() || id_vec.size() == 0) return false;

	wxFileName wx_fn(ofname);
	wx_fn.SetExt("gal");
	wxString final_fon(wx_fn.GetFullPath());
	ofstream out;
	out.open(GET_ENCODED_FILENAME(final_fon));
	if (!(out.is_open() && out.good())) return false;

    wxString layer_name(_layer_name);
    // if layer_name contains an empty space, the layer name should be
    // braced with quotes "layer name"
    if (layer_name.Contains(" ")) {
        layer_name = "\"" + layer_name + "\"";
    }

	size_t num_obs = (int) id_vec.size();
	out << "0 " << num_obs << " " << layer_name;
	out << " " << id_var_name << endl;

	for (size_t i=0; i<num_obs; ++i) {
		out << id_vec[i];
		out << " " << g[i].Size() << endl;
		for (int cp=g[i].Size(); --cp >= 0;) {
			out << id_vec[g[i][cp]];
			if (cp > 0)
                out << " ";
		}
		out << endl;
	}
	return true;
}

bool Gda::SaveGal(const GalElement* g,
                  const wxString& _layer_name,
                  const wxString& ofname,
                  const wxString& id_var_name,
                  const std::vector<wxString>& id_vec)
{
	using namespace std;
	if (g == NULL || ofname.empty() ||
        id_var_name.empty() || id_vec.size() == 0) return false;

	wxFileName wx_fn(ofname);
	wx_fn.SetExt("gal");
	wxString final_fon(wx_fn.GetFullPath());
	ofstream out;
	out.open(GET_ENCODED_FILENAME(final_fon));
	if (!(out.is_open() && out.good()))
        return false;

    wxString layer_name(_layer_name);

    // if layer_name contains an empty space, the layer name should be
    // braced with quotes "layer name"
    if (layer_name.Contains(" ")) {
        layer_name = "\"" + layer_name + "\"";
    }
	size_t num_obs = (int) id_vec.size();
	out << "0 " << num_obs << " " << layer_name;
	out << " " << id_var_name << endl;

	for (size_t i=0; i<num_obs; ++i) {
		out << id_vec[i];
		out << " " << g[i].Size() << endl;
		for (int cp=g[i].Size(); --cp >= 0;) {
			out << id_vec[g[i][cp]];
			if (cp > 0)
                out << " ";
		}
		out << endl;
	}
	return true;
}

bool Gda::SaveSpaceTimeGal(const GalElement* g,
                  const std::vector<wxString>& time_ids,
                  const wxString& _layer_name,
                  const wxString& ofname,
                  const wxString& id_var_name,
                  const std::vector<wxString>& id_vec)
{
	using namespace std;
	if (g == NULL || ofname.empty() ||
        id_var_name.empty() || id_vec.size() == 0) return false;

	wxFileName wx_fn(ofname);
	wx_fn.SetExt("gal");
	wxString final_fon(wx_fn.GetFullPath());
	ofstream out;
	out.open(GET_ENCODED_FILENAME(final_fon));
	if (!(out.is_open() && out.good())) return false;

	size_t num_obs = id_vec.size();
    size_t num_t = time_ids.size();
    size_t n = num_obs * num_t;

    wxString layer_name(_layer_name);
    // if layer_name contains an empty space, the layer name should be
    // braced with quotes "layer name"
    if (layer_name.Contains(" ")) {
        layer_name = "\"" + layer_name + "\"";
    }

	out << "0 " << n << " " << layer_name;
	out << " " << id_var_name << endl;

    for (size_t i=0; i<num_t; ++i) {
    	for (size_t j=0; j<num_obs; ++j) {
            out << id_vec[i] << "_t" << time_ids[i];
    		out << " " << g[i].Size() << endl;

    		for (int cp=g[i].Size(); --cp >= 0;) {
    			out << id_vec[g[i][cp]] << "_t" << time_ids[i];
    			if (cp > 0) out << " ";
    		}
    		out << endl;
    	}
    }
	return true;
}

/** Add higher order neighbors up to (and including) distance.
 If cummulative true, then include lower orders as well.  Otherwise,
 only include elements on frontier. */
void Gda::MakeHigherOrdContiguity(size_t distance, size_t obs,
                                  GalElement* W,
                                  bool cummulative)
{
	using namespace std;
	if (obs < 1 || distance <=1) return;
	vector<vector<long> > X(obs);
	for (size_t i=0; i<obs; ++i) {
		vector<set<long> > n_at_d(distance+1);
		n_at_d[0].insert(i);
		for (size_t j=0, sz=W[i].Size(); j<sz; ++j) {
			n_at_d[1].insert(W[i][j]);
		}
		for (size_t d=2; d<=distance; ++d) {
			for (set<long>::const_iterator it=n_at_d[d-1].begin();
					 it!=n_at_d[d-1].end(); ++it)
			{
				for (size_t j=0, sz=W[*it].Size(); j<sz; ++j) {
					long nbr = W[*it][j];
					if (n_at_d[d-1].find(nbr) == n_at_d[d-1].end() &&
							n_at_d[d-2].find(nbr) == n_at_d[d-2].end()) {
						n_at_d[d].insert(nbr);
					}
				}
			}
		}
		size_t sz_Xi = 0;
		for (size_t d=(cummulative ? 1 : distance); d<=distance; ++d) {
			sz_Xi += n_at_d[d].size();
		}
		X[i].resize(sz_Xi);
		size_t cnt=0;
		for (size_t d=(cummulative ? 1 : distance); d<=distance; ++d) {
			for (set<long>::const_iterator it=n_at_d[d].begin();
					 it!=n_at_d[d].end(); ++it) { X[i][cnt++] = *it; }
		}
		sort(X[i].begin(), X[i].end(), greater<long>());
	}
	for (size_t i=0; i<obs; ++i) {
		W[i].SetSizeNbrs(X[i].size());
		for (size_t j=0, sz=X[i].size(); j<sz; ++j) W[i].SetNbr(j, X[i][j]);
	}
}