/*~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~\
|  Phycas: Python software for phylogenetic analysis						  |
|  Copyright (C) 2006 Mark T. Holder, Paul O. Lewis and David L. Swofford	  |
|																			  |
|  This program 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 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 General Public License for more details.								  |
|																			  |
|  You should have received a copy of the GNU General Public License along	  |
|  with this program; if not, write to the Free Software Foundation, Inc.,	  |
|  51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.				  |
\~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/

#include "square_matrix.hpp"
#include "rectangular_matrix.hpp"

#include <boost/format.hpp>
#include <iostream> //temporary!
#include <iterator> //temporary!

#include "ncl/nxsallocatematrix.h"

namespace phycas
{

unsigned RectangularMatrix::_k = 0;	//temporary!

/*----------------------------------------------------------------------------------------------------------------------
|	RectangularMatrix default constructor. Sets `_nrows', '_ncols' and `_m' data members to 0.
*/
RectangularMatrix::RectangularMatrix()
  :	 _id(++_k),  _m(0), _nrows(0), _ncols(0), _varcov(0), _dirty_mean(true), _dirty_varcov(true)
	{
	//std::cerr << "====> constructing default RectangularMatrix " << _id << " <====" << (this) << std::endl;
	}

/*----------------------------------------------------------------------------------------------------------------------
|	RectangularMatrix constructor. Creates rectangular matrix `_m' with row dimension `_nrows', column dimension
|   '_ncols', and sets all elements of `_m' to `value'.
*/
RectangularMatrix::RectangularMatrix(
  unsigned nrows,   /**< the row dimension of the matrix */
  unsigned ncols,   /**< the column dimension of the matrix */
  double value)		/**< the value to which to set all elements */
  : _id(++_k), _m(0), _nrows(nrows), _ncols(ncols), _varcov(0), _dirty_mean(true), _dirty_varcov(true)
	{
	//std::cerr << "====> constructing RectangularMatrix " << _id << " of size " << _nrows << " rows, " << _ncols << " columns, with all values set to " << value << " <====" << (this) << std::endl;
	CreateMatrix(_nrows, _ncols, value);
	}

/*----------------------------------------------------------------------------------------------------------------------
|	RectangularMatrix copy constructor. This is necessary because the standard vector resize function in some
|   implementations of the standard template library creates only one element with the default constructor, then creates
|   the remaining elements using the copy constructor.
*/
RectangularMatrix::RectangularMatrix(
  const RectangularMatrix & other)	/**< is the RectangularMatrix to copy */
  : _id(++_k), _m(0), _nrows(other._nrows), _ncols(other._ncols), _varcov(0), _dirty_mean(true), _dirty_varcov(true)
  	{
	//std::cerr << "====> copy constructing RectangularMatrix " << _id << " from " << other._id << " <====" << (this) << std::endl;
	if (_nrows > 0 && _ncols > 0)
		{
		CreateMatrix(_nrows, _ncols, 0.0);
		double * pother = &other._m[0][0];
		double * p = &_m[0][0];
		unsigned last = _nrows*_ncols;
		for (unsigned i = 0; i < last; ++i)
			*p++ = *pother++;
		}
	}

/*----------------------------------------------------------------------------------------------------------------------
|	RectangularMatrix destructor. If `_m' exists, deletes it.
*/
RectangularMatrix::~RectangularMatrix()
	{
	//std::cerr << "====> destroying RectangularMatrix " << _id << " <====" << (this) << std::endl;
	if (_m)
		DeleteTwoDArray<double>(_m);
	if (_varcov)
		delete _varcov;
	_m = 0;
	_nrows = 0;
	_ncols = 0;
	_varcov = 0;
    _dirty_mean = true;
    _dirty_varcov = true;
	_id = UINT_MAX;
	}

/*----------------------------------------------------------------------------------------------------------------------
|	Allocates memory for `_m' data member.
*/
void RectangularMatrix::CreateMatrix(
  unsigned nrows,   /**< is the row dimension of the rectangular matrix to be created */
  unsigned ncols,   /**< is the column dimension of the rectangular matrix to be created */
  double value)		/**< is the value to which each element will be set */
	{
	//std::cerr << "----> RectangularMatrix::CreateMatrix " << _id << ", _nrows = " << nrows << ", _ncols = " << ncols << ", value = " << value << " <----" << std::endl;
    _dirty_mean = true;
    _dirty_varcov = true;
    _nrows = nrows;
    _ncols = ncols;
	if (_m)
		DeleteTwoDArray<double>(_m);
	_m = 0;
	if (_varcov)
		delete _varcov;
	_varcov = 0;
	if (_nrows > 0 && _ncols > 0)
		{
		_m = NewTwoDArray<double>(_nrows, _ncols);
		Fill(value);
		}
	}

/*----------------------------------------------------------------------------------------------------------------------
|	Returns the dimensions of the data member `_m' as a std::pair<unsigned,unsigned> (number of rows, number of columns).
*/
std::pair<unsigned, unsigned> RectangularMatrix::GetDimensions() const
	{
	return std::pair<unsigned, unsigned>(_nrows, _ncols);
	}

/*----------------------------------------------------------------------------------------------------------------------
|	Returns the number of rows of the data member `_m'.
*/
unsigned RectangularMatrix::GetNRows() const
	{
	return _nrows;
	}

/*----------------------------------------------------------------------------------------------------------------------
|	Returns the number of columns of the data member `_m'.
*/
unsigned RectangularMatrix::GetNCols() const
	{
	return _ncols;
	}

/*----------------------------------------------------------------------------------------------------------------------
|	Accessor function that simply returns `_m'. This allows `_m' to be passed to functions that expect a two-dimensional
|	array of doubles, but keep in mind that this is somewhat unsafe.
*/
double * * RectangularMatrix::GetMatrixAsRawPointer() const
	{
	return _m;
	}

/*----------------------------------------------------------------------------------------------------------------------
|	Operator that allows access to this RectangularMatrix object as if it were a two-dimensional array of doubles.
*/
double * RectangularMatrix::operator[](
  unsigned row) const
	{
	PHYCAS_ASSERT(row < _nrows);
	return _m[row];
	}

/*----------------------------------------------------------------------------------------------------------------------
|	Returns element at row `i', column `'j of the matrix.
*/
double RectangularMatrix::GetElement(unsigned i, unsigned j) const
	{
	PHYCAS_ASSERT(i < _nrows);
	PHYCAS_ASSERT(j < _ncols);
    return _m[i][j];
	}

/*----------------------------------------------------------------------------------------------------------------------
|	Sets element at row `i', column `j' of the matrix to the value `v'.
*/
void RectangularMatrix::SetElement(unsigned i, unsigned j, double v)
	{
	PHYCAS_ASSERT(i < _nrows);
	PHYCAS_ASSERT(j < _ncols);
    _m[i][j] = v;
    _dirty_mean = true;
    _dirty_varcov = true;
	}

/*----------------------------------------------------------------------------------------------------------------------
|	Sets element at row `i', column `j' of the matrix to the current value plus `v'.
*/
void RectangularMatrix::AddToElement(unsigned i, unsigned j, double v)
	{
	PHYCAS_ASSERT(i < _nrows);
	PHYCAS_ASSERT(j < _ncols);
    _m[i][j] += v;
    _dirty_mean = true;
    _dirty_varcov = true;
	}

/*----------------------------------------------------------------------------------------------------------------------
|	Returns row `i' of the matrix as a vector.
*/
std::vector<double> RectangularMatrix::GetRow(unsigned i) const
	{
	PHYCAS_ASSERT(i < _nrows);
    double * first = &_m[i][0];
    double * last = first + _ncols;
    return std::vector<double>(first, last);
	}

/*----------------------------------------------------------------------------------------------------------------------
|	Sets row `i' of the matrix to the values in the vector `v'.
*/
void RectangularMatrix::SetRow(unsigned i, const std::vector<double> & v)
	{
	PHYCAS_ASSERT(i < _nrows);
	PHYCAS_ASSERT(v.size() == _ncols);
    unsigned j = 0;
    for (std::vector<double>::const_iterator it = v.begin(); it != v.end(); ++it)
        {
        double value = *it;
        _m[i][j++] = value;
        }
    _dirty_mean = true;
    _dirty_varcov = true;
	}

/*----------------------------------------------------------------------------------------------------------------------
|	Returns the entire matrix as a vector by row.
*/
std::vector<double> RectangularMatrix::GetMatrix() const
	{
    std::vector<double> v;
    double * ptr = (double *)&_m[0];
    for (unsigned i = 0; i < _nrows*_ncols; ++i)
        v.push_back(ptr[i]);
    return v;
	}

/*----------------------------------------------------------------------------------------------------------------------
|	Copies supplied vector `v' into this matrix. Vector `v' is expected to have length `nrows'*`ncols', and matrix is
|   read from `v' by row. The data member `_nrows' is set equal to `nrows', and the data member '_ncols' is set equal to
|   'ncols'.
*/
void RectangularMatrix::SetMatrix(unsigned nrows, unsigned ncols, std::vector<double> v)
	{
	PHYCAS_ASSERT(nrows*ncols == (unsigned)v.size());
    CreateMatrix(nrows, ncols, 0.0);
    double * ptr = (double *)&_m[0][0];
    for (std::vector<double>::iterator it = v.begin(); it != v.end(); ++it)
        {
        double val = *it;
        *ptr++ = val;
        }
    _dirty_mean = true;
    _dirty_varcov = true;
	}

/*----------------------------------------------------------------------------------------------------------------------
|	Fills all cells of matrix `_m' with `value'.
*/
void RectangularMatrix::Fill(
  double value) /**< is the value to which every element in matrix is to be set */
	{
    std::pair<unsigned,unsigned> dim = GetDimensions();
	unsigned last = dim.first*dim.second;
	double * p = &_m[0][0];
	for (unsigned i = 0; i < last; ++i)
		*p++ = value;
    _dirty_mean = true;
    _dirty_varcov = true;
	}

/*----------------------------------------------------------------------------------------------------------------------
|	Assumes '_nrows' > 0, '_ncols' > 0, and `_m' exists. Multiplies each element of `_m' by the supplied `scalar'.
*/
void RectangularMatrix::ScalarMultiply(
  double scalar)	/**< is the scalar value multiplied by each element */
	{
	//std::cerr << "----> RectangularMatrix::ScalarMultiply " << _id << ", scalar = " << scalar << " <----" << std::endl;
	PHYCAS_ASSERT(_nrows > 0);
	PHYCAS_ASSERT(_ncols > 0);
	unsigned last = _nrows*_ncols;
	double * p = &_m[0][0];
	for (unsigned i = 0; i < last; ++i)
		*p++ *= scalar;
    _dirty_mean = true;
    _dirty_varcov = true;
	}

/*----------------------------------------------------------------------------------------------------------------------
|	Assumes '_nrows' > 0, '_ncols' > 0, and `_m' exists. Sets each element of `_m' to the supplied `scalar'.
*/
void RectangularMatrix::SetToScalar(
  double scalar)	/**< is the scalar value to which each element is set */
	{
	//std::cerr << "----> RectangularMatrix::SetToScalar " << _id << ", scalar = " << scalar << " <----" << std::endl;
	PHYCAS_ASSERT(_nrows > 0);
	PHYCAS_ASSERT(_ncols > 0);
	unsigned last = _nrows*_ncols;
	double * p = &_m[0][0];
	for (unsigned i = 0; i < last; ++i)
		*p++ = scalar;
    _dirty_mean = true;
    _dirty_varcov = true;
	}

/*----------------------------------------------------------------------------------------------------------------------
|	Saves a representation of the matrix to the supplied string for use in debugging. The caller should supply a format
|	string suitable for representing a single value of the matrix: e.g. "%12.5f\t".
*/
void RectangularMatrix::MatrixToString(
  std::string & s,		 /**< the string to which a representation will be saved */
  std::string fmt) const /**< the format string */
	{
    std::pair<unsigned,unsigned> dim = GetDimensions();
	//s += boost::str(boost::format("This is RectangularMatrix %d\n") % _id);
	for (unsigned i = 0; i < dim.first; ++i)
		{
		for (unsigned j = 0; j < dim.second; ++j)
			{
			s += str(boost::format(fmt) % _m[i][j]);
			}
		s += '\n';
		}
	}

/*----------------------------------------------------------------------------------------------------------------------
|	Returns string representation of the matrix.
*/
std::string RectangularMatrix::GetStringRepresentation() const
	{
    std::string s;
    std::string fmt = "\t%g";
    MatrixToString(s, fmt);
    return s;
	}

/*----------------------------------------------------------------------------------------------------------------------
|	Returns string representation of the matrix.
*/
std::string RectangularMatrix::GetFormattedStringRepresentation(std::string fmt = "%g") const
	{
    std::string s;
    MatrixToString(s, fmt);
    return s;
	}

/*----------------------------------------------------------------------------------------------------------------------
|	Returns the data member `_mean'. Calls ComputeMean() first if `_mean' is not up-to-date.
*/
std::vector<double> RectangularMatrix::GetMean()
	{
    if (_dirty_mean || _mean.size() != _ncols)
        ComputeMean();
    return _mean;
	}

/*----------------------------------------------------------------------------------------------------------------------
|	Returns a copy of the data member `_varcov'. Calls ComputeVarCovMatrix() first if  `_varcov' is not up-to-date.
*/
SquareMatrix * RectangularMatrix::GetVarCovMatrix()
	{
    if (_dirty_varcov || !_varcov || _varcov->GetDimension() != _ncols)
        ComputeVarCovMatrix();
    PHYCAS_ASSERT(_varcov);
    return new SquareMatrix(*_varcov);
	}

/*----------------------------------------------------------------------------------------------------------------------
|	Recomputes the mean vector `_mean'.
*/
void RectangularMatrix::ComputeMean()
	{
    if (_nrows == 0)
        {
        _mean.clear();
        _dirty_mean = false;
        return;
        }

    // compute sample mean vector
    double n = (double)(_nrows);
    _mean.assign(_ncols, 0.0);
    for (unsigned i = 0; i < _nrows; ++i)
        {
        for (unsigned j = 0; j < _ncols; ++j)
            {
            double v = _m[i][j];
            _mean[j] += v/n;
            }
        }

    _dirty_mean = false;
	}

/*----------------------------------------------------------------------------------------------------------------------
|	Recomputes the variance-covariance matrix `_varcov'.
*/
void RectangularMatrix::ComputeVarCovMatrix()
	{
    if (_nrows == 0)
        {
        _varcov->Clear();
        }
    else if (_nrows == 1)
        {
        _varcov->SetToScalar(0.0);
        }
    else
        {
        // compute sample mean vector
        double n = (double)(_nrows);
        //_mean.assign(_ncols, 0.0);
        //std::vector<double> sample_sum(_ncols, 0.0);
        //std::vector<double> sample_ss(_ncols, 0.0);
        //for (unsigned i = 0; i < _nrows; ++i)
        //    {
        //    for (unsigned j = 0; j < _ncols; ++j)
        //        {
        //        double v = _m[i][j];
        //        _mean[j] += v/n;
        //        sample_sum[j] += v;
        //        sample_ss[j] += v*v;
        //        }
        //    }

        if (_dirty_mean)
            ComputeMean();

        // compute sample variance-covariance matrix
        if (_varcov && _varcov->GetDimension() == _ncols)
            _varcov->SetToScalar(0.0);
        else
            {
            delete _varcov;
            _varcov = new SquareMatrix(_ncols, 0.0);
            }

        for (unsigned i = 0; i < _ncols; ++i)
            {
            for (unsigned j = i; j < _ncols; ++j)
                {
                for (unsigned k = 0; k < _nrows; ++k)
                    {
                    double tmp = (_m[k][i] - _mean[i])*(_m[k][j] - _mean[j])/(n - 1.0);
                    _varcov->AddToElement(i, j, tmp);
                    }
                if (j != i)
                    {
                    double tmp = _varcov->GetElement(i, j);
                    _varcov->SetElement(j, i, tmp);
                    }
                }
            }
        }
    _dirty_varcov = false;
	}

} // namespace phycas


/// @file  embed_from_ref.cpp
/// @brief Embed from an embedded subset

#include <boost/program_options.hpp>
#include <ogt/ogt.hpp>
#include <future>
#include <iomanip>
#include <iostream>
#include <thread>
#include <vector>

using namespace boost::program_options;
using dlib::find_min;
using dlib::newton_search_strategy;
using dlib::bfgs_search_strategy;
using dlib::objective_delta_stop_strategy;
using Eigen::Index;
using Eigen::MatrixXd;
using Eigen::SparseMatrix;
using Eigen::VectorXd;
using ogt::core::AB_LT_AC;
using ogt::core::Collection;
using ogt::core::createCosinePosOracle;
using ogt::core::createEuclideanPosOracle;
using ogt::core::createJaccardPosOracle;
using ogt::core::Oracle;
using ogt::embed::dlib_matrix;
using ogt::embed::dlib_vector;
using ogt::embed::dlib_to_eigen_matrix;
using ogt::embed::eigen_to_dlib_matrix;
using ogt::embed::dlib_to_eigen_vector;
using ogt::embed::eigen_to_dlib_vector;
using ogt::embed::CmpConstraint;
using ogt::embed::embedCmpWithSOE;
using ogt::embed::EmbedConfig;
using ogt::embed::EmbedErr;
using ogt::embed::EmbedResult;
using ogt::embed::sphere_intersection_margin;
using ogt::io::isSparseFile;
using ogt::io::loadDenseMatrix;
using ogt::io::loadSparseMatrix;
using ogt::io::openSink;
using ogt::io::saveEmbedding;
using ogt::linalg::DenseNZIterator;
using ogt::linalg::posToDist;
using ogt::util::random_nchoosek;
using ogt::util::random_subset;
using std::async;
using std::cerr;
using std::cout;
using std::endl;
using std::future;
using std::make_shared;
using std::map;
using std::min;
using std::set;
using std::shared_ptr;
using std::string;
using std::stringstream;
using std::vector;

std::ostream& log() {
	std::time_t t = std::time(nullptr);
    std::tm tm = *std::localtime(&t);
    cerr << std::put_time(&tm, "%c %Z") << ' ';
    return cerr;
}

const char * DIST_EUCLIDEAN = "EUCLIDEAN";
const char * DIST_COSINE = "COSINE";
const char * DIST_JACCARD = "JACCARD";

struct pt_pos {
	pt_pos(size_t pt, VectorXd pos, vector<CmpConstraint> cmps)
		: pt(pt), pos(pos), cmps(cmps), createdAt(clock()) {}
	const size_t pt;
	const VectorXd pos;
	const vector<CmpConstraint> cmps;
	const clock_t createdAt;
};

struct idx_dist {
	idx_dist(size_t idx, double dist) : idx(idx), dist(dist) {}
	size_t idx;
	double dist;
};

struct fft_idx {
	fft_idx(size_t idx, double dist) : idx(idx), dist(dist) {}

	void findDists(const vector<size_t>& subset, const MatrixXd& refD) {
		// Prepare the sorted distance vector
		dists.clear();
		dists.reserve(refD.rows());
		for (Index i = 0; i < refD.cols(); i++) {
			dists.emplace_back(subset[i], refD(idx, i));
		}
		std::sort(dists.begin(), dists.end(), [](const auto& a, const auto& b) {
			return a.dist < b.dist;
		});
	}

	size_t idx;
	double dist;
	vector<idx_dist> dists;
};

struct pt_shell {
	pt_shell() : center(0), minDist(0), maxDist(0) {}
	size_t center;
	double minDist;
	double maxDist;
};

/// Find a farthest-first traversal of a set
vector<fft_idx> fft(const vector<size_t>& subset, const MatrixXd& refD) {
	vector<fft_idx> order;
	size_t nObj = refD.rows();
	order.reserve(nObj);

	// Find the first point, on the convex hull
	VectorXd dist = refD.row(0);
	fft_idx choice(0, 0);
	for (size_t i = 1; i < nObj; i++) {
		if (dist(i) > choice.dist) {
			choice.idx = i;
			choice.dist = dist(i);
		}
	}
	choice.findDists(subset, refD);
	order.push_back(choice);
	vector<fft_idx> minDist;
	for (size_t i = 0; i < nObj; i++) {
		if (i != choice.idx) {
			minDist.emplace_back(i, refD(choice.idx, i));
		}
	}

	// Iterate through the other points until done
	while (!minDist.empty()) {
		std::sort(minDist.begin(), minDist.end(), [](const auto& a, const auto& b) {
			return a.dist < b.dist;
		});
		choice = minDist.back();
		choice.findDists(subset, refD);
		minDist.pop_back();
		order.push_back(choice);
		for (auto& ptd : minDist) {
			ptd.dist = std::min(ptd.dist, refD(choice.idx, ptd.idx));
		}
	}

	return order;
}

/// Tries to use the suggested anchor and pick distance bounds to the point.
bool find_anchor(size_t pt, const fft_idx& ptd,
	const vector<size_t>& subset, shared_ptr<Oracle> oracle,
	pt_shell& anchor, vector<CmpConstraint>& cmps) {
	anchor.center = ptd.idx;
	size_t center = subset[ptd.idx];

	// Make sure the point can be compared to the proposed anchor
	if ((*oracle)(center, ptd.dists.front().idx, pt) != AB_LT_AC ||
		(*oracle)(center, pt, ptd.dists.back().idx) != AB_LT_AC) {
		return false;
	}

	// Find the point in the vector
	auto ref = ptd.dists.begin();
	auto pre = ref;
	for (; (*oracle)(center, ref->idx, pt) == AB_LT_AC; ref = next(ref)) {
		// cmps.emplace_back(center, ref->idx, pt);
		pre = ref;
		anchor.minDist = ref->dist;
	}
	for (; ref != ptd.dists.end()
		&& (*oracle)(center, pt, ref->idx) != AB_LT_AC; ref = next(ref)) {}
	if (ref == ptd.dists.end()) {
		return false;
	}
	anchor.maxDist = ref->dist;
	cmps.emplace_back(center, pre->idx, pt);
	cmps.emplace_back(center, pt, ref->idx);
	// auto post = std::lower_bound(ptd.dists.begin(), ptd.dists.end(), pt,
	// 	[&](const idx_dist& ref, size_t pt) {
	// 		return (*oracle)(center, ref.idx, pt) == AB_LT_AC;
	// 	});
	// if (post == ptd.dists.begin() || post == ptd.dists.end()) {
	// 	return false;
	// }
	// cmps.emplace_back(center, pt, ref->idx);
	// anchor.maxDist = post->dist;

	// auto pre = prev(post);
	// for (; (*oracle)(center, pre->idx, pt) != AB_LT_AC; pre = prev(pre)) {}
	// anchor.minDist = pre->dist;
	// cmps.emplace_back(center, pre->idx, pt);
	// cmps.emplace_back(center, pt, post->idx);
	return true;
}

/// The least-squares objective for sphere_intersection_opt
struct opt_objective {
	opt_objective(const MatrixXd& center, const VectorXd& radius,
		const VectorXd& margin)
		: center(center), radius(radius), margin(margin) {}
	const MatrixXd& center;
	const VectorXd& radius;
	const VectorXd& margin;

	double operator()(const dlib_vector& dy) const {
		double res = 0;
		VectorXd y = dlib_to_eigen_vector(dy);
		for (Index j = 0; j < center.cols(); j++) {
			double val = (y - center.col(j)).norm() - radius(j);
			if (fabs(val) > margin(j)) {
				res += val * val;
			}
		}
		return res;
	}
};

/// The gradient of the least-squares objective for sphere_intersection_opt
struct opt_objective_gradient {
	opt_objective_gradient(const MatrixXd& center, const VectorXd& radius,
		const VectorXd& margin)
		: center(center), radius(radius), margin(margin) {}
	const MatrixXd& center;
	const VectorXd& radius;
	const VectorXd& margin;

	dlib_vector operator()(const dlib_vector& dy) const {
		VectorXd y = dlib_to_eigen_vector(dy);
		VectorXd res = VectorXd::Zero(y.size());
		for (Index j = 0; j < center.cols(); j++) {
			VectorXd diff = y - center.col(j);
			double dist = diff.norm();
			if (fabs(dist - radius(j)) > margin(j)) {
				res += ((dist - radius(j)) / dist) * diff;
			}
		}
		return eigen_to_dlib_vector(2 * res);
	}
};

// /// The Hessian of the least-squares objective for sphere_intersection_opt
// struct opt_objective_hessian {
// 	opt_objective_hessian(const MatrixXd& center, const VectorXd& radius)
// 		: center(center), radius(radius) {}
// 	const MatrixXd& center;
// 	const VectorXd& radius;

// 	dlib_matrix operator()(const dlib_vector& dy) const {
// 		VectorXd y = dlib_to_eigen_vector(dy);
// 		MatrixXd res = MatrixXd::Zero(dy.size(), dy.size());
// 		for (Index j = 0; j < center.cols(); j++) {
// 			VectorXd diff = y - center.col(j);
// 			double dist = diff.norm();
// 			double sqdist = diff.squaredNorm();
// 			double scale = (1 - ((dist - radius(j)) / dist)) / sqdist;
// 			for (Index i = 0; i < center.rows(); i++) {
// 				double val1 = y(i) - center(i,j);
// 				for (Index k = 0; k <= i; k++) {
// 					res(i,k) += val1 * diff(k) * scale;
// 					res(k,i) = res(i,k);
// 				}
// 			}
// 		}
// 		return 2 * eigen_to_dlib_matrix(res);
// 	}
// };

/// Embed a point based on shell intersection
VectorXd shell_intersection(const Eigen::MatrixXd &center,
	const Eigen::VectorXd& radius, const Eigen::VectorXd& margin, double /*eps*/,
	double min_delta, size_t max_iter, bool verbose) {

	// Using sphere intersection plus margin
	// auto locs = sphere_intersection_margin(centers, radii, thickness,
	// 	eps, minDelta, maxIter, verbose);
	// return locs[0];

	// Direct method
	const Index nDim = center.rows();
	dlib_vector dx = eigen_to_dlib_vector(VectorXd::Random(nDim));
	objective_delta_stop_strategy stopper =
		objective_delta_stop_strategy(min_delta, max_iter);
	if (verbose) {
		stopper = stopper.be_verbose();
	}
	try {
		find_min(
			// newton_search_strategy() may produce NaN in some cases?
			// newton_search_strategy(opt_objective_hessian(center, radius)),
			bfgs_search_strategy(),
			stopper,
			opt_objective(center, radius, margin),
			opt_objective_gradient(center, radius, margin),
			dx,
			0);
	} catch (dlib::error err) {
		throw EmbedErr(err.what());
	}
	return dlib_to_eigen_vector(dx);
}

/// Run a single embedding, perhaps in a thread pool.
vector<pt_pos> embed(vector<size_t> pts, size_t nDim, size_t numAnchors,
	const MatrixXd& refX,
	const vector<size_t>& subset, const vector<fft_idx>& refFFT,
	shared_ptr<Oracle> oracle) {
	vector<pt_pos> result;
	for (size_t pt : pts) {

		// Identify 2(d+1) ref. points close to pt in FFT order
		if (numAnchors == 0) {
			numAnchors = 2 * (nDim + 1);
		}
		size_t nextAnchor = 0;
		MatrixXd centers(nDim, numAnchors);
		VectorXd radii(numAnchors);
		VectorXd thickness(numAnchors);
		vector<CmpConstraint> cmps;
		for (const auto& ptd : refFFT) {
			pt_shell anchor;
			if (find_anchor(pt, ptd, subset, oracle, anchor, cmps)) {
				centers.col(nextAnchor) = refX.row(ptd.idx);
				radii(nextAnchor) = (anchor.minDist + anchor.maxDist) / 2;
				thickness(nextAnchor) = (anchor.maxDist - anchor.minDist) / 2;

				nextAnchor++;
				if (nextAnchor >= numAnchors) break;
			}
		}

		// Now embed the point using the spherical shell intersection
		try {
			const double eps = 1e-6;
			const double minDelta = 1e-9;
			const size_t maxIter = 1000;
			const bool verbose = false;
			auto loc = shell_intersection(centers, radii, thickness, eps, minDelta,
				maxIter, verbose);
			cerr << "Done embedding " << pt << endl;
			result.emplace_back(pt, loc, cmps);
		} catch(std::invalid_argument e) {
			log() << "Could not embed " << pt << ": " << e.what() << endl;
			continue;
		} catch (ogt::embed::EmbedErr e) {
			log() << "Could not embed " << pt << ": " << e.what() << endl;
			continue;
		} catch(...) {
			log() << "Could not embed " << pt << ": unknown exception" << endl;
			continue;
		}
	}
	return result;
}

/// Run the program.
int run_embed_from_ref(string posPath, string refPath, size_t numToEmbed,
	string distType, string soeCmpPath, string embPath, string orderPath,
	size_t numAnchors, double noise) {

	// Prepare to save the output order
	const clock_t started = clock();
	auto fOrder = openSink(orderPath);
	if (!fOrder) {
		log() << "Invalid --order path: could not open " << orderPath << endl;
		return 1;
	}
	auto saveOrder = [&](const pt_pos& pt) {
		(*fOrder) << static_cast<float>(pt.createdAt - started) / CLOCKS_PER_SEC
			<< "," << pt.pt << endl;
	};

	// Load the true position matrix and create a distance oracle
	log() << "Loading position matrix" << endl;
	MatrixXd truePos;
	Eigen::SparseMatrix<double, Eigen::RowMajor> spTruePos;
	shared_ptr<Oracle> oracle;
	size_t nObj, trueDim;
	if (isSparseFile(posPath)) {
		spTruePos = loadSparseMatrix(posPath);
		if (distType == DIST_EUCLIDEAN) {
			oracle = createEuclideanPosOracle(spTruePos);
		} else if (distType == DIST_COSINE) {
			oracle = createCosinePosOracle(spTruePos);
		} else if (distType == DIST_JACCARD) {
			oracle = createJaccardPosOracle<SparseMatrix<double, Eigen::RowMajor>,
				Eigen::SparseVector<double>,
				Eigen::SparseVector<double>::InnerIterator>(spTruePos);
		} else {
			log() << "Invalid distance function: " << distType << endl;
			return 0;
		}
		nObj = spTruePos.rows();
		trueDim = spTruePos.cols();
	} else {
		truePos = loadDenseMatrix(posPath);
		if (distType == DIST_EUCLIDEAN) {
			oracle = createEuclideanPosOracle(truePos);
		} else if (distType == DIST_COSINE) {
			oracle = createCosinePosOracle(truePos);
		} else if (distType == DIST_JACCARD) {
			oracle = createJaccardPosOracle<MatrixXd, VectorXd,
				DenseNZIterator<VectorXd>>(truePos);
		} else {
			log() << "Invalid distance function: " << distType << endl;
			return 1;
		}
		nObj = truePos.rows();
		trueDim = truePos.cols();
	}
	log() << "Position matrix is " << nObj << "x" << trueDim << endl;
	if (nObj < 1) {
		log() << "Error: empty position matrix from " << posPath << endl;
		return 1;
	}
	if (noise > 0) {
		oracle = createUniformNoisyOracle(oracle, noise);
	}

	// Load the reference embedding
	log() << "Loading reference embedding" << endl;
	VectorXd objects;
	MatrixXd refX;
	try {
		auto emb = loadDenseMatrix(refPath);
		objects = emb.col(0);
		refX = emb.rightCols(emb.cols() - 1);
	} catch(std::runtime_error e) {
		log() << e.what() << endl;
		return 1;
	}
	const size_t nDim = refX.cols();
	log() << "Reference embedding is " << refX.rows() << "x" << nDim << endl;

	log() << "Finding distances for reference embedding" << endl;
	MatrixXd refD = posToDist(refX);

	// Load the SOE comparisons
	vector<CmpConstraint> cmps;
	if (!soeCmpPath.empty()) {
		auto soeCmp = loadDenseMatrix(soeCmpPath);
		for (Index ii = 0; ii < soeCmp.rows(); ii++) {
			cmps.emplace_back(soeCmp(ii, 0), soeCmp(ii, 1), soeCmp(ii, 2));
		}
	}

	// Construct the collection
	vector<size_t> subset;
	map<size_t, size_t> refIdx;
	for (Index i = 0; i < objects.size(); i++) {
		subset.push_back(objects(i));
		refIdx[objects(i)] = i;
	}
	log() << "Running FFT" << endl;
	vector<fft_idx> refFFT = fft(subset, refD);

	// Choose points for the final embedding
	// numToEmbed = min(nObj - subset.size(), numToEmbed);
	// numToEmbed = subset.size();
	numToEmbed = 0; // embed all points
	vector<size_t> embedded(subset);
	map<size_t, size_t> embIdx(refIdx);
	MatrixXd embX(refX);
	while (embedded.size() < nObj) {
		numToEmbed *= 2;
		numToEmbed = min(nObj - embedded.size(), numToEmbed);
		vector<size_t> embedPts;
		if (numToEmbed > 0 && numToEmbed < nObj - embedded.size()) {
			vector<size_t> options;
			for (size_t pt = 0; pt < nObj; pt++) {
				if (embIdx.find(pt) == embIdx.end()) {
					options.push_back(pt);
				}
			}
			auto ss = random_subset<size_t>(options.begin(), options.end(), numToEmbed);
			embedPts.insert(embedPts.begin(), ss.begin(), ss.end());
			embedPts.insert(embedPts.begin(), embedded.begin(), embedded.end());
			sort(embedPts.begin(), embedPts.end());
		} else {
			embedPts.resize(nObj);
			iota(embedPts.begin(), embedPts.end(), 0);
		}

		// Embed each point which is not in the reference
		log() << "Embedding " << embedPts.size() << " / " << nObj << " points" << endl;
		MatrixXd Xhat = MatrixXd::Random(embedPts.size(), nDim);
		vector<future<vector<pt_pos>>> futures;
		map<size_t, size_t> ptIdx;
		const size_t batch = std::thread::hardware_concurrency();
		auto from = embedPts.begin();
		auto to = from;
		for (size_t ib = 0; ib < batch; ib++) {
			from = to;
			if (ib == batch - 1) {
				to = embedPts.end();
			} else {
				to = next(from, embedPts.size() / batch);
			}
			vector<size_t> points;
			for (auto it = from; it != to; it++) {
				ptIdx[*it] = distance(embedPts.begin(), it);
				auto fit = embIdx.find(*it);
				if (fit == embIdx.end()) {
					points.push_back(*it);
				} else {
					// Save reference point positions
					Xhat.row(ptIdx[*it]) = embX.row(fit->second);
				}
			}
			futures.push_back(async(std::launch::async, embed, points, nDim,
				numAnchors, refX, subset, refFFT, oracle));
		}
		for (auto& future : futures) {
			for (const pt_pos& result : future.get()) {
				try {
					log() << "Saved " << result.pt << endl;
					Xhat.row(ptIdx[result.pt]) = result.pos;
					cmps.insert(cmps.end(), result.cmps.begin(), result.cmps.end());
					saveOrder(result);
				} catch(...) {
					// skip it, sad
				}
			}
		}

		if (numToEmbed > 0 && !soeCmpPath.empty()) {

			// Use SOE to finish the embedding
			log() << "Saving embedding" << endl;
			stringstream ss;
			ss << embPath << "_" << numToEmbed << ".pre.csv";
			saveEmbedding(Xhat, embedPts, ss.str());

			// Convert point indexes for the triples
			vector<CmpConstraint> localCmps(cmps);
			stringstream sscmp;
			sscmp << embPath << "_" << numToEmbed << ".cmps.csv";
			auto cmpFile = openSink(sscmp.str());
			for (auto& cmp : localCmps) {
				(*cmpFile) << cmp.a << "," << cmp.b << "," << cmp.c << endl;
				cmp.a = ptIdx[cmp.a];
				cmp.b = ptIdx[cmp.b];
				cmp.c = ptIdx[cmp.c];
				cmp.d = ptIdx[cmp.d];
			}
			cmpFile.reset();

			log() << "Using as " << Xhat.rows() << " x " << Xhat.cols()
				<< " initialization for SOE embedding" << endl;
			EmbedConfig config;
			config.nDim = nDim;
			config.minDelta = 1e-12;
			config.maxIter = 1e4;
			config.verbose = true;
			config.margin = 1e-2;
			auto result = embedCmpWithSOE(localCmps, Xhat, config);
			log() << "SOE embedding for " << numToEmbed << " achieved loss " << result.loss << endl;
			Xhat = result.X;

			auto Xrand = MatrixXd::Random(Xhat.rows(), Xhat.cols());
			auto result2 = embedCmpWithSOE(localCmps, Xrand, config);
			log() << "SOE for " << numToEmbed << " from random loss: " << result2.loss
				<< " init: " << result.loss << endl;
		}

		// Save the embedding.
		log() << "Saving embedding" << endl;
		// stringstream ss2;
		// ss2 << embPath << "_" << numToEmbed << ".post.csv";
		// saveEmbedding(Xhat, embedPts, ss2.str());
		saveEmbedding(Xhat, embedPts, embPath);

		// Prepare for the next round
		embedded = embedPts;
		embIdx = ptIdx;
		embX = Xhat;
	}

	log() << "Done" << endl;
	return 0;
}

/// Program entry point.
int main(int argc, char * argv[]) {
	try {
		stringstream distDesc;
		distDesc << "Distance function: " << DIST_EUCLIDEAN << ", " << DIST_COSINE << ", or " << DIST_JACCARD;
		options_description desc{"eval_embedding options"};
		desc.add_options()
			("help,h", "Show this usage message")
			("posfile", value<string>(), "Input file containing dataset")
			("ref", value<string>(), "Input embedding of reference subset")
			("num_pts", value<size_t>()->default_value(0), "Embed at most this many randomly-selected points")
			("dist", value<string>()->default_value("EUCLIDEAN"), distDesc.str().c_str())
			("soe_cmp", value<string>()->default_value(""), "Use SOE with the specified triples after the shell embedding")
			("embedding", value<string>(), "Output file for the embedding")
			("order", value<string>(), "Output file for the point embedding order and timing")
			("anchors", value<size_t>()->default_value(0), "Number of anchors to use")
			("noise", value<double>()->default_value(0), "Prob. of flipping a comparison outcome")
			;

		variables_map vm;
		store(parse_command_line(argc, argv, desc), vm);
		notify(vm);

		bool valid = true;
		if (vm.count("help")) {
			cerr << desc << endl;
			valid = false;
		}
		if (!vm.count("posfile")) {
			cerr << "--posfile is required" << endl;
			valid = false;
		}
		if (!vm.count("ref")) {
			cerr << "--ref is required" << endl;
			valid = false;
		}
		if (!vm.count("embedding")) {
			cerr << "--embedding is required" << endl;
			valid = false;
		}
		if (!vm.count("order")) {
			cerr << "--order is required" << endl;
			valid = false;
		}
		if (valid) {
			return run_embed_from_ref(
				vm["posfile"].as<string>(),
				vm["ref"].as<string>(),
				vm["num_pts"].as<size_t>(),
				vm["dist"].as<string>(),
				vm["soe_cmp"].as<string>(),
				vm["embedding"].as<string>(),
				vm["order"].as<string>(),
				vm["anchors"].as<size_t>(),
				vm["noise"].as<double>());
		}
	} catch (const error &ex) {
		cerr << ex.what() << endl;
	}
	return 1;
}


#include <boost/archive/text_iarchive.hpp>
#include <boost/archive/text_oarchive.hpp>
#include <boost/mpi/collectives/gather.hpp>
#include <boost/mpi/communicator.hpp>
#include <boost/mpi/environment.hpp>
#include <fstream>
#include <iostream>
#include <stdexcept>
#include <vector>

template <typename T> class Centroid {
public:
  T _mean;
  T _weight;
  T _meansq;
  Centroid() : Centroid(0.0, 0.0, 0.0) {}
  Centroid(T mean) : Centroid(mean, 1.0, mean * mean) {}
  Centroid(T mean, T weight, T meansq)
      : _mean(mean), _weight(weight), _meansq(meansq) {}

  inline T mean() const { return _mean; }
  inline T weight() const  { return _weight; }
  inline T meansq() const { return _meansq; }
  inline void add(const Centroid<T> &c) {
    _weight += c._weight;
    _mean += c._weight * (c._mean - _mean) / _weight;
    _meansq += c._meansq;
  }
};

std::ostream &operator<<(std::ostream &stream, const Centroid<float> &centroid) {
  stream << "(" << centroid.mean() << ", " << centroid.weight() << ", "
         << centroid.meansq() << ")\n";
  return stream;
}

namespace boost {
namespace serialization {
template <class A>
void serialize(A &ar, Centroid<float> &c, const unsigned int version) {
  ar &c._mean;
  ar &c._weight;
  ar &c._meansq;
}
} // namespace serialization
} // namespace boost

namespace bmpi = boost::mpi;

class Distributor {
  const bmpi::environment env;

public:
  const bmpi::communicator world;
  const int rank;
  const int size;
  inline bool isMaster() { return rank == 0; }
  Distributor(int argc, char **argv)
      : env(argc, argv), world(), rank(world.rank()), size(world.size()) {}
};

int main(int argc, char **argv) {
  Distributor dist(argc, argv);

  Centroid<float> centroid(dist.rank * 10.0);

  if (dist.isMaster()) {
    std::vector<Centroid<float>> list;
    bmpi::gather(dist.world, centroid, list, 0);
    for (auto c : list) {
      std::cout << c;
    }
  } else {
    bmpi::gather(dist.world, centroid, 0);
  }
}


/////////////////////////////////////////////////////////////////////////////
//
// (C) Copyright Ion Gaztanaga  2007-2014
//
// Distributed under the Boost Software License, Version 1.0.
//    (See accompanying file LICENSE_1_0.txt or copy at
//          http://www.boost.org/LICENSE_1_0.txt)
//
// See http://www.boost.org/libs/intrusive for documentation.
//
/////////////////////////////////////////////////////////////////////////////
// The implementation of splay trees is based on the article and code published
// in C++ Users Journal "Implementing Splay Trees in C++" (September 1, 2005).
//
// The splay code has been modified and (supposedly) improved by Ion Gaztanaga.
//
// Here is the copyright notice of the original file containing the splay code:
//
//  splay_tree.h -- implementation of a STL compatible splay tree.
//
//  Copyright (c) 2004 Ralf Mattethat
//
//  Permission to copy, use, modify, sell and distribute this software
//  is granted provided this copyright notice appears in all copies.
//  This software is provided "as is" without express or implied
//  warranty, and with no claim as to its suitability for any purpose.
//
/////////////////////////////////////////////////////////////////////////////

#ifndef BOOST_INTRUSIVE_SPLAYTREE_ALGORITHMS_HPP
#define BOOST_INTRUSIVE_SPLAYTREE_ALGORITHMS_HPP

#include <boost/intrusive/detail/config_begin.hpp>
#include <boost/intrusive/intrusive_fwd.hpp>
#include <boost/intrusive/detail/assert.hpp>
#include <boost/intrusive/detail/algo_type.hpp>
#include <boost/intrusive/detail/uncast.hpp>
#include <boost/intrusive/bstree_algorithms.hpp>

#include <cstddef>

#if defined(BOOST_HAS_PRAGMA_ONCE)
#  pragma once
#endif

namespace boost {
namespace intrusive {

/// @cond
namespace detail {

template<class NodeTraits>
struct splaydown_assemble_and_fix_header
{
   typedef typename NodeTraits::node_ptr node_ptr;

   splaydown_assemble_and_fix_header(const node_ptr & t, const node_ptr & header, const node_ptr &leftmost, const node_ptr &rightmost)
      : t_(t)
      , null_node_(header)
      , l_(null_node_)
      , r_(null_node_)
      , leftmost_(leftmost)
      , rightmost_(rightmost)
   {}

   ~splaydown_assemble_and_fix_header()
   {
      this->assemble();

      //Now recover the original header except for the
      //splayed root node.
      //"t_" is the current root and "null_node_" is the header node
      NodeTraits::set_parent(null_node_, t_);
      NodeTraits::set_parent(t_, null_node_);
      //Recover leftmost/rightmost pointers
      NodeTraits::set_left (null_node_, leftmost_);
      NodeTraits::set_right(null_node_, rightmost_);
   }

   private:

   void assemble()
   {
      //procedure assemble;
      //    left(r), right(l) := right(t), left(t);
      //    left(t), right(t) := right(null), left(null);
      //end assemble;
      {  //    left(r), right(l) := right(t), left(t);

         node_ptr const old_t_left  = NodeTraits::get_left(t_);
         node_ptr const old_t_right = NodeTraits::get_right(t_);
         NodeTraits::set_right(l_, old_t_left);
         NodeTraits::set_left (r_, old_t_right);
         if(old_t_left){
            NodeTraits::set_parent(old_t_left, l_);
         }
         if(old_t_right){
            NodeTraits::set_parent(old_t_right, r_);
         }
      }
      {  //    left(t), right(t) := right(null), left(null);
         node_ptr const null_right = NodeTraits::get_right(null_node_);
         node_ptr const null_left  = NodeTraits::get_left(null_node_);
         NodeTraits::set_left (t_, null_right);
         NodeTraits::set_right(t_, null_left);
         if(null_right){
            NodeTraits::set_parent(null_right, t_);
         }
         if(null_left){
            NodeTraits::set_parent(null_left, t_);
         }
      }
   }

   public:
   node_ptr t_, null_node_, l_, r_, leftmost_, rightmost_;
};

}  //namespace detail {
/// @endcond

//!   A splay tree is an implementation of a binary search tree. The tree is
//!   self balancing using the splay algorithm as described in
//!
//!      "Self-Adjusting Binary Search Trees
//!      by Daniel Dominic Sleator and Robert Endre Tarjan
//!      AT&T Bell Laboratories, Murray Hill, NJ
//!      Journal of the ACM, Vol 32, no 3, July 1985, pp 652-686
//!
//! splaytree_algorithms is configured with a NodeTraits class, which encapsulates the
//! information about the node to be manipulated. NodeTraits must support the
//! following interface:
//!
//! Typedefs:
//!
//! <tt>node</tt>: The type of the node that forms the binary search tree
//!
//! <tt>node_ptr</tt>: A pointer to a node
//!
//! <tt>const_node_ptr</tt>: A pointer to a const node
//!
//! Static functions:
//!
//! <tt>static node_ptr get_parent(const_node_ptr n);</tt>
//!
//! <tt>static void set_parent(node_ptr n, node_ptr parent);</tt>
//!
//! <tt>static node_ptr get_left(const_node_ptr n);</tt>
//!
//! <tt>static void set_left(node_ptr n, node_ptr left);</tt>
//!
//! <tt>static node_ptr get_right(const_node_ptr n);</tt>
//!
//! <tt>static void set_right(node_ptr n, node_ptr right);</tt>
template<class NodeTraits>
class splaytree_algorithms
   #ifndef BOOST_INTRUSIVE_DOXYGEN_INVOKED
   : public bstree_algorithms<NodeTraits>
   #endif
{
   /// @cond
   private:
   typedef bstree_algorithms<NodeTraits> bstree_algo;
   /// @endcond

   public:
   typedef typename NodeTraits::node            node;
   typedef NodeTraits                           node_traits;
   typedef typename NodeTraits::node_ptr        node_ptr;
   typedef typename NodeTraits::const_node_ptr  const_node_ptr;

   //! This type is the information that will be
   //! filled by insert_unique_check
   typedef typename bstree_algo::insert_commit_data insert_commit_data;

   public:
   #ifdef BOOST_INTRUSIVE_DOXYGEN_INVOKED
   //! @copydoc ::boost::intrusive::bstree_algorithms::get_header(const const_node_ptr&)
   static node_ptr get_header(const const_node_ptr & n);

   //! @copydoc ::boost::intrusive::bstree_algorithms::begin_node
   static node_ptr begin_node(const const_node_ptr & header);

   //! @copydoc ::boost::intrusive::bstree_algorithms::end_node
   static node_ptr end_node(const const_node_ptr & header);

   //! @copydoc ::boost::intrusive::bstree_algorithms::swap_tree
   static void swap_tree(const node_ptr & header1, const node_ptr & header2);

   //! @copydoc ::boost::intrusive::bstree_algorithms::swap_nodes(const node_ptr&,const node_ptr&)
   static void swap_nodes(const node_ptr & node1, const node_ptr & node2);

   //! @copydoc ::boost::intrusive::bstree_algorithms::swap_nodes(const node_ptr&,const node_ptr&,const node_ptr&,const node_ptr&)
   static void swap_nodes(const node_ptr & node1, const node_ptr & header1, const node_ptr & node2, const node_ptr & header2);

   //! @copydoc ::boost::intrusive::bstree_algorithms::replace_node(const node_ptr&,const node_ptr&)
   static void replace_node(const node_ptr & node_to_be_replaced, const node_ptr & new_node);

   //! @copydoc ::boost::intrusive::bstree_algorithms::replace_node(const node_ptr&,const node_ptr&,const node_ptr&)
   static void replace_node(const node_ptr & node_to_be_replaced, const node_ptr & header, const node_ptr & new_node);

   //! @copydoc ::boost::intrusive::bstree_algorithms::unlink(const node_ptr&)
   static void unlink(const node_ptr & node);

   //! @copydoc ::boost::intrusive::bstree_algorithms::unlink_leftmost_without_rebalance
   static node_ptr unlink_leftmost_without_rebalance(const node_ptr & header);

   //! @copydoc ::boost::intrusive::bstree_algorithms::unique(const const_node_ptr&)
   static bool unique(const const_node_ptr & node);

   //! @copydoc ::boost::intrusive::bstree_algorithms::size(const const_node_ptr&)
   static std::size_t size(const const_node_ptr & header);

   //! @copydoc ::boost::intrusive::bstree_algorithms::next_node(const node_ptr&)
   static node_ptr next_node(const node_ptr & node);

   //! @copydoc ::boost::intrusive::bstree_algorithms::prev_node(const node_ptr&)
   static node_ptr prev_node(const node_ptr & node);

   //! @copydoc ::boost::intrusive::bstree_algorithms::init(const node_ptr&)
   static void init(const node_ptr & node);

   //! @copydoc ::boost::intrusive::bstree_algorithms::init_header(const node_ptr&)
   static void init_header(const node_ptr & header);

   #endif   //#ifdef BOOST_INTRUSIVE_DOXYGEN_INVOKED

   //! @copydoc ::boost::intrusive::bstree_algorithms::erase(const node_ptr&,const node_ptr&)
   //! Additional notes: the previous node of z is splayed to speed up range deletions.
   static void erase(const node_ptr & header, const node_ptr & z)
   {
      //posibility 1
      if(NodeTraits::get_left(z)){
         splay_up(bstree_algo::prev_node(z), header);
      }

      //possibility 2
      //if(NodeTraits::get_left(z)){
      //   node_ptr l = NodeTraits::get_left(z);
      //   splay_up(l, header);
      //}

      //if(NodeTraits::get_left(z)){
      //   node_ptr l = bstree_algo::prev_node(z);
      //   splay_up_impl(l, z);
      //}

      //possibility 4
      //splay_up(z, header);

      bstree_algo::erase(header, z);
   }

   #ifdef BOOST_INTRUSIVE_DOXYGEN_INVOKED
   //! @copydoc ::boost::intrusive::bstree_algorithms::clone(const const_node_ptr&,const node_ptr&,Cloner,Disposer)
   template <class Cloner, class Disposer>
   static void clone
      (const const_node_ptr & source_header, const node_ptr & target_header, Cloner cloner, Disposer disposer);

   //! @copydoc ::boost::intrusive::bstree_algorithms::clear_and_dispose(const node_ptr&,Disposer)
   template<class Disposer>
   static void clear_and_dispose(const node_ptr & header, Disposer disposer);

   #endif   //#ifdef BOOST_INTRUSIVE_DOXYGEN_INVOKED
   //! @copydoc ::boost::intrusive::bstree_algorithms::count(const const_node_ptr&,const KeyType&,KeyNodePtrCompare)
   //! Additional notes: an element with key `key` is splayed.
   template<class KeyType, class KeyNodePtrCompare>
   static std::size_t count
      (const node_ptr & header, const KeyType &key, KeyNodePtrCompare comp)
   {
      std::pair<node_ptr, node_ptr> ret = equal_range(header, key, comp);
      std::size_t n = 0;
      while(ret.first != ret.second){
         ++n;
         ret.first = next_node(ret.first);
      }
      return n;
   }

   //! @copydoc ::boost::intrusive::bstree_algorithms::count(const const_node_ptr&,const KeyType&,KeyNodePtrCompare)
   //! Additional note: no splaying is performed
   template<class KeyType, class KeyNodePtrCompare>
   static std::size_t count
      (const const_node_ptr & header, const KeyType &key, KeyNodePtrCompare comp)
   {  return bstree_algo::count(header, key, comp);  }

   //! @copydoc ::boost::intrusive::bstree_algorithms::lower_bound(const const_node_ptr&,const KeyType&,KeyNodePtrCompare)
   //! Additional notes: the first node of the range is splayed.
   template<class KeyType, class KeyNodePtrCompare>
   static node_ptr lower_bound
      (const node_ptr & header, const KeyType &key, KeyNodePtrCompare comp)
   {
      splay_down(detail::uncast(header), key, comp);
      node_ptr y = bstree_algo::lower_bound(header, key, comp);
      //splay_up(y, detail::uncast(header));
      return y;
   }

   //! @copydoc ::boost::intrusive::bstree_algorithms::lower_bound(const const_node_ptr&,const KeyType&,KeyNodePtrCompare)
   //! Additional note: no splaying is performed
   template<class KeyType, class KeyNodePtrCompare>
   static node_ptr lower_bound
      (const const_node_ptr & header, const KeyType &key, KeyNodePtrCompare comp)
   {  return bstree_algo::lower_bound(header, key, comp);  }

   //! @copydoc ::boost::intrusive::bstree_algorithms::upper_bound(const const_node_ptr&,const KeyType&,KeyNodePtrCompare)
   //! Additional notes: the first node of the range is splayed.
   template<class KeyType, class KeyNodePtrCompare>
   static node_ptr upper_bound
      (const node_ptr & header, const KeyType &key, KeyNodePtrCompare comp)
   {
      splay_down(detail::uncast(header), key, comp);
      node_ptr y = bstree_algo::upper_bound(header, key, comp);
      //splay_up(y, detail::uncast(header));
      return y;
   }

   //! @copydoc ::boost::intrusive::bstree_algorithms::upper_bound(const const_node_ptr&,const KeyType&,KeyNodePtrCompare)
   //! Additional note: no splaying is performed
   template<class KeyType, class KeyNodePtrCompare>
   static node_ptr upper_bound
      (const const_node_ptr & header, const KeyType &key, KeyNodePtrCompare comp)
   {  return bstree_algo::upper_bound(header, key, comp);  }

   //! @copydoc ::boost::intrusive::bstree_algorithms::find(const const_node_ptr&, const KeyType&,KeyNodePtrCompare)
   //! Additional notes: the found node of the lower bound is splayed.
   template<class KeyType, class KeyNodePtrCompare>
   static node_ptr find
      (const node_ptr & header, const KeyType &key, KeyNodePtrCompare comp)
   {
      splay_down(detail::uncast(header), key, comp);
      return bstree_algo::find(header, key, comp);
   }

   //! @copydoc ::boost::intrusive::bstree_algorithms::find(const const_node_ptr&, const KeyType&,KeyNodePtrCompare)
   //! Additional note: no splaying is performed
   template<class KeyType, class KeyNodePtrCompare>
   static node_ptr find
      (const const_node_ptr & header, const KeyType &key, KeyNodePtrCompare comp)
   {  return bstree_algo::find(header, key, comp);  }

   //! @copydoc ::boost::intrusive::bstree_algorithms::equal_range(const const_node_ptr&,const KeyType&,KeyNodePtrCompare)
   //! Additional notes: the first node of the range is splayed.
   template<class KeyType, class KeyNodePtrCompare>
   static std::pair<node_ptr, node_ptr> equal_range
      (const node_ptr & header, const KeyType &key, KeyNodePtrCompare comp)
   {
      splay_down(detail::uncast(header), key, comp);
      std::pair<node_ptr, node_ptr> ret = bstree_algo::equal_range(header, key, comp);
      //splay_up(ret.first, detail::uncast(header));
      return ret;
   }

   //! @copydoc ::boost::intrusive::bstree_algorithms::equal_range(const const_node_ptr&,const KeyType&,KeyNodePtrCompare)
   //! Additional note: no splaying is performed
   template<class KeyType, class KeyNodePtrCompare>
   static std::pair<node_ptr, node_ptr> equal_range
      (const const_node_ptr & header, const KeyType &key, KeyNodePtrCompare comp)
   {  return bstree_algo::equal_range(header, key, comp);  }

   //! @copydoc ::boost::intrusive::bstree_algorithms::lower_bound_range(const const_node_ptr&,const KeyType&,KeyNodePtrCompare)
   //! Additional notes: the first node of the range is splayed.
   template<class KeyType, class KeyNodePtrCompare>
   static std::pair<node_ptr, node_ptr> lower_bound_range
      (const node_ptr & header, const KeyType &key, KeyNodePtrCompare comp)
   {
      splay_down(detail::uncast(header), key, comp);
      std::pair<node_ptr, node_ptr> ret = bstree_algo::lower_bound_range(header, key, comp);
      //splay_up(ret.first, detail::uncast(header));
      return ret;
   }

   //! @copydoc ::boost::intrusive::bstree_algorithms::lower_bound_range(const const_node_ptr&,const KeyType&,KeyNodePtrCompare)
   //! Additional note: no splaying is performed
   template<class KeyType, class KeyNodePtrCompare>
   static std::pair<node_ptr, node_ptr> lower_bound_range
      (const const_node_ptr & header, const KeyType &key, KeyNodePtrCompare comp)
   {  return bstree_algo::lower_bound_range(header, key, comp);  }

   //! @copydoc ::boost::intrusive::bstree_algorithms::bounded_range(const const_node_ptr&,const KeyType&,const KeyType&,KeyNodePtrCompare,bool,bool)
   //! Additional notes: the first node of the range is splayed.
   template<class KeyType, class KeyNodePtrCompare>
   static std::pair<node_ptr, node_ptr> bounded_range
      (const node_ptr & header, const KeyType &lower_key, const KeyType &upper_key, KeyNodePtrCompare comp
      , bool left_closed, bool right_closed)
   {
      splay_down(detail::uncast(header), lower_key, comp);
      std::pair<node_ptr, node_ptr> ret =
         bstree_algo::bounded_range(header, lower_key, upper_key, comp, left_closed, right_closed);
      //splay_up(ret.first, detail::uncast(header));
      return ret;
   }

   //! @copydoc ::boost::intrusive::bstree_algorithms::bounded_range(const const_node_ptr&,const KeyType&,const KeyType&,KeyNodePtrCompare,bool,bool)
   //! Additional note: no splaying is performed
   template<class KeyType, class KeyNodePtrCompare>
   static std::pair<node_ptr, node_ptr> bounded_range
      (const const_node_ptr & header, const KeyType &lower_key, const KeyType &upper_key, KeyNodePtrCompare comp
      , bool left_closed, bool right_closed)
   {  return bstree_algo::bounded_range(header, lower_key, upper_key, comp, left_closed, right_closed);  }

   //! @copydoc ::boost::intrusive::bstree_algorithms::insert_equal_upper_bound(const node_ptr&,const node_ptr&,NodePtrCompare)
   //! Additional note: the inserted node is splayed
   template<class NodePtrCompare>
   static node_ptr insert_equal_upper_bound
      (const node_ptr & header, const node_ptr & new_node, NodePtrCompare comp)
   {
      splay_down(header, new_node, comp);
      return bstree_algo::insert_equal_upper_bound(header, new_node, comp);
   }

   //! @copydoc ::boost::intrusive::bstree_algorithms::insert_equal_lower_bound(const node_ptr&,const node_ptr&,NodePtrCompare)
   //! Additional note: the inserted node is splayed
   template<class NodePtrCompare>
   static node_ptr insert_equal_lower_bound
      (const node_ptr & header, const node_ptr & new_node, NodePtrCompare comp)
   {
      splay_down(header, new_node, comp);
      return bstree_algo::insert_equal_lower_bound(header, new_node, comp);
   }

   //! @copydoc ::boost::intrusive::bstree_algorithms::insert_equal(const node_ptr&,const node_ptr&,const node_ptr&,NodePtrCompare)
   //! Additional note: the inserted node is splayed
   template<class NodePtrCompare>
   static node_ptr insert_equal
      (const node_ptr & header, const node_ptr & hint, const node_ptr & new_node, NodePtrCompare comp)
   {
      splay_down(header, new_node, comp);
      return bstree_algo::insert_equal(header, hint, new_node, comp);
   }

   //! @copydoc ::boost::intrusive::bstree_algorithms::insert_before(const node_ptr&,const node_ptr&,const node_ptr&)
   //! Additional note: the inserted node is splayed
   static node_ptr insert_before
      (const node_ptr & header, const node_ptr & pos, const node_ptr & new_node)
   {
      bstree_algo::insert_before(header, pos, new_node);
      splay_up(new_node, header);
      return new_node;
   }

   //! @copydoc ::boost::intrusive::bstree_algorithms::push_back(const node_ptr&,const node_ptr&)
   //! Additional note: the inserted node is splayed
   static void push_back(const node_ptr & header, const node_ptr & new_node)
   {
      bstree_algo::push_back(header, new_node);
      splay_up(new_node, header);
   }

   //! @copydoc ::boost::intrusive::bstree_algorithms::push_front(const node_ptr&,const node_ptr&)
   //! Additional note: the inserted node is splayed
   static void push_front(const node_ptr & header, const node_ptr & new_node)
   {
      bstree_algo::push_front(header, new_node);
      splay_up(new_node, header);
   }

   //! @copydoc ::boost::intrusive::bstree_algorithms::insert_unique_check(const const_node_ptr&,const KeyType&,KeyNodePtrCompare,insert_commit_data&)
   //! Additional note: nodes with the given key are splayed
   template<class KeyType, class KeyNodePtrCompare>
   static std::pair<node_ptr, bool> insert_unique_check
      (const node_ptr & header, const KeyType &key
      ,KeyNodePtrCompare comp, insert_commit_data &commit_data)
   {
      splay_down(header, key, comp);
      return bstree_algo::insert_unique_check(header, key, comp, commit_data);
   }

   //! @copydoc ::boost::intrusive::bstree_algorithms::insert_unique_check(const const_node_ptr&,const node_ptr&,const KeyType&,KeyNodePtrCompare,insert_commit_data&)
   //! Additional note: nodes with the given key are splayed
   template<class KeyType, class KeyNodePtrCompare>
   static std::pair<node_ptr, bool> insert_unique_check
      (const node_ptr & header, const node_ptr &hint, const KeyType &key
      ,KeyNodePtrCompare comp, insert_commit_data &commit_data)
   {
      splay_down(header, key, comp);
      return bstree_algo::insert_unique_check(header, hint, key, comp, commit_data);
   }

   #ifdef BOOST_INTRUSIVE_DOXYGEN_INVOKED
   //! @copydoc ::boost::intrusive::bstree_algorithms::insert_unique_commit(const node_ptr&,const node_ptr&,const insert_commit_data&)
   static void insert_unique_commit
      (const node_ptr & header, const node_ptr & new_value, const insert_commit_data &commit_data);

   //! @copydoc ::boost::intrusive::bstree_algorithms::is_header
   static bool is_header(const const_node_ptr & p);

   //! @copydoc ::boost::intrusive::bstree_algorithms::rebalance
   static void rebalance(const node_ptr & header);

   //! @copydoc ::boost::intrusive::bstree_algorithms::rebalance_subtree
   static node_ptr rebalance_subtree(const node_ptr & old_root);

   #endif   //#ifdef BOOST_INTRUSIVE_DOXYGEN_INVOKED

   // bottom-up splay, use data_ as parent for n    | complexity : logarithmic    | exception : nothrow
   static void splay_up(const node_ptr & node, const node_ptr & header)
   {  priv_splay_up<true>(node, header); }

   // top-down splay | complexity : logarithmic    | exception : strong, note A
   template<class KeyType, class KeyNodePtrCompare>
   static node_ptr splay_down(const node_ptr & header, const KeyType &key, KeyNodePtrCompare comp, bool *pfound = 0)
   {  return priv_splay_down<true>(header, key, comp, pfound);   }

   private:

   /// @cond

   // bottom-up splay, use data_ as parent for n    | complexity : logarithmic    | exception : nothrow
   template<bool SimpleSplay>
   static void priv_splay_up(const node_ptr & node, const node_ptr & header)
   {
      // If (node == header) do a splay for the right most node instead
      // this is to boost performance of equal_range/count on equivalent containers in the case
      // where there are many equal elements at the end
      node_ptr n((node == header) ? NodeTraits::get_right(header) : node);
      node_ptr t(header);

      if( n == t ) return;

      for( ;; ){
         node_ptr p(NodeTraits::get_parent(n));
         node_ptr g(NodeTraits::get_parent(p));

         if( p == t )   break;

         if( g == t ){
            // zig
            rotate(n);
         }
         else if ((NodeTraits::get_left(p) == n && NodeTraits::get_left(g) == p)    ||
                  (NodeTraits::get_right(p) == n && NodeTraits::get_right(g) == p)  ){
            // zig-zig
            rotate(p);
            rotate(n);
         }
         else {
            // zig-zag
            rotate(n);
            if(!SimpleSplay){
               rotate(n);
            }
         }
      }
   }

   template<bool SimpleSplay, class KeyType, class KeyNodePtrCompare>
   static node_ptr priv_splay_down(const node_ptr & header, const KeyType &key, KeyNodePtrCompare comp, bool *pfound = 0)
   {
      //Most splay tree implementations use a dummy/null node to implement.
      //this function. This has some problems for a generic library like Intrusive:
      //
      // * The node might not have a default constructor.
      // * The default constructor could throw.
      //
      //We already have a header node. Leftmost and rightmost nodes of the tree
      //are not changed when splaying (because the invariants of the tree don't
      //change) We can back up them, use the header as the null node and
      //reassign old values after the function has been completed.
      node_ptr const old_root  = NodeTraits::get_parent(header);
      node_ptr const leftmost  = NodeTraits::get_left(header);
      node_ptr const rightmost = NodeTraits::get_right(header);
      if(leftmost == rightmost){ //Empty or unique node
         if(pfound){
            *pfound = old_root && !comp(key, old_root) && !comp(old_root, key);
         }
         return old_root ? old_root : header;
      }
      else{
         //Initialize "null node" (the header in our case)
         NodeTraits::set_left (header, node_ptr());
         NodeTraits::set_right(header, node_ptr());
         //Class that will backup leftmost/rightmost from header, commit the assemble(),
         //and will restore leftmost/rightmost to header even if "comp" throws
         detail::splaydown_assemble_and_fix_header<NodeTraits> commit(old_root, header, leftmost, rightmost);
         bool found = false;

         for( ;; ){
            if(comp(key, commit.t_)){
               node_ptr const t_left = NodeTraits::get_left(commit.t_);
               if(!t_left)
                  break;
               if(comp(key, t_left)){
                  bstree_algo::rotate_right_no_parent_fix(commit.t_, t_left);
                  commit.t_ = t_left;
                  if( !NodeTraits::get_left(commit.t_) )
                     break;
                  link_right(commit.t_, commit.r_);
               }
               else{
                  link_right(commit.t_, commit.r_);
                  if(!SimpleSplay && comp(t_left, key)){
                     if( !NodeTraits::get_right(commit.t_) )
                        break;
                     link_left(commit.t_, commit.l_);
                  }
               }
            }
            else if(comp(commit.t_, key)){
               node_ptr const t_right = NodeTraits::get_right(commit.t_);
               if(!t_right)
                  break;

               if(comp(t_right, key)){
                     bstree_algo::rotate_left_no_parent_fix(commit.t_, t_right);
                     commit.t_ = t_right;
                     if( !NodeTraits::get_right(commit.t_) )
                        break;
                     link_left(commit.t_, commit.l_);
               }
               else{
                  link_left(commit.t_, commit.l_);
                  if(!SimpleSplay && comp(key, t_right)){
                     if( !NodeTraits::get_left(commit.t_) )
                        break;
                     link_right(commit.t_, commit.r_);
                  }
               }
            }
            else{
               found = true;
               break;
            }
         }

         //commit.~splaydown_assemble_and_fix_header<NodeTraits>() will first
         //"assemble()" + link the new root & recover header's leftmost & rightmost
         if(pfound){
            *pfound = found;
         }
         return commit.t_;
      }
   }

   // break link to left child node and attach it to left tree pointed to by l   | complexity : constant | exception : nothrow
   static void link_left(node_ptr & t, node_ptr & l)
   {
      //procedure link_left;
      //    t, l, right(l) := right(t), t, t
      //end link_left
      NodeTraits::set_right(l, t);
      NodeTraits::set_parent(t, l);
      l = t;
      t = NodeTraits::get_right(t);
   }

   // break link to right child node and attach it to right tree pointed to by r | complexity : constant | exception : nothrow
   static void link_right(node_ptr & t, node_ptr & r)
   {
      //procedure link_right;
      //    t, r, left(r) := left(t), t, t
      //end link_right;
      NodeTraits::set_left(r, t);
      NodeTraits::set_parent(t, r);
      r = t;
      t = NodeTraits::get_left(t);
   }

   // rotate n with its parent                     | complexity : constant    | exception : nothrow
   static void rotate(const node_ptr & n)
   {
      //procedure rotate_left;
      //    t, right(t), left(right(t)) := right(t), left(right(t)), t
      //end rotate_left;
      node_ptr p = NodeTraits::get_parent(n);
      node_ptr g = NodeTraits::get_parent(p);
      //Test if g is header before breaking tree
      //invariants that would make is_header invalid
      bool g_is_header = bstree_algo::is_header(g);

      if(NodeTraits::get_left(p) == n){
         NodeTraits::set_left(p, NodeTraits::get_right(n));
         if(NodeTraits::get_left(p))
            NodeTraits::set_parent(NodeTraits::get_left(p), p);
         NodeTraits::set_right(n, p);
      }
      else{ // must be ( p->right == n )
         NodeTraits::set_right(p, NodeTraits::get_left(n));
         if(NodeTraits::get_right(p))
            NodeTraits::set_parent(NodeTraits::get_right(p), p);
         NodeTraits::set_left(n, p);
      }

      NodeTraits::set_parent(p, n);
      NodeTraits::set_parent(n, g);

      if(g_is_header){
         if(NodeTraits::get_parent(g) == p)
            NodeTraits::set_parent(g, n);
         else{//must be ( g->right == p )
            BOOST_INTRUSIVE_INVARIANT_ASSERT(false);
            NodeTraits::set_right(g, n);
         }
      }
      else{
         if(NodeTraits::get_left(g) == p)
            NodeTraits::set_left(g, n);
         else  //must be ( g->right == p )
            NodeTraits::set_right(g, n);
      }
   }

   /// @endcond
};

/// @cond

template<class NodeTraits>
struct get_algo<SplayTreeAlgorithms, NodeTraits>
{
   typedef splaytree_algorithms<NodeTraits> type;
};

template <class ValueTraits, class NodePtrCompare, class ExtraChecker>
struct get_node_checker<SplayTreeAlgorithms, ValueTraits, NodePtrCompare, ExtraChecker>
{
   typedef detail::bstree_node_checker<ValueTraits, NodePtrCompare, ExtraChecker> type;
};

/// @endcond

} //namespace intrusive
} //namespace boost

#include <boost/intrusive/detail/config_end.hpp>

#endif //BOOST_INTRUSIVE_SPLAYTREE_ALGORITHMS_HPP


#include <deque>
#include <unordered_set>
#include <set>
#include <iostream>
#include <memory>
#include <algorithm>

#include <boost/log/trivial.hpp>

#include "de_bruijn/graph.h"
#include "noise_filtering.h"

using namespace debruijn;

// Define a debruijn graph with s-mers of genes as nodes
debruijn::Graph::Graph(uint8_t s)
    : next_id(0)
    , size(s)
{
    nodes.reserve(200000);
};

debruijn::Graph::~Graph() { nodes.clear(); }

// Add a node in dbg corresponding to a fixed size deque of pangenome graph
// node/orientation ids and labelled with the read_ids which cover it
OrientedNodePtr debruijn::Graph::add_node(
    const std::deque<uint_least32_t>& node_ids, uint32_t read_id)
{
    const bool correct_number_of_nodes_to_add = node_ids.size() == size;
    if (!correct_number_of_nodes_to_add) {
        fatal_error("Error adding node to de Bruijn Graph: expected node of size ",
            size, ", received node of size ", node_ids.size());
    }

    if (node_hash.find(node_ids) != node_hash.end()) {
        nodes[node_hash[node_ids]]->read_ids.insert(read_id);
        return make_pair(nodes[node_hash[node_ids]], true);
    } else if (node_hash.find(rc_hashed_node_ids(node_ids)) != node_hash.end()) {
        auto rc = rc_hashed_node_ids(node_ids);
        nodes[node_hash[rc]]->read_ids.insert(read_id);
        return make_pair(nodes[node_hash[rc]], false);
    }

    NodePtr n;
    n = std::make_shared<Node>(next_id, node_ids, read_id);
    nodes[next_id] = n;
    node_hash[node_ids] = next_id;

    if (next_id % 1000 == 0) {
        BOOST_LOG_TRIVIAL(debug) << "added node " << next_id;
    }

    next_id++;
    return make_pair(n, true);
}

// An edge is valid if the kmer of node/orientation ids for from
// overlaps the first k-1 nodes/orientations of to
// Note that forward and reverse complement kmers are treated as
// equal, but an edge is only valid if the orientation they were found
// in in the read allows the overlap
bool edge_is_valid(OrientedNodePtr from, OrientedNodePtr to)
{
    std::deque<uint_least32_t> hashed_node_ids_from = from.first->hashed_node_ids;
    std::deque<uint_least32_t> hashed_node_ids_to = to.first->hashed_node_ids;
    if (!from.second) {
        hashed_node_ids_from = rc_hashed_node_ids(hashed_node_ids_from);
    }

    if (!to.second) {
        hashed_node_ids_to = rc_hashed_node_ids(hashed_node_ids_to);
    }

    return overlap_forwards(hashed_node_ids_from, hashed_node_ids_to);
}

// Add directed edge between from and to
void debruijn::Graph::add_edge(OrientedNodePtr from, OrientedNodePtr to)
{
    const bool nodes_are_valid = from.first != nullptr and to.first != nullptr;
    if (!nodes_are_valid) {
        fatal_error("Error adding edge to de Bruijn Graph: from or to node is invalid");
    }

    if (!edge_is_valid(from, to)) {
        fatal_error("Error adding edge to de Bruijn Graph: edge from ", *from.first,
            " to ", *to.first, " is invalid");
    }

    if (from.second
        and from.first->out_nodes.find(to.first->id) == from.first->out_nodes.end()) {
        from.first->out_nodes.insert(to.first->id);
    } else if (!from.second
        and from.first->in_nodes.find(to.first->id) == from.first->in_nodes.end()) {
        from.first->in_nodes.insert(to.first->id);
    }

    if (to.second
        and to.first->in_nodes.find(from.first->id) == to.first->in_nodes.end()) {
        to.first->in_nodes.insert(from.first->id);
    } else if (!to.second
        and to.first->out_nodes.find(from.first->id) == to.first->out_nodes.end()) {
        to.first->out_nodes.insert(from.first->id);
    }
}

// Remove all mentions of de bruijn node with id given from graph
void debruijn::Graph::remove_node(const uint32_t dbg_node_id)
{
    auto it = nodes.find(dbg_node_id);
    if (it != nodes.end()) {
        // remove this node from lists of out nodes from other graph nodes
        for (const auto& n : it->second->out_nodes) {
            nodes[n]->in_nodes.erase(dbg_node_id);
            nodes[n]->out_nodes.erase(dbg_node_id);
        }
        for (const auto& n : it->second->in_nodes) {
            nodes[n]->out_nodes.erase(dbg_node_id);
            nodes[n]->in_nodes.erase(dbg_node_id);
        }

        // and remove from nodes
        nodes.erase(dbg_node_id);
    }
}

// Remove all copies of read from de bruijn node
void debruijn::Graph::remove_read_from_node(
    const uint32_t read_id, const uint32_t dbg_node_id)
{
    auto it = nodes.find(dbg_node_id);
    bool found_read_intersect;
    if (it != nodes.end()) {
        auto rit = it->second->read_ids.find(read_id);
        if (rit != it->second->read_ids.end()) {
            it->second->read_ids.erase(rit);

            // if there are no more reads covering it, remove the node
            if (it->second->read_ids.empty()) {
                remove_node(dbg_node_id);
            } else {
                // otherwise, remove any outnodes which no longer share a read
                for (std::unordered_set<uint32_t>::iterator nit
                     = it->second->out_nodes.begin();
                     nit != it->second->out_nodes.end();) {
                    found_read_intersect = false;
                    for (const auto& r : it->second->read_ids) {
                        if (nodes[*nit]->read_ids.find(r)
                            != nodes[*nit]->read_ids.end()) {
                            found_read_intersect = true;
                            break;
                        }
                    }
                    if (!found_read_intersect) {
                        nodes[*nit]->in_nodes.erase(dbg_node_id);
                        nit = it->second->out_nodes.erase(nit);
                    } else {
                        nit++;
                    }
                }
                for (std::unordered_set<uint32_t>::iterator nit
                     = it->second->in_nodes.begin();
                     nit != it->second->in_nodes.end();) {
                    found_read_intersect = false;
                    for (const auto& r : it->second->read_ids) {
                        if (nodes[*nit]->read_ids.find(r)
                            != nodes[*nit]->read_ids.end()) {
                            found_read_intersect = true;
                            break;
                        }
                    }
                    if (!found_read_intersect) {
                        nodes[*nit]->out_nodes.erase(dbg_node_id);
                        nit = it->second->in_nodes.erase(nit);
                    } else {
                        nit++;
                    }
                }
            }
        }
    }
}

// Get the dbg node ids corresponding to leaves
std::unordered_set<uint32_t> debruijn::Graph::get_leaves(uint_least32_t covg_thresh)
{
    std::unordered_set<uint32_t> s;
    for (const auto& c : nodes) {
        BOOST_LOG_TRIVIAL(debug)
            << "node " << *c.second << " has " << c.second->out_nodes.size() << " + "
            << c.second->in_nodes.size() << " outnodes";
        if (c.second->read_ids.size() > covg_thresh) {
            continue;
        } else if (c.second->out_nodes.size() + c.second->in_nodes.size() <= 1) {
            s.insert(c.second->id);
        }
    }
    return s;
}

// Get deques of dbg node ids corresponding to maximal non-branching paths in dbg
std::set<std::deque<uint32_t>> debruijn::Graph::get_unitigs()
{
    std::set<std::deque<uint32_t>> all_tigs;
    std::set<uint32_t> seen;

    for (const auto& node_entry : nodes) {
        const auto& id = node_entry.first;
        const auto& node_ptr = node_entry.second;

        const bool node_seen = seen.find(id) != seen.end();
        const bool at_branch
            = (node_ptr->out_nodes.size() > 1) or (node_ptr->in_nodes.size() > 1);
        if (node_seen or at_branch)
            continue;

        std::deque<uint32_t> tig = { id };
        extend_unitig(tig);
        for (const auto& other_id : tig)
            seen.insert(other_id);
        all_tigs.insert(tig);
    }
    return all_tigs;
}

// Extend a dbg path on either end until reaching a branch point
void debruijn::Graph::extend_unitig(std::deque<uint32_t>& tig)
{
    const bool tig_is_empty = (tig.empty());
    const bool node_is_isolated = (tig.size() == 1
        and (nodes[tig.back()]->out_nodes.size() + nodes[tig.back()]->in_nodes.size())
            == 0);
    if (tig_is_empty or node_is_isolated) {
        return;
    }

    bool can_extend = nodes[tig.back()]->out_nodes.size() == 1;
    bool use_outnodes = true;
    while (can_extend) {

        if (use_outnodes) {
            tig.push_back(*nodes[tig.back()]->out_nodes.begin());
        } else {
            tig.push_back(*nodes[tig.back()]->in_nodes.begin());
        }

        if (std::find(nodes[tig.back()]->in_nodes.begin(),
                nodes[tig.back()]->in_nodes.end(), *----tig.end())
            != nodes[tig.back()]->in_nodes.end()) {
            can_extend = nodes[tig.back()]->out_nodes.size() == 1
                and nodes[tig.back()]->in_nodes.size() <= 1
                and tig.front() != tig.back();
            use_outnodes = true;
        } else if (std::find(nodes[tig.back()]->out_nodes.begin(),
                       nodes[tig.back()]->out_nodes.end(), *----tig.end())
            != nodes[tig.back()]->out_nodes.end()) {
            can_extend = nodes[tig.back()]->in_nodes.size() == 1
                and nodes[tig.back()]->out_nodes.size() <= 1
                and tig.front() != tig.back();
            use_outnodes = false;
        } else {
            can_extend = false;
        }
    }

    if (tig.size() == 1) {
        can_extend = nodes[tig.front()]->in_nodes.size() == 1
            and nodes[tig.front()]->out_nodes.size() <= 1;
        use_outnodes = false;
    } else {

        if (std::find(nodes[tig.front()]->in_nodes.begin(),
                nodes[tig.front()]->in_nodes.end(), *++tig.begin())
            != nodes[tig.front()]->in_nodes.end()) {
            can_extend = nodes[tig.front()]->out_nodes.size() == 1
                and nodes[tig.front()]->in_nodes.size() <= 1
                and tig.front() != tig.back();
            use_outnodes = true;
        } else if (std::find(nodes[tig.front()]->out_nodes.begin(),
                       nodes[tig.front()]->out_nodes.end(), *++tig.begin())
            != nodes[tig.front()]->out_nodes.end()) {
            can_extend = nodes[tig.front()]->in_nodes.size() == 1
                and nodes[tig.front()]->out_nodes.size() <= 1
                and tig.front() != tig.back();
            use_outnodes = false;
        } else {
            can_extend = false;
        }
    }

    while (can_extend) {
        if (use_outnodes) {
            tig.push_front(*nodes[tig.front()]->out_nodes.begin());
        } else {
            tig.push_front(*nodes[tig.front()]->in_nodes.begin());
        }

        if (std::find(nodes[tig.front()]->in_nodes.begin(),
                nodes[tig.front()]->in_nodes.end(), *++tig.begin())
            != nodes[tig.front()]->in_nodes.end()) {
            can_extend = nodes[tig.front()]->out_nodes.size() == 1
                and nodes[tig.front()]->in_nodes.size() <= 1
                and tig.front() != tig.back();
            use_outnodes = true;
        } else if (std::find(nodes[tig.front()]->out_nodes.begin(),
                       nodes[tig.front()]->out_nodes.end(), *++tig.begin())
            != nodes[tig.front()]->out_nodes.end()) {
            can_extend = nodes[tig.front()]->in_nodes.size() == 1
                and nodes[tig.front()]->out_nodes.size() <= 1
                and tig.front() != tig.back();
            use_outnodes = false;
        } else {
            can_extend = false;
        }
    }

    while (tig.size() > 1 and tig.front() == tig.back()) {
        tig.pop_back();
    }

    std::stringstream tig_ss;
    for (const auto& n : tig)
        tig_ss << n << " ";
    BOOST_LOG_TRIVIAL(debug) << "got tig of length " << tig.size() << ": "
                             << tig_ss.str();
}

// Search the outnodes of node_ptr_to_search for node_ptr_to_find
bool debruijn::Graph::found_in_out_nodes(
    const NodePtr node_ptr_to_search, const NodePtr node_ptr_to_find) const
{
    for (const auto& i : node_ptr_to_search->out_nodes) {
        if (*nodes.at(i) == *node_ptr_to_find) {
            return true;
        }
    }
    return false;
}

// Search the innodes of node_ptr_to_search for node_ptr_to_find
bool debruijn::Graph::found_in_in_nodes(
    const NodePtr node_ptr_to_search, const NodePtr node_ptr_to_find) const
{
    for (const auto& i : node_ptr_to_search->in_nodes) {
        if (*nodes.at(i) == *node_ptr_to_find) {
            return true;
        }
    }
    return false;
}

// Graphs are equal if they have nodes corresponding to the same kmer
// of node/orientations, with outnodes and innodes the same
bool debruijn::Graph::operator==(const Graph& y) const
{
    // want the graphs to have the same nodes, even if
    // the ids given them is different.
    if (nodes.size() != y.nodes.size()) {
        BOOST_LOG_TRIVIAL(debug) << "different num nodes";
        return false;
    }

    for (const auto& t : nodes) {
        bool found = false;
        for (const auto& s : y.nodes) {
            if (*t.second == *s.second) {
                found = true;

                // also check the outnodes are the same
                if (t.second->out_nodes.size() + t.second->in_nodes.size()
                    != s.second->out_nodes.size() + s.second->in_nodes.size()) {
                    BOOST_LOG_TRIVIAL(debug) << "node has different number of outnodes";
                    return false;
                }

                for (const auto& i : t.second->out_nodes) {
                    if (not y.found_in_out_nodes(s.second, nodes.at(i))
                        and not y.found_in_in_nodes(s.second, nodes.at(i))) {
                        BOOST_LOG_TRIVIAL(debug)
                            << "did not find " << i << " in outnode or innodes";
                        return false;
                    }
                }
                for (const auto& i : t.second->in_nodes) {
                    if (not y.found_in_out_nodes(s.second, nodes.at(i))
                        and not y.found_in_in_nodes(s.second, nodes.at(i))) {
                        BOOST_LOG_TRIVIAL(debug)
                            << "did not find " << i << " in outnode or innodes";
                        return false;
                    }
                }
                break;
            }
        }
        if (!found) {
            BOOST_LOG_TRIVIAL(debug)
                << "did not find node " << t.first << " " << *t.second;
            return false;
        }
    }

    // nodes can't be equal within a graph so don't need to check vice versa
    return true;
}

bool debruijn::Graph::operator!=(const Graph& y) const { return !(*this == y); }

namespace debruijn {
std::ostream& operator<<(std::ostream& out, const Graph& m)
{
    for (const auto& n : m.nodes) {
        out << n.first << ": " << *(n.second) << std::endl;
        for (const auto& o : n.second->out_nodes) {
            out << n.first << " -> " << o << std::endl;
        }
    }
    return out;
}
}