/* ----------------------------------------------------------------------------

 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

 * See LICENSE for the license information

 * -------------------------------------------------------------------------- */

/**
 * @file   GaussianConditional.cpp
 * @brief  Conditional Gaussian Base class
 * @author Christian Potthast
 */

#include <string.h>
#include <boost/format.hpp>
#ifdef __GNUC__
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wunused-variable"
#endif
#include <boost/lambda/lambda.hpp>
#include <boost/lambda/bind.hpp>
#ifdef __GNUC__
#pragma GCC diagnostic pop
#endif

#include <gtsam/linear/linearExceptions.h>
#include <gtsam/linear/GaussianConditional.h>
#include <gtsam/linear/GaussianFactor.h>
#include <gtsam/linear/JacobianFactor.h>

using namespace std;

namespace gtsam {

/* ************************************************************************* */
GaussianConditional::GaussianConditional() : rsd_(matrix_) {}

/* ************************************************************************* */
GaussianConditional::GaussianConditional(Index key) : IndexConditional(key), rsd_(matrix_) {}

/* ************************************************************************* */
GaussianConditional::GaussianConditional(Index key,const Vector& d, const Matrix& R, const Vector& sigmas) :
      IndexConditional(key), rsd_(matrix_), sigmas_(sigmas) {
  assert(R.rows() <= R.cols());
  long dims[] = { R.cols(), 1 };
  rsd_.copyStructureFrom(rsd_type(matrix_, dims, dims+2, d.size()));
  rsd_(0) = R.triangularView<Eigen::Upper>();
  get_d_() = d;
}

/* ************************************************************************* */
GaussianConditional::GaussianConditional(Index key, const Vector& d, const Matrix& R,
    Index name1, const Matrix& S, const Vector& sigmas) :
    IndexConditional(key,name1), rsd_(matrix_), sigmas_(sigmas) {
  assert(R.rows() <= R.cols());
  long dims[] = { R.cols(), S.cols(), 1 };
  rsd_.copyStructureFrom(rsd_type(matrix_, dims, dims+3, d.size()));
  rsd_(0) = R.triangularView<Eigen::Upper>();
  rsd_(1) = S;
  get_d_() = d;
}

/* ************************************************************************* */
GaussianConditional::GaussianConditional(Index key, const Vector& d, const Matrix& R,
    Index name1, const Matrix& S, Index name2, const Matrix& T, const Vector& sigmas) :
    IndexConditional(key,name1,name2), rsd_(matrix_), sigmas_(sigmas) {
  assert(R.rows() <= R.cols());
  long dims[] = { R.cols(), S.cols(), T.cols(), 1 };
  rsd_.copyStructureFrom(rsd_type(matrix_, dims, dims+4, d.size()));
  rsd_(0) = R.triangularView<Eigen::Upper>();
  rsd_(1) = S;
  rsd_(2) = T;
  get_d_() = d;
}

/* ************************************************************************* */
  GaussianConditional::GaussianConditional(Index key, const Vector& d,
      const Matrix& R, const list<pair<Index, Matrix> >& parents, const Vector& sigmas) :
    IndexConditional(key, GetKeys(parents.size(), parents.begin(), parents.end())), rsd_(matrix_), sigmas_(sigmas) {
  assert(R.rows() <= R.cols());
  size_t* dims = (size_t*)alloca(sizeof(size_t)*(1+parents.size()+1)); // FIXME: alloca is bad, just ask Google.
  dims[0] = R.cols();
  size_t j=1;
  std::list<std::pair<Index, Matrix> >::const_iterator parent=parents.begin();
  for(; parent!=parents.end(); ++parent,++j)
    dims[j] = parent->second.cols();
  dims[j] = 1;
  rsd_.copyStructureFrom(rsd_type(matrix_, dims, dims+1+parents.size()+1, d.size()));
  rsd_(0) = R.triangularView<Eigen::Upper>();
  j = 1;
  for(std::list<std::pair<Index, Matrix> >::const_iterator parent=parents.begin(); parent!=parents.end(); ++parent) {
    rsd_(j).noalias() = parent->second;
    ++ j;
  }
  get_d_() = d;
}

/* ************************************************************************* */
GaussianConditional::GaussianConditional(const std::list<std::pair<Index, Matrix> >& terms,
    const size_t nrFrontals, const Vector& d, const Vector& sigmas) :
    IndexConditional(GetKeys(terms.size(), terms.begin(), terms.end()), nrFrontals),
    rsd_(matrix_), sigmas_(sigmas) {
  size_t* dims = (size_t*)alloca(sizeof(size_t)*(terms.size()+1)); // FIXME: alloca is bad, just ask Google.
  size_t j=0;
  typedef pair<Index, Matrix> Index_Matrix;
  BOOST_FOREACH(const Index_Matrix& term, terms) {
    dims[j] = term.second.cols();
    ++ j;
  }
  dims[j] = 1;
  rsd_.copyStructureFrom(rsd_type(matrix_, dims, dims+terms.size()+1, d.size()));
  j=0;
  BOOST_FOREACH(const Index_Matrix& term, terms) {
    rsd_(j) = term.second;
    ++ j;
  }
  get_d_() = d;
}

/* ************************************************************************* */
GaussianConditional::GaussianConditional(const GaussianConditional& rhs) :
  rsd_(matrix_) {
  *this = rhs;
}

/* ************************************************************************* */
GaussianConditional& GaussianConditional::operator=(const GaussianConditional& rhs) {
  if(this != &rhs) {
    this->Base::operator=(rhs);  // Copy keys
    rsd_.assignNoalias(rhs.rsd_);     // Copy matrix and block configuration
    sigmas_ = rhs.sigmas_;       // Copy sigmas
  }
  return *this;
}

/* ************************************************************************* */
void GaussianConditional::print(const string &s, const IndexFormatter& formatter) const
{
  cout << s << ": density on ";
  for(const_iterator it = beginFrontals(); it != endFrontals(); ++it) {
    cout << (boost::format("[%1%]")%(formatter(*it))).str() << " ";
  }
  cout << endl;
  gtsam::print(Matrix(get_R()),"R");
  for(const_iterator it = beginParents() ; it != endParents() ; ++it ) {
    gtsam::print(Matrix(get_S(it)), (boost::format("A[%1%]")%(formatter(*it))).str());
  }
  gtsam::print(Vector(get_d()),"d");
  gtsam::print(sigmas_,"sigmas");
}

/* ************************************************************************* */
bool GaussianConditional::equals(const GaussianConditional &c, double tol) const {
  // check if the size of the parents_ map is the same
  if (parents().size() != c.parents().size())
    return false;

  // check if R_ and d_ are linear independent
  for (size_t i=0; i<rsd_.rows(); i++) {
    list<Vector> rows1; rows1.push_back(Vector(get_R().row(i)));
    list<Vector> rows2; rows2.push_back(Vector(c.get_R().row(i)));

    // check if the matrices are the same
    // iterate over the parents_ map
    for (const_iterator it = beginParents(); it != endParents(); ++it) {
      const_iterator it2 = c.beginParents() + (it-beginParents());
      if(*it != *(it2))
        return false;
      rows1.push_back(row(get_S(it), i));
      rows2.push_back(row(c.get_S(it2), i));
    }

    Vector row1 = concatVectors(rows1);
    Vector row2 = concatVectors(rows2);
    if (!linear_dependent(row1, row2, tol))
      return false;
  }

  // check if sigmas are equal
  if (!(equal_with_abs_tol(sigmas_, c.sigmas_, tol))) return false;

  return true;
}

/* ************************************************************************* */
JacobianFactor::shared_ptr GaussianConditional::toFactor() const {
  return JacobianFactor::shared_ptr(new JacobianFactor(*this));
}

/* ************************************************************************* */
void GaussianConditional::solveInPlace(VectorValues& x) const {
  static const bool debug = false;
  if(debug) this->print("Solving conditional in place");
  Vector xS = internal::extractVectorValuesSlices(x, this->beginParents(), this->endParents());
  xS = this->get_d() - this->get_S() * xS;
  Vector soln = this->get_R().triangularView<Eigen::Upper>().solve(xS);

  // Check for indeterminant solution
  if(soln.unaryExpr(!boost::lambda::bind(ptr_fun(isfinite<double>), boost::lambda::_1)).any())
    throw IndeterminantLinearSystemException(this->keys().front());

  if(debug) {
    gtsam::print(Matrix(this->get_R()), "Calling backSubstituteUpper on ");
    gtsam::print(soln, "full back-substitution solution: ");
  }
  internal::writeVectorValuesSlices(soln, x, this->beginFrontals(), this->endFrontals());
}

/* ************************************************************************* */
void GaussianConditional::solveTransposeInPlace(VectorValues& gy) const {
  Vector frontalVec = internal::extractVectorValuesSlices(gy, beginFrontals(), endFrontals());
  frontalVec = gtsam::backSubstituteUpper(frontalVec,Matrix(get_R()));

  // Check for indeterminant solution
  if(frontalVec.unaryExpr(!boost::lambda::bind(ptr_fun(isfinite<double>), boost::lambda::_1)).any())
    throw IndeterminantLinearSystemException(this->keys().front());

  GaussianConditional::const_iterator it;
  for (it = beginParents(); it!= endParents(); it++) {
    const Index i = *it;
    transposeMultiplyAdd(-1.0,get_S(it),frontalVec,gy[i]);
  }
  internal::writeVectorValuesSlices(frontalVec, gy, beginFrontals(), endFrontals());
}

/* ************************************************************************* */
void GaussianConditional::scaleFrontalsBySigma(VectorValues& gy) const {
  Vector frontalVec = internal::extractVectorValuesSlices(gy, beginFrontals(), endFrontals());
  frontalVec = emul(frontalVec, get_sigmas());
  internal::writeVectorValuesSlices(frontalVec, gy, beginFrontals(), endFrontals());
}

}


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_ARITHMETIC_FUNCTIONS_DEC_HPP_INCLUDED
#define BOOST_SIMD_ARITHMETIC_FUNCTIONS_DEC_HPP_INCLUDED
#include <boost/simd/include/functor.hpp>

namespace boost { namespace simd
{
  namespace tag
  {
    /*!
      @brief  dec generic tag

      Represents the dec function in generic contexts.

      @par Models:
      Hierarchy
    **/
    struct dec_ : ext::elementwise_<dec_>
    {
      /// @brief Parent hierarchy
      typedef ext::elementwise_<dec_> parent;
      template<class... Args>
      static BOOST_FORCEINLINE BOOST_AUTO_DECLTYPE dispatch(Args&&... args)
      BOOST_AUTO_DECLTYPE_BODY( dispatching_dec_( ext::adl_helper(), static_cast<Args&&>(args)... ) )
    };
  }
  namespace ext
  {
    template<class Site, class... Ts>
    BOOST_FORCEINLINE generic_dispatcher<tag::dec_, Site> dispatching_dec_(adl_helper, boost::dispatch::meta::unknown_<Site>, boost::dispatch::meta::unknown_<Ts>...)
    {
      return generic_dispatcher<tag::dec_, Site>();
    }
    template<class... Args>
    struct impl_dec_;
  }
  /*!
    Decrements a value by 1.

    @par semantic:
    For any given value @c x of type @c T:

    @code
    T r = dec(x);
    @endcode

    is equivalent to:

    @code
    T r =  x-T(1);
    @endcode

    @see  @funcref{minusone}, @funcref{minus}
    @param  a0

    @return      a value of the same type as the input.

  **/
  BOOST_DISPATCH_FUNCTION_IMPLEMENTATION(tag::dec_, dec, 1)
} }

#endif


/*
 * ExpressionTree.cpp
 *
 *  Created on: 2013/06/19
 *      Author: kryozahiro
 */

#include "ExpressionTree.h"

#include <cassert>
#include <algorithm>
#include <boost/lexical_cast.hpp>
#include "cpputil/GenericIo.h"
#include "../../problem/SantaFeTrail.h"
using namespace std;
namespace pt = boost::property_tree;

vector<shared_ptr<Program>> ExpressionTree::generate(const ProgramType& programType, const pt::ptree& node, int size, mt19937_64& randomEngine) {
	int maxDepth = node.get<int>("MaxDepth");
	ExpressionTree prototype(programType, maxDepth);

	const pt::ptree& operators = node.get_child("Operators");
	for (const pt::ptree::value_type& kvp : operators) {
		if (kvp.first == "Constant") {
			string rangeStr = kvp.second.data();
			pair<double, double> range = boost::lexical_cast<pair<double, double>>(rangeStr);
			prototype.addOperator(make_shared<ConstantNode>(range));
		} else if (kvp.first == "Variable") {
			prototype.addOperator(make_shared<VariableNode>());
		} else if (kvp.first == "Add") {
			prototype.addOperator(make_shared<OperatorNode>(OperatorNode::ADD));
		} else if (kvp.first == "Sub") {
			prototype.addOperator(make_shared<OperatorNode>(OperatorNode::SUB));
		} else if (kvp.first == "Mul") {
			prototype.addOperator(make_shared<OperatorNode>(OperatorNode::MUL));
		} else if (kvp.first == "Div") {
			prototype.addOperator(make_shared<OperatorNode>(OperatorNode::DIV));
		} else if (kvp.first == "Pow") {
			prototype.addOperator(make_shared<OperatorNode>(OperatorNode::POW));
		} else if (kvp.first == "SantaFeTrailOperators") {
			prototype.addOperator(make_shared<OperatorNode>(SantaFeTrail::IF_FOOD_AHEAD));
			prototype.addOperator(make_shared<OperatorNode>(SantaFeTrail::PROGN2));
			prototype.addOperator(make_shared<OperatorNode>(SantaFeTrail::PROGN3));
			prototype.addOperator(make_shared<OperatorNode>(SantaFeTrail::MOVE));
			prototype.addOperator(make_shared<OperatorNode>(SantaFeTrail::LEFT));
			prototype.addOperator(make_shared<OperatorNode>(SantaFeTrail::RIGHT));
		} else {
			assert(false);
		}
	}

	string method = node.get<string>("Method");
	if (method == "RampedHalfAndHalf") {
		return ExpressionTree::rampedHalfAndHalf(prototype, size, maxDepth, randomEngine);
	} else {
		assert(false);
	}
	return vector<shared_ptr<Program>>();
}

vector<shared_ptr<Program>> ExpressionTree::rampedHalfAndHalf(const ExpressionTree& prototype, int size, int maxDepth, mt19937_64& randomEngine) {
	vector<shared_ptr<Program>> trees;

	int eachSize = size / (maxDepth - 1);
	for (int depth = 2; depth <= maxDepth; ++depth) {
		for (int k = 0; k < eachSize / 2; ++k) {
			shared_ptr<ExpressionTree> full = make_shared<ExpressionTree>(prototype);
			full->maxDepth = depth;
			full->full(randomEngine);
			trees.push_back(full);
		}
		for (int k = 0; k < eachSize - eachSize / 2; ++k) {
			shared_ptr<ExpressionTree> grow = make_shared<ExpressionTree>(prototype);
			grow->maxDepth = depth;
			grow->grow(randomEngine);
			trees.push_back(grow);
		}
	}

	return trees;
}

void ExpressionTree::normalCrossover(ExpressionTree& parent1, ExpressionTree& parent2, int rootIndex, mt19937_64& randomEngine) {
	assert(&parent1 != &parent2);

	//入れ替える部分木を選ぶ
	assert(parent1.roots[rootIndex]);
	std::vector<std::shared_ptr<ExpressionNode>> flat1 = ExpressionNode::flatten(parent1.roots[rootIndex]);
	std::uniform_int_distribution<int> dist1(0, flat1.size() - 1);
	std::shared_ptr<ExpressionNode> target1 = flat1[dist1(randomEngine)];
	int depth1 = target1->getDepth();
	int height1 = target1->getHeight();

	assert(parent2.roots[rootIndex]);
	std::vector<std::shared_ptr<ExpressionNode>> flat2 = ExpressionNode::flatten(parent2.roots[rootIndex]);
	flat2.erase(remove_if(flat2.begin(), flat2.end(), [&](std::shared_ptr<ExpressionNode>& node2) {
		return (depth1 + node2->getHeight() > parent1.maxDepth) or (node2->getDepth() + height1 > parent2.maxDepth);
	}), flat2.end());
	if (flat2.size() == 0) {
		//TODO
		return;
	}
	std::uniform_int_distribution<int> dist2(0, flat2.size() - 1);
	std::shared_ptr<ExpressionNode> target2 = flat2[dist2(randomEngine)];

	//入れ替える
	ExpressionNode* up1 = target1->getParent();
	ExpressionNode* up2 = target2->getParent();
	if (up1 == nullptr) {
		parent1.roots[rootIndex] = target2;
		parent1.roots[rootIndex]->setParent(nullptr);
	} else {
		up1->replaceChild(*target1, target2);
	}
	if (up2 == nullptr) {
		parent2.roots[rootIndex] = target1;
		parent2.roots[rootIndex]->setParent(nullptr);
	} else {
		up2->replaceChild(*target2, target1);
	}
}

void ExpressionTree::normalMutation(ExpressionTree& solution, int rootIndex, std::mt19937_64& randomEngine) {
	std::uniform_int_distribution<int> mutationDist(1, 100);
	if (mutationDist(randomEngine) > 5) {
		return;
	}

	//変異の開始点を選ぶ
	std::vector<std::shared_ptr<ExpressionNode>> flat = ExpressionNode::flatten(solution.roots[rootIndex]);
	std::uniform_int_distribution<int> dist(0, flat.size() - 1);
	std::shared_ptr<ExpressionNode> target = flat[dist(randomEngine)];
	ExpressionNode* up = target->getParent();

	//新しい木を作る
	int maxDepth = solution.maxDepth - target->getDepth();
	std::uniform_int_distribution<int> depthDist(0, maxDepth);
	std::uniform_int_distribution<int> useGrow(0, 1);
	std::shared_ptr<ExpressionNode> subTree;
	if (useGrow(randomEngine)) {
		subTree = solution.makeSubTree(depthDist(randomEngine), randomEngine, &ExpressionTree::makeNodeByFull);
	} else {
		subTree = solution.makeSubTree(depthDist(randomEngine), randomEngine, &ExpressionTree::makeNodeByGrow);
	}

	//変異させる
	if (up == nullptr) {
		solution.roots[rootIndex] = subTree;
		solution.roots[rootIndex]->setParent(nullptr);
	} else {
		up->replaceChild(*target, subTree);
	}
}

ExpressionTree::ExpressionTree(const ProgramType& programType, int maxDepth) : Program(programType) {
	this->maxDepth = maxDepth;
}

ExpressionTree::ExpressionTree(const ExpressionTree& tree) : Program(tree.getProgramType()), maxDepth(tree.maxDepth) {
	for (shared_ptr<ExpressionNode> root : tree.roots) {
		roots.push_back(shared_ptr<ExpressionNode>(root->clone()));
	}
	nonterminals = tree.nonterminals;
	terminals = tree.terminals;
}

vector<double> ExpressionTree::operator()(const vector<double>& input) {
	assert((unsigned int)getProgramType().getOutputType().getSize() == roots.size());
	assert(getProgramType().getInputType().accepts(input));

	//式の実行
	vector<double> output(roots.size(), 0);
	for (unsigned int i = 0; i < roots.size(); ++i) {
		output[i] = roots[i]->operator()(input);
	}

	//出力のクリッピング
	getProgramType().getOutputType().clip(output);
	return output;
}

string ExpressionTree::toString() const {
	string ret;
	for (unsigned int i = 0; i < roots.size(); ++i) {
		ret += "tree" + to_string(i) + ": " + roots[i]->toString() + "\n";
	}
	return ret;
}

void ExpressionTree::crossoverImpl(const std::string& method, ExpressionTree& other, std::mt19937_64& randomEngine) {
	assert(this->roots.size() == other.roots.size());
	for (unsigned int i = 0; i < roots.size(); ++i) {
		ExpressionTree::normalCrossover(*this, other, i, randomEngine);
	}
}

void ExpressionTree::mutationImpl(const std::string& method, std::mt19937_64& randomEngine) {
	for (unsigned int i = 0; i < roots.size(); ++i) {
		ExpressionTree::normalMutation(*this, i, randomEngine);
	}
}

void ExpressionTree::addOperator(shared_ptr<ExpressionNode> op) {
	if (op->getArity() == 0) {
		terminals.push_back(op);
	} else {
		nonterminals.push_back(op);
	}
}

void ExpressionTree::grow(mt19937_64& randomEngine) {
	roots = vector<shared_ptr<ExpressionNode>>(getProgramType().getOutputType().getSize(), nullptr);
	for (shared_ptr<ExpressionNode>& root : roots) {
		root = makeSubTree(maxDepth, randomEngine, &ExpressionTree::makeNodeByGrow);
	}
}

void ExpressionTree::full(mt19937_64& randomEngine) {
	roots = vector<shared_ptr<ExpressionNode>>(getProgramType().getOutputType().getSize(), nullptr);
	for (shared_ptr<ExpressionNode>& root : roots) {
		root = makeSubTree(maxDepth, randomEngine, &ExpressionTree::makeNodeByFull);
	}
}

shared_ptr<ExpressionNode> ExpressionTree::makeSubTree(int maxDepth, mt19937_64& randomEngine, MethodMakeNode method) {
	shared_ptr<ExpressionNode> rootNode = (this->*method)(0, maxDepth, randomEngine);
	rootNode->randomize(getProgramType(), randomEngine);

	vector<shared_ptr<ExpressionNode>> now;
	now.push_back(rootNode);

	for (int thisDepth = 1; thisDepth <= maxDepth; ++thisDepth) {
		vector<shared_ptr<ExpressionNode>> next;
		for (shared_ptr<ExpressionNode> node : now) {
			for (int i = 0; i < node->getArity(); ++i) {
				std::shared_ptr<ExpressionNode> child = (this->*method)(thisDepth, maxDepth, randomEngine);
				child->randomize(getProgramType(), randomEngine);
				node->addChild(child);
				next.push_back(child);
			}
		}
		now = next;
	}
	return rootNode;
}

shared_ptr<ExpressionNode> ExpressionTree::makeNodeByGrow(int thisDepth, int maxDepth, mt19937_64& randomEngine) {
	if (thisDepth == maxDepth) {
		uniform_int_distribution<int> dist(0, terminals.size() - 1);
		return shared_ptr<ExpressionNode>(terminals[dist(randomEngine)]->clone());
	}
	uniform_int_distribution<int> dist(0, nonterminals.size() + terminals.size() - 1);
	int select = dist(randomEngine);
	if ((unsigned int)select < nonterminals.size()) {
		return shared_ptr<ExpressionNode>(nonterminals[select]->clone());
	}
	return shared_ptr<ExpressionNode>(terminals[select - nonterminals.size()]->clone());
}

shared_ptr<ExpressionNode> ExpressionTree::makeNodeByFull(int thisDepth, int maxDepth, mt19937_64& randomEngine) {
	if (thisDepth == maxDepth) {
		uniform_int_distribution<int> dist(0, terminals.size() - 1);
		return shared_ptr<ExpressionNode>(terminals[dist(randomEngine)]->clone());
	}
	uniform_int_distribution<int> dist(0, nonterminals.size() - 1);
	return shared_ptr<ExpressionNode>(nonterminals[dist(randomEngine)]->clone());
}


#include <fftw3.h>
#include <boost/timer/timer.hpp>
#include <boost/chrono.hpp>
#include <cmath>
#include <iostream>
#include <complex>
#include <Eigen/Dense>
#include <vector>
#include <fstream>
#include "chirpz.h"

using namespace Eigen;
namespace chirpz {

void hello(fc32_t c) {
    std::cout << "Hello World : " << c << std::endl;

}

template class ChirpZ<c32_t>;
template class ChirpZ<c64_t>;
template class ChirpZ2d<c32_t>;
template class ChirpZ2d<c64_t>;

}


#include <boost/accumulators/statistics/weighted_covariance.hpp>


#include "SumoNetwork.h"
#include "carla/geom/Math.h"
#include "carla/geom/Triangulation.h"
#include <boost/algorithm/string.hpp>
#include <pugixml/pugixml.hpp>
#include <string>
#include <sstream>
#include <fstream>

namespace carla {
namespace sumonetwork {

static std::vector<geom::Vector2D> parse_shape_list(const std::string& s) {
  if (s.empty()) return {};

  std::vector<std::string> split_list;
  boost::split(
      split_list,
      s,
      boost::is_any_of(", "),
      boost::token_compress_on);

  std::vector<geom::Vector2D> position_list;
  for (size_t i = 0; i < split_list.size(); i += 2) {
    position_list.emplace_back(
        std::stof(split_list[i + 1]), // Swap for SUMO -> CARLA.
        std::stof(split_list[i]));    // Swap for SUMO -> CARLA.
  }

  return position_list;
}

static geom::Vector2D parse_coordinates(const std::string& s) {
  if (s.empty()) return geom::Vector2D(0, 0);

  std::vector<std::string> split_list;
  boost::split(
      split_list,
      s,
      boost::is_any_of(","),
      boost::token_compress_on);

  // Swap x, y for SUMO -> CARLA.
  return geom::Vector2D(std::stof(split_list[1]), std::stof(split_list[0]));
}

static std::pair<geom::Vector2D, geom::Vector2D> parse_bounds(const std::string& s) {
  std::vector<std::string> split_list;
  boost::split(
      split_list,
      s,
      boost::is_any_of(","),
      boost::token_compress_on);

  // Swap x, y for SUMO -> CARLA.
  return std::make_pair(
      geom::Vector2D(std::stof(split_list[1]), std::stof(split_list[0])),
      geom::Vector2D(std::stof(split_list[3]), std::stof(split_list[2])));
}

static std::vector<std::string> parse_string_list(const std::string& s) {
  if (s.empty()) return {};

  std::vector<std::string> split_list;
  boost::split(
      split_list,
      s,
      boost::is_any_of(" "),
      boost::token_compress_on);
  return split_list;
}

SumoNetwork SumoNetwork::Load(const std::string& file) {
  std::ifstream ifs;
  ifs.open(file, std::ios::in);
  std::ostringstream ss;
  ss << ifs.rdbuf();
  ifs.close();

  pugi::xml_document xml;
  xml.load_string(ss.str().c_str());

  SumoNetwork sumo_network;

  pugi::xml_node net_node = xml.child("net");

  pugi::xml_node location_node = net_node.child("location");
  sumo_network._offset = parse_coordinates(location_node.attribute("netOffset").value());

  std::pair<geom::Vector2D, geom::Vector2D> bounds = parse_bounds(location_node.attribute("convBoundary").value());
  sumo_network._bounds_min = bounds.first;
  sumo_network._bounds_max = bounds.second;

  // origBoundary: LonLat -> (parse_bounds swaps) -> LatLon
  std::pair<geom::Vector2D, geom::Vector2D> originalBounds = parse_bounds(location_node.attribute("origBoundary").value());
  sumo_network._original_bounds_min = originalBounds.first;
  sumo_network._original_bounds_max = originalBounds.second;

  for (pugi::xml_node edge_node : net_node.children("edge")) {
    Edge edge;
    edge.id = edge_node.attribute("id").value();
    edge.from = edge_node.attribute("from").value();
    edge.to = edge_node.attribute("to").value();
    edge.priority = edge_node.attribute("priority").as_int();
    std::string function = edge_node.attribute("function").value();
    if (function == "internal") edge.function = Function::Internal;
    else if (function == "connector") edge.function = Function::Connector;
    else if (function == "crossing") edge.function = Function::Crossing;
    else if (function == "walkingarea") edge.function = Function::WalkingArea;
    else edge.function = Function::Normal;

    // Temp vector required because order of lanes read do not necessarily match
    // order of their indexes.
    std::vector<Lane> lanes_temp;
    for (pugi::xml_node lane_node : edge_node.children("lane")) {
      Lane lane;
      lane.id = lane_node.attribute("id").value();
      lane.index = lane_node.attribute("index").as_uint();
      lane.speed = lane_node.attribute("speed").as_float();
      lane.length = lane_node.attribute("length").as_float();
      lane.shape = parse_shape_list(lane_node.attribute("shape").value());
      lanes_temp.emplace_back(std::move(lane));
    }
    edge.lanes.resize(lanes_temp.size());
    // TODO: Check if sorting passes or there are invalid instances, e.g. indexes
    // that exceed the bounds of the number of lanes.
    for (size_t i = 0; i < lanes_temp.size(); i++) {
      edge.lanes[lanes_temp[i].index] = std::move(lanes_temp[i]);
    }

    sumo_network._edges.emplace(edge.id, std::move(edge));
  }

  for (pugi::xml_node junction_node : net_node.children("junction")) {
    Junction junction;
    junction.id = junction_node.attribute("id").value();
    junction.x = junction_node.attribute("y").as_float(); // Swap for SUMO -> CARLA.
    junction.y = junction_node.attribute("x").as_float(); // Swap for SUMO -> CARLA.
    junction.inc_lanes = parse_string_list(junction_node.attribute("incLanes").value());
    junction.int_lanes = parse_string_list(junction_node.attribute("intLanes").value());
    junction.shape = parse_shape_list(junction_node.attribute("shape").value());
    sumo_network._junctions.emplace(junction.id, std::move(junction));
  }

  for (pugi::xml_node connection_node : net_node.children("connection")) {
    Connection connection;
    connection.from = connection_node.attribute("from").value();
    connection.to = connection_node.attribute("to").value();
    connection.from_lane = connection_node.attribute("fromLane").as_uint();
    connection.to_lane = connection_node.attribute("toLane").as_uint();
    connection.via = connection_node.attribute("via").value();
    sumo_network._connections.emplace_back(std::move(connection));
  }

  sumo_network.Build();

  return sumo_network;
}

void SumoNetwork::Build() {
  std::vector<rt_value_t> index_entries;
  for (const auto& edge_entry : _edges) {
    const Edge& edge = edge_entry.second;
    for (const Lane& lane : edge.lanes) {
      for (size_t i = 0; i < lane.shape.size() - 1; i++) {
        index_entries.emplace_back(
            rt_segment_t(
              rt_point_t(lane.shape[i].x, lane.shape[i].y),
              rt_point_t(lane.shape[i + 1].x, lane.shape[i + 1].y)),
            std::make_tuple(edge.id, lane.index, i));
      }
      _lane_to_parent_edge_map[lane.id] = std::make_pair(edge.id, lane.index);
      _outgoing_connections_map.emplace(lane.id, std::vector<size_t>());
    }
  }
  _segments_index = rt_tree_t(index_entries);

  for (size_t i = 0; i < _connections.size(); i++) {
    const Connection& connection = _connections[i];

    if (!connection.via.empty()){
      _internal_edge_to_connection_map[connection.via] = i;
    }
    _outgoing_connections_map.at(_edges[connection.from].lanes[connection.from_lane].id).emplace_back(i);
  }
}

geom::Vector2D SumoNetwork::GetRoutePointPosition(const RoutePoint& route_point) const {
  const geom::Vector2D& start = _edges.at(route_point.edge).lanes[route_point.lane].shape[route_point.segment];
  const geom::Vector2D& end = _edges.at(route_point.edge).lanes[route_point.lane].shape[route_point.segment + 1];
  return start + route_point.offset * (end - start).MakeUnitVector();
}

RoutePoint SumoNetwork::GetNearestRoutePoint(const geom::Vector2D& position) const {
  std::vector<rt_value_t> results;
  _segments_index.query(boost::geometry::index::nearest(rt_point_t(position.x, position.y), 1), std::back_inserter(results));
  rt_value_t& result = results[0];

  geom::Vector2D segment_start(
      boost::geometry::get<0, 0>(result.first),
      boost::geometry::get<0, 1>(result.first));
  geom::Vector2D segment_end(
      boost::geometry::get<1, 0>(result.first),
      boost::geometry::get<1, 1>(result.first));
  geom::Vector2D direction = (segment_end - segment_start).MakeUnitVector();

  float t = geom::Vector2D::DotProduct(
      position - segment_start,
      direction);
  t = std::max(0.0f, std::min((segment_end - segment_start).Length(), t));

  return RoutePoint{
    std::get<0>(result.second),
    std::get<1>(result.second),
    std::get<2>(result.second),
    t};
}

std::vector<RoutePoint> SumoNetwork::GetNextRoutePoints(const RoutePoint& route_point, float distance) const {
  const Edge& edge = _edges.at(route_point.edge);
  const Lane& lane = edge.lanes[route_point.lane];

  const geom::Vector2D& segment_start = lane.shape[route_point.segment];
  const geom::Vector2D& segment_end = lane.shape[route_point.segment + 1];
  float segment_length = (segment_end - segment_start).Length();

  if (route_point.offset + distance <= segment_length) {
    return { {route_point.edge, route_point.lane, route_point.segment, route_point.offset + distance} };
  } else if (route_point.segment < lane.shape.size() - 2) {
    return GetNextRoutePoints(
        RoutePoint{route_point.edge, route_point.lane, route_point.segment + 1, 0},
        distance - (segment_length - route_point.offset));
  } else {
    std::vector<RoutePoint> next_route_points;
    for (size_t connection_index : _outgoing_connections_map.at(lane.id)) {
      const Connection& connection = _connections[connection_index];
      if (connection.via == "") {
        std::vector<RoutePoint> results = GetNextRoutePoints(
            RoutePoint{connection.to, connection.to_lane, 0, 0},
            distance - (segment_length - route_point.offset));
        next_route_points.insert(next_route_points.end(), results.begin(), results.end());
      } else {
        const std::pair<std::string, uint32_t>& lane_parent = _lane_to_parent_edge_map.at(connection.via);
        std::vector<RoutePoint> results = GetNextRoutePoints(
            RoutePoint{lane_parent.first, lane_parent.second, 0, 0},
            distance - (segment_length - route_point.offset));
        next_route_points.insert(next_route_points.end(), results.begin(), results.end());
      }
    }

    return next_route_points;
  }
}

std::vector<std::vector<RoutePoint>> SumoNetwork::GetNextRoutePaths(const RoutePoint& route_point, size_t num_points, float interval) const {
  if (num_points == 0) return {{route_point}};

  std::vector<std::vector<RoutePoint>> result;
  for (const RoutePoint& next_route_point : GetNextRoutePoints(route_point, interval)) {
    std::vector<std::vector<RoutePoint>> next_route_paths = GetNextRoutePaths(next_route_point, num_points - 1, interval);
    result.reserve(next_route_paths.size());
    for (const std::vector<RoutePoint>& next_route_path : next_route_paths) {
      result.emplace_back();
      result.back().reserve(1 + next_route_path.size());
      result.back().emplace_back(route_point);
      result.back().insert(result.back().end(), next_route_path.begin(), next_route_path.end());
    }
  }
  return result;
}

occupancy::OccupancyMap SumoNetwork::CreateOccupancyMap() const {
  occupancy::OccupancyMap occupancy_map;

  for (const auto& edge_entry : _edges) {
    const Edge& edge = edge_entry.second;
    for (const Lane& lane : edge.lanes) {
      // Extra 0.10m to close any gap between lanes.
      occupancy_map = occupancy_map.Union(occupancy::OccupancyMap(lane.shape, 4.10f));
    }
  }

  for (const auto& junction_entry : _junctions) {
    const Junction& junction = junction_entry.second;
    occupancy_map = occupancy_map.Union(occupancy::OccupancyMap(junction.shape));
  }

  return occupancy_map;
}

occupancy::OccupancyMap SumoNetwork::CreateRoadmarkOccupancyMap() const {
  occupancy::OccupancyMap roadmark_occupancy_map;
  for (const auto& edge_entry : _edges) {
    const Edge& edge = edge_entry.second;
    for (const Lane& lane : edge.lanes) {
      roadmark_occupancy_map = roadmark_occupancy_map.Union(occupancy::OccupancyMap(lane.shape, 4.00f, 0.10f));
    }
  }

  occupancy::OccupancyMap junction_occupancy_map;
  for (const auto& junction_entry : _junctions) {
    const Junction& junction = junction_entry.second;
    junction_occupancy_map = junction_occupancy_map.Union(occupancy::OccupancyMap(junction.shape));
  }

  return roadmark_occupancy_map.Difference(junction_occupancy_map);
}

segments::SegmentMap SumoNetwork::CreateSegmentMap() const {
  std::vector<geom::Segment2D> segments;

  for (const auto& edge_entry : _edges) {
    const Edge& edge = edge_entry.second;
    for (const Lane& lane : edge.lanes) {
      for (size_t i = 0; i < lane.shape.size() - 1; i++) {
        segments.emplace_back(lane.shape[i], lane.shape[i + 1]);
      }
    }
  }

  return segments::SegmentMap(std::move(segments));
}

}
}


//
// Created by David Oberacker on 2019-07-31.
//

#include <string>
#include <map>
#include <queue>
#include <boost/dynamic_bitset.hpp>

#include "common/common.hpp"

struct Node
{
    uint8_t Symbol;
    bool IsLeaf;
    int64_t Left;
    int64_t Right;
};

uint8_t decodeByte(boost::dynamic_bitset<> data, int64_t* offset) {
    uint8_t value = 0;
    for (int i = 7; i >= 0; --i)
    {
        uint8_t v = data[(*offset)++] ? 1 : 0;
        value |= (uint8_t)(v << (unsigned int) i);
    }
    return value;
};

int decodeNode(const boost::dynamic_bitset<>& data, int64_t* offset, Node* tree, int64_t* index)
{
    uint32_t current = (*index);
    (*index)++;

    bool isLeaf = data[(*offset)++];

    if (isLeaf)
    {
        uint8_t value = decodeByte(data, offset);
        tree[current].Left = -1;
        tree[current].Right = -1;
        tree[current].IsLeaf = true;
        tree[current].Symbol = value;
    }
    else
    {
        tree[current].Left = decodeNode(data, offset, tree, index);
        tree[current].Right = decodeNode(data, offset, tree, index);
        tree[current].IsLeaf = false;
    }

    return current;
}

 void convertBytesToBits(const char* data, uint32_t data_size, boost::dynamic_bitset<>* bit_map)
{
    for (int i = 0, o = 0; i < data_size; i++)
    {
        for (int j = 7; j >= 0; j--)
        {
            bool s = ((data[i] & (1 << j)) != 0);
            (*bit_map)[o++] = s;
        }
    }
}

bool BORDERLANDS_COMMON_API
D4v3::Borderlands::Common::Huffman::decode(const char *input_array, uint32_t input_size, char *output_array,
                                           int32_t output_size) noexcept(false) {
    auto* bitArray = new boost::dynamic_bitset<>(input_size * 8);
    convertBytesToBits(input_array, input_size, bitArray);

    auto* tree = new Node[511];
    int64_t index = 0;
    int64_t offset = 0;

    decodeNode(*bitArray, &offset, tree, &index);

    int32_t left = output_size;

    uint32_t o = 0;
    while (left > 0)
    {
        Node branch = tree[0];
        while (!branch.IsLeaf)
        {
            branch = tree[(*bitArray)[offset++] == false ? branch.Left : branch.Right];
        }

        output_array[o++] = branch.Symbol;
        left--;
    }

    delete[] tree;
    delete bitArray;

    return true;
}


// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG (haftungsbeschränkt), www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also license.mtl.txt in the distribution.

#ifndef MTL_DOT_INCLUDE
#define MTL_DOT_INCLUDE

#include <boost/numeric/mtl/concept/std_concept.hpp>
#include <boost/numeric/mtl/concept/collection.hpp>
#include <boost/numeric/mtl/operation/conj.hpp>
#include <boost/numeric/meta_math/loop1.hpp>
#include <boost/numeric/linear_algebra/identity.hpp>
#include <boost/numeric/mtl/interface/vpt.hpp>
#include <boost/numeric/mtl/utility/omp_size_type.hpp>
#include <boost/numeric/mtl/utility/static_assert.hpp>
#include <boost/numeric/mtl/utility/exception.hpp>

namespace mtl {

    namespace vector {

	namespace detail {

	    // Result type of dot product
	    template <typename Vector1, typename Vector2>
	    struct dot_result
	    {
		typedef typename Multiplicable<typename Collection<Vector1>::value_type,
					       typename Collection<Vector2>::value_type>::result_type type;
	    };

	    // Whether or not conjugating first argument
	    struct without_conj
	    {
		template <typename Value>
		Value operator()(const Value& v) { return v; }
	    };

	    struct with_conj
	    {
		template <typename Value>
		typename mtl::sfunctor::conj<Value>::result_type
		operator() (const Value& v)
		{
		    using mtl::conj;
		    return conj(v);
		}
	    };
	}

	namespace sfunctor {

	    template <unsigned long Index0, unsigned long Max0>
	    struct dot_aux
		: public meta_math::loop1<Index0, Max0>
	    {
		typedef meta_math::loop1<Index0, Max0>                                    base;
		typedef dot_aux<base::next_index0, Max0>                                  next_t;

		template <typename Value, typename Vector1, typename Vector2, typename Size, typename ConjOpt>
		static inline void
		apply(Value& tmp00, Value& tmp01, Value& tmp02, Value& tmp03, Value& tmp04,
		      Value& tmp05, Value& tmp06, Value& tmp07,
		      const Vector1& v1, const Vector2& v2, Size i, ConjOpt conj_opt)
		{
		    // vampir_trace<9901> tracer;
		    tmp00+= conj_opt(v1[ i + base::index0 ]) * v2[ i + base::index0 ];
		    next_t::apply(tmp01, tmp02, tmp03, tmp04, tmp05, tmp06, tmp07, tmp00,
				  v1, v2, i, conj_opt);
		}
	    };

	    template <unsigned long Max0>
	    struct dot_aux<Max0, Max0>
	    {
		typedef meta_math::loop1<Max0, Max0>                                      base;

		template <typename Value, typename Vector1, typename Vector2, typename Size, typename ConjOpt>
		static inline void
		apply(Value& tmp00, Value&, Value&, Value&, Value&, Value&, Value&, Value&,
		      const Vector1& v1, const Vector2& v2, Size i, ConjOpt conj_opt)
		{
		    tmp00+= conj_opt(v1[ i + base::index0 ]) * v2[ i + base::index0 ];
		}
	    };

	    template <unsigned long Unroll>
	    struct dot
	    {
		template <typename Vector1, typename Vector2, typename ConjOpt>
		typename detail::dot_result<Vector1, Vector2>::type
		static inline apply(const Vector1& v1, const Vector2& v2, ConjOpt conj_opt)
		{
		    MTL_STATIC_ASSERT((Unroll >= 1), "Unroll size must be at least 1.");
		    // MTL_STATIC_ASSERT((Unroll <= 8), "Maximal unrolling is 8."); // Might be relaxed in future versions

		    vampir_trace<2003> tracer;
		    MTL_THROW_IF(mtl::size(v1) != mtl::size(v2), incompatible_size());
		    typedef typename detail::dot_result<Vector1, Vector2>::type  value_type;

#                 ifdef MTL_WITH_OPENMP
		    value_type dummy, z= math::zero(dummy), result= z;
		    typedef typename mtl::traits::omp_size_type<typename Collection<Vector1>::size_type>::type size_type;
		    size_type  i_max= mtl::size(v1), i_block= Unroll * (i_max / Unroll);

                    #pragma omp parallel
		    {

			vampir_trace<8001> tracer;
			value_type tmp00= z, tmp01= z, tmp02= z, tmp03= z, tmp04= z, tmp05= z, tmp06= z, tmp07= z;

			#pragma omp for
			for (size_type i= 0; i < i_block; i+= Unroll)
			    dot_aux<1, Unroll>::apply(tmp00, tmp01, tmp02, tmp03, tmp04, tmp05, tmp06, tmp07, v1, v2, i, conj_opt);

			#pragma omp critical
			    result+= ((tmp00 + tmp01) + (tmp02 + tmp03)) + ((tmp04 + tmp05) + (tmp06 + tmp07));
		    }
		    for (size_type i= i_block; i < i_max; i++)
			result+= conj_opt(v1[i]) * v2[i];

		    return result;
#                 else
		    typedef typename Collection<Vector1>::size_type              size_type;

		    value_type dummy, z= math::zero(dummy), tmp00= z, tmp01= z, tmp02= z, tmp03= z, tmp04= z,
			       tmp05= z, tmp06= z, tmp07= z;
		    size_type  i_max= mtl::size(v1), i_block= Unroll * (i_max / Unroll);

		    for (size_type i= 0; i < i_block; i+= Unroll)
			dot_aux<1, Unroll>::apply(tmp00, tmp01, tmp02, tmp03, tmp04, tmp05, tmp06, tmp07, v1, v2, i, conj_opt);

		    for (size_type i= i_block; i < i_max; i++)
			tmp00+= conj_opt(v1[i]) * v2[i];
		    return ((tmp00 + tmp01) + (tmp02 + tmp03)) + ((tmp04 + tmp05) + (tmp06 + tmp07));
#                 endif
		}

	    };
	}

	template <typename Vector1, typename Vector2, typename ConjOpt>
	typename detail::dot_result<Vector1, Vector2>::type
	inline dot_simple(const Vector1& v1, const Vector2& v2, ConjOpt conj_opt)
	{
	    vampir_trace<2040> tracer;
	    typedef typename Collection<Vector1>::size_type              size_type;
	    typedef typename detail::dot_result<Vector1, Vector2>::type  value_type;

	    value_type dummy, s= math::zero(dummy);
	    for (size_type i= 0, i_max= mtl::size(v1); i < i_max; ++i)
		s+= conj_opt(v1[i]) * v2[i];
	    return s;
	}

	template <unsigned long Unroll, typename Vector1, typename Vector2, typename ConjOpt>
	struct dot_class
	{
	    typedef typename detail::dot_result<Vector1, Vector2>::type result_type;
	    dot_class(const Vector1& v1, const Vector2& v2) : v1(v1), v2(v2) {}

	    operator result_type() const { return sfunctor::dot<Unroll>::apply(v1, v2, ConjOpt()); }

	    const Vector1& v1;
	    const Vector2& v2;
	};

	template <typename Vector1, typename Vector2, typename ConjOpt>
	struct dot_class<1, Vector1, Vector2, ConjOpt>
	{
	    typedef typename detail::dot_result<Vector1, Vector2>::type result_type;
	    dot_class(const Vector1& v1, const Vector2& v2) : v1(v1), v2(v2) {}

	    operator result_type() const { return dot_simple(v1, v2, ConjOpt()); }

	    const Vector1& v1;
	    const Vector2& v2;
	};

	/// Lazy dot product
	/** It is automatically evaluated when (implicitly) converted to result_type which doesn't work in template expressions.
	    Can be used for source-to-source transformations. **/
	template <typename Vector1, typename Vector2>
	dot_class<4, Vector1, Vector2, detail::with_conj>
	inline lazy_dot(const Vector1& v1, const Vector2& v2)
	{
	    return dot_class<4, Vector1, Vector2, detail::with_conj>(v1, v2);
	}

	template <unsigned long Unroll, typename Vector1, typename Vector2>
	dot_class<Unroll, Vector1, Vector2, detail::with_conj>
	inline lazy_dot(const Vector1& v1, const Vector2& v2)
	{
	    return dot_class<Unroll, Vector1, Vector2, detail::with_conj>(v1, v2);
	}

	template <typename Vector1, typename Vector2>
	dot_class<4, Vector1, Vector2, detail::without_conj>
	inline lazy_dot_real(const Vector1& v1, const Vector2& v2)
	{
	    return dot_class<4, Vector1, Vector2, detail::without_conj>(v1, v2);
	}

	template <unsigned long Unroll, typename Vector1, typename Vector2>
	dot_class<Unroll, Vector1, Vector2, detail::without_conj>
	inline lazy_dot_real(const Vector1& v1, const Vector2& v2)
	{
	    return dot_class<Unroll, Vector1, Vector2, detail::without_conj>(v1, v2);
	}

	/// Dot product defined as hermitian(v) * w
	/** Unrolled four times by default **/
	template <typename Vector1, typename Vector2>
	typename detail::dot_result<Vector1, Vector2>::type
	inline dot(const Vector1& v1, const Vector2& v2)
	{
	    // return dot_simple(v1, v2, detail::with_conj());
	    return sfunctor::dot<4>::apply(v1, v2, detail::with_conj());
	}

	/// Dot product with user-specified unrolling defined as hermitian(v) * w
	template <unsigned long Unroll, typename Vector1, typename Vector2>
	typename detail::dot_result<Vector1, Vector2>::type
	inline dot(const Vector1& v1, const Vector2& v2)
	{
	    return sfunctor::dot<Unroll>::apply(v1, v2, detail::with_conj());
	}
	/// Dot product without conjugate defined as trans(v) * w
	/** Unrolled four times by default **/
	template <typename Vector1, typename Vector2>
	typename detail::dot_result<Vector1, Vector2>::type
	inline dot_real(const Vector1& v1, const Vector2& v2)
	{
	    return sfunctor::dot<4>::apply(v1, v2, detail::without_conj());
	}

	/// Dot product without conjugate with user-specified unrolling defined as trans(v) * w
	template <unsigned long Unroll, typename Vector1, typename Vector2>
	typename detail::dot_result<Vector1, Vector2>::type
	inline dot_real(const Vector1& v1, const Vector2& v2)
	{
	    return sfunctor::dot<Unroll>::apply(v1, v2, detail::without_conj());
	}

    } // namespace vector

    using vector::dot;
    using vector::dot_real;
    using vector::lazy_dot;
    using vector::lazy_dot_real;

} // namespace mtl

#endif // MTL_DOT_INCLUDE


#include <iostream>

#include <boost\polygon\voronoi.hpp>

#include "Utility.h"
#include "UtilRandom.h"
#include "Pathfinding.h"
#include "Vec2.h"

#include "RoadNetwork.h"
#include "MajorRoad.h"
#include "MinorRoad.h"
#include "RoadNode.h"
#include "MstEdge.h"
#include "MstNode.h"
#include "MinimumSpanningTree.h"
#include "WaterData.h"
#include "BuildingLot.h"
#include "Line.h"

using namespace boost::polygon;
typedef int coordinate_type;
typedef point_data<coordinate_type> point_type;
typedef voronoi_diagram<double> VD;

RoadNetwork::RoadNetwork()
{
	minorRoads = std::vector<MinorRoad*>();
	majorRoads = std::vector<MajorRoad*>();
	roadNodes = std::vector<std::vector<RoadNode*>>();
}

RoadNetwork::~RoadNetwork() {}

void RoadNetwork::GenerateMinorRoads(VD* voronoi)
{
	for (auto const &edge : voronoi->edges())
	{
		if (edge.vertex0() != NULL && edge.vertex1() != NULL)
		{
			int sX = edge.vertex0()->x();
			int sY = edge.vertex0()->y();

			int eX = edge.vertex1()->x();
			int eY = edge.vertex1()->y();

			// See if this exists already
			bool exists = false;
			for (auto r : minorRoads)
			{
				// If this is the same road but reversed (start->end vs end->start)
				if (r->End()->x == sX && r->End()->y == sY
					&& r->Start()->x == eX && r->Start()->y == eY)
				{
					exists = true;
					break;
				}
			}
			if (exists) continue;

			// If not, create
			MinorRoad* road = new MinorRoad(sX, sY, eX, eY);
			minorRoads.push_back(road);
		}
	}
}

void RoadNetwork::GenerateMajorRoads(MinimumSpanningTree* mst, int offsetForRoadNodes)
{
	std::cout << "Total edges: " << mst->GetAllEdges().size() << std::endl;
	std::cout << "Pathfinding " << mst->GetTreeEdges().size() << " roads..." << std::endl;
	int roadCounter = 0;

	for (MstEdge* edge : mst->GetTreeEdges())
	{
		if (edge->start->position->x == edge->end->position->x && edge->start->position->y == edge->end->position->y)
		{
			continue;
		}

		// Create the road
		MajorRoad* road = new MajorRoad();

		// Pathfind to get the nodes
		road->nodes = Pathfinding::PathFind(roadNodes,
			edge->start->position->x / offsetForRoadNodes,
			edge->start->position->y / offsetForRoadNodes,
			edge->end->position->x / offsetForRoadNodes,
			edge->end->position->y / offsetForRoadNodes,
			offsetForRoadNodes);

		// Split the road into segments
		for (std::vector<RoadNode*>::iterator iter = road->nodes.begin(); iter != road->nodes.end(); iter++)
		{
			if (Utility::ReturnNextIter(iter) != road->nodes.end())
			{
				road->segments.push_back(
					new MajorRoad((*iter)->position, (*Utility::ReturnNextIter(iter))->position));
			}
		}

		majorRoads.push_back(road);

		std::cout << "\r" << roadCounter++ << " roads complete.\t\t";
	}
	std::cout << std::endl;
}

void RoadNetwork::ValidateRoads(WaterData* waterData)
{
	/*******************************************************
	Remove any voronoi minor roads that
	overlap main mst roads and reconnect the
	voronoi to the main roads
	********************************************************/
	std::vector<Vec2*> expandPoints;

	std::cout << "Checking for minor roads to prune..." << std::endl;

	// For each minor road
	for (MinorRoad* minor : minorRoads)
	{
		// Check each major road
		for (MajorRoad* major : majorRoads)
		{
			// Check each segment
			for (auto seg : major->segments)
			{
				if (Utility::GetIntersectionPointWithFiniteLines(minor->Start(), minor->End(), seg->Start(), seg->End()) != nullptr)
				{
					// Mark this road for deletion
					minor->markedForDeletion = true;

					// Tell its neighbours to expand and find a major road to connect to
					expandPoints.push_back(new Vec2(minor->Start()->x, minor->Start()->y));
					expandPoints.push_back(new Vec2(minor->End()->x, minor->End()->y));
				}
			}
		}
	}

	std::cout << "Deleting roads." << std::endl;

	// Remove and minor roads that have been marked for deletion
	for (std::vector<MinorRoad*>::iterator iter = minorRoads.begin(); iter != minorRoads.end(); /**/)
	{
		if ((*iter)->markedForDeletion)
		{
			// Tell its neighbours to expand and find a major road to connect to
			iter = minorRoads.erase(iter);
		}
		else
		{
			iter++;
		}
	}

	// Time to reconnect the voronoi to the main roads

	std::cout << "Reconnecting minor roads to highways" << std::endl;

	// For each point
	for (Vec2* point : expandPoints)
	{
		// Find the closest roadNode to it
		RoadNode* closest = nullptr;

		// Check each node
		for (auto road : majorRoads)
		{
			for (RoadNode* node : road->nodes)
			{
				// The first will be closest by default
				if (closest == nullptr) { closest = node; }

				// If node is closer than closest, closest becomes node
				if (Vec2::DistanceBetween(*node->position, *point) < Vec2::DistanceBetween(*closest->position, *point))
				{
					closest = node;
				}
			}
		}

		// After finding the closest, create this new road
		minorRoads.push_back(
			new MinorRoad(point->x,
				point->y,
				closest->position->x,
				closest->position->y));
	}

	// Now that we've built new minor roads, we need to check for collisions between any minor roads

	std::cout << "Checking for new collisions." << std::endl;

	for (auto minorRoad : minorRoads)
	{
		for (auto other : minorRoads)
		{
			// Skip itself
			if (*minorRoad == *other) { continue; }

			// if Other is already marked for deletion, skip
			// We don't want both roads to get deleted which would cause there to be
			// 0 connections instead of 2 (we want there to be 1)
			if (other->markedForDeletion) { continue; }

			// Get the intersection point
			Vec2* iPoint = Utility::GetIntersectionPointWithFiniteLines(minorRoad->Start(), minorRoad->End(), other->Start(), other->End());

			// If it's not null
			if (iPoint != nullptr)
			{
				// Make sure its not where the roads naturally connect
				if (*iPoint == *minorRoad->Start()
					|| *iPoint == *minorRoad->End()
					|| *iPoint == *other->Start()
					|| *iPoint == *other->End())
				{
					continue;
				}
			}
			else { continue; }

			// If not, continue
			minorRoad->markedForDeletion = true;
		}
	}

	// We can also detect here to delete and roads that have an end which sits inside of a body of water
	for (auto minorRoad : minorRoads)
	{
		if (waterData->IsWater(minorRoad->Start()->x, minorRoad->Start()->y)
			||
			waterData->IsWater(minorRoad->End()->x, minorRoad->End()->y))
		{
			minorRoad->markedForDeletion = true;
		}
	}

	// Next we delete any minor roads that are above a certain length threshold
	// this is to remove the long outwards lines left over from the voronoi
	for (auto minorRoad : minorRoads)
	{
		if (Vec2::DistanceBetween(*minorRoad->Start(), *minorRoad->End()) > maxMinorRoadDist)
		{
			minorRoad->markedForDeletion = true;
		}
	}

	// Remove any new minor roads that have been marked for deletion
	for (std::vector<MinorRoad*>::iterator iter = minorRoads.begin(); iter != minorRoads.end(); /**/)
	{
		if ((*iter)->markedForDeletion)
		{
			// Erase
			iter = minorRoads.erase(iter);
		}
		else
		{
			iter++;
		}
	}

	std::cout << "Neighbouring minor roads " << std::endl;

	// Now that we've deleted all of these roads, neighbour the remaining roads
	for (auto minorRoad : minorRoads)
	{
		// Against every other road
		for (auto otherRoad : minorRoads)
		{
			// Skip itself
			if (*minorRoad == *otherRoad) { continue; }

			// Check for neighbours

			// If otherRoad has minorRoad.Start
			if (*minorRoad->Start() == *otherRoad->Start()
				||
				*minorRoad->Start() == *otherRoad->End())
			{
				minorRoad->startChildren.push_back(minorRoad);
			}

			// If otherRoad has minorRoad.End
			if (*minorRoad->End() == *otherRoad->Start()
				||
				*minorRoad->End() == *otherRoad->End())
			{
				minorRoad->endChildren.push_back(minorRoad);
			}
		}
	}

	std::cout << "Removing minor roads with zero neighbours" << std::endl;

	// Remove any minor roads that have 0 neighbours (and thus are sitting along)
	for (std::vector<MinorRoad*>::iterator iter = minorRoads.begin(); iter != minorRoads.end(); /**/)
	{
		// no neighbours on BOTH sides, it has to have 0 end AND 0 start
		if ((*iter)->endChildren.size() == 0 && (*iter)->startChildren.size() == 0)
		{
			iter = minorRoads.erase(iter);
		}
		else
		{
			iter++;
		}
	}
}

void RoadNetwork::GenerateBuildingLots()
{
	std::cout << "Generating building lots on minor roads" << std::endl;
	int lotCounter = 0;
	for (MinorRoad* road : minorRoads)
	{
		std::cout << "\rGenerating lot of road: " << lotCounter++ << "\t\t";
		road->GenerateBuildingLots();
	}
	std::cout << std::endl;
}

void RoadNetwork::ValidateBuildingLots(WaterData* waterData)
{
	std::cout << "Checking for building lot collisions..." << std::endl;

	/* MINOR ROADS */

	/* Remove any buildingLots ontop of water */
	for (MinorRoad* minorRoad : minorRoads)
	{
		for (BuildingLot* lot : minorRoad->lots)
		{
			if (waterData->IsWater(lot->minX, lot->minY)
				||
				waterData->IsWater(lot->minX, lot->maxY)
				||
				waterData->IsWater(lot->maxX, lot->minY)
				||
				waterData->IsWater(lot->maxX, lot->maxY))
			{
				lot->markForDeletion = true;
			}
		}
	}

	/* First, we prioritive collisions against minor roads and major roads */
	std::cout << "Between minor roads..." << std::endl;
	// For every building lot
	for (MinorRoad* minorRoad : minorRoads)
	{
		for (BuildingLot* lot : minorRoad->lots)
		{
			// Check every minor road
			for (MinorRoad* otherRoad : minorRoads)
			{
				// Skip itself
				if (*minorRoad == *otherRoad) { continue; }

				// Check if each line of the lot intersects with the otherRoad
				for (Line* line : lot->lines)
				{
					if (Utility::GetIntersectionPointWithFiniteLines(line->start, line->end, otherRoad->Start(), otherRoad->End()) != nullptr)
					{
						lot->markForDeletion = true;
					}
				}
			}
		}
	}

	std::cout << "Between major roads..." << std::endl;
	int majorRoadsChecked = 0;
	// For every building lot
	for (MinorRoad* minorRoad : minorRoads)
	{
		for (BuildingLot* lot : minorRoad->lots)
		{
			// For every major road
			for (MajorRoad* majorRoad : majorRoads)
			{
				// Check each segment of the major road
				for (auto seg : majorRoad->segments)
				{
					// For each line of the buildnig lot
					for (auto line : lot->lines)
					{
						if (Utility::GetIntersectionPointWithFiniteLines(line->start, line->end, seg->Start(), seg->End()) != nullptr)
						{
							lot->markForDeletion = true;
						}
					}
				}
			}
		}
	}

	int minorLotDeletionCounter = 0;
	for (MinorRoad* road : minorRoads)
	{
		std::vector<BuildingLot*> lots = road->lots;

		for (std::vector<BuildingLot*>::iterator it = lots.begin(); it != lots.end(); /**/)
		{
			if ((*it)->markForDeletion)
			{
				it = lots.erase(it);
				minorLotDeletionCounter++;
			}
			else
				++it;
		}
		road->lots = lots;
	}

	/* Next, we check for collisions against other building lots */

	std::cout << "Between other building lots..." << std::endl;
	// Check every road against every other road
	for (MinorRoad* minorRoad : minorRoads)
	{
		for (MinorRoad* otherRoad : minorRoads)
		{
			// Do not check the road against itself
			if (*minorRoad == *otherRoad) { continue; }

			// Check each lot of each road
			for (BuildingLot* lot : minorRoad->lots)
			{
				for (BuildingLot* otherLot : otherRoad->lots)
				{
					// If the lots are far away we do not need to check for interceptions
					if (Vec2::DistanceBetween(*lot->bottomLeft, *otherLot->bottomLeft) > 15)
					{
						continue;
					}

					if (lot->IsLotWithin(otherLot))
					{
						lot->markForDeletion = true;
						otherLot->markForDeletion = true;
					}
				}
			}
		}
	}

	for (MinorRoad* road : minorRoads)
	{
		std::vector<BuildingLot*> lots = road->lots;

		for (std::vector<BuildingLot*>::iterator it = lots.begin(); it != lots.end(); /**/)
		{
			if ((*it)->markForDeletion)
			{
				it = lots.erase(it);
				minorLotDeletionCounter++;
			}
			else
				++it;
		}
		road->lots = lots;
	}
}

void RoadNetwork::GenerateBuildings()
{

}


/*
 * Copyright 2012-2019 CNRS-UM LIRMM, CNRS-AIST JRL
 */

// includes
// std
#include <iostream>

// boost
#define BOOST_TEST_MODULE BodyTest
#include <boost/test/unit_test.hpp>

// SpaceVecAlg
#include <SpaceVecAlg/SpaceVecAlg>

// RBDyn
#include "RBDyn/Body.h"

BOOST_AUTO_TEST_CASE(BodyTest)
{
  using namespace Eigen;
  using namespace sva;
  using namespace rbd;

  double mass = 1.;
  Matrix3d I;
  I << 1., 2., 3., 2., 1., 4., 3., 4., 1.;
  Vector3d h = Vector3d::Random() * 100.;

  RBInertiad rbi(mass, h, I);

  // Test first constructor
  Body b1(rbi, "b1");

  BOOST_CHECK_EQUAL(b1.name(), "b1");
  BOOST_CHECK_EQUAL(b1.inertia().mass(), rbi.mass());
  BOOST_CHECK_EQUAL(b1.inertia().momentum(), rbi.momentum());
  BOOST_CHECK_EQUAL(b1.inertia().inertia(), rbi.inertia());

  // Test second constructor
  Body b2(mass, Vector3d::UnitX(), I, "b2");

  BOOST_CHECK_EQUAL(b2.name(), "b2");
  BOOST_CHECK_EQUAL(b2.inertia().mass(), mass);
  BOOST_CHECK_EQUAL(b2.inertia().momentum(), mass * Vector3d::UnitX());
  BOOST_CHECK_EQUAL(b2.inertia().inertia(), I);

  // Test operator==
  BOOST_CHECK_EQUAL(b1, b1);
  BOOST_CHECK_NE(b1, b2);

  // Test operator!=
  BOOST_CHECK(!(b1 != b1));
  BOOST_CHECK(b1 != b2);
}


#include <boost/accumulators/statistics/rolling_mean.hpp>


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_IEEE_FUNCTIONS_SIMD_COMMON_NEXTAFTER_HPP_INCLUDED
#define BOOST_SIMD_IEEE_FUNCTIONS_SIMD_COMMON_NEXTAFTER_HPP_INCLUDED
#include <boost/simd/ieee/functions/nextafter.hpp>
#include <boost/simd/include/functions/simd/is_equal.hpp>
#include <boost/simd/include/functions/simd/is_not_equal.hpp>
#include <boost/simd/include/functions/simd/is_greater.hpp>
#include <boost/simd/include/functions/simd/seladd.hpp>
#include <boost/simd/include/functions/simd/if_else.hpp>
#include <boost/simd/include/functions/simd/next.hpp>
#include <boost/simd/include/functions/simd/prev.hpp>
#include <boost/simd/include/functions/simd/oneplus.hpp>
#include <boost/simd/include/functions/simd/minusone.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::nextafter_, tag::cpu_
                            , (A0)(X)
                            , ((simd_<arithmetic_<A0>,X>))((simd_<arithmetic_<A0>,X>))
                            )
  {

    typedef A0 result_type;

    BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    {
      return select(boost::simd::is_equal(a0,a1),
                    a0,
                    select(gt(a1,a0),oneplus(a0),minusone(a0)));
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::nextafter_, tag::cpu_
                            , (A0)(X)
                            , ((simd_<floating_<A0>,X>))((simd_<floating_<A0>,X>))
                            )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    {
      return select(lt(a0, a1), next(a0), select(eq(a0, a1),  a0, prev(a0)));
    }
  };
} } }

#endif


#include <iostream>
#include <vector>
#include <fstream>
#include <utility>
#include <iterator>
#include <cstdlib>
#include <string>
#include <limits>
#include <cerrno>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/graph_concepts.hpp>
#include <boost/concept_check.hpp>
#include <boost/operators.hpp>
#include <boost/iterator.hpp>
#include <boost/multi_array.hpp>
#include <boost/random.hpp>
#include <sys/time.h>
#include "../src/edmonds_optimum_branching.hpp"
#include "boost/tuple/tuple.hpp"
#include <boost/numeric/ublas/matrix_sparse.hpp>
#include <boost/numeric/ublas/io.hpp>
using boost::tuple;
using namespace std;
using namespace boost;

#ifdef DEBUG
#define DEBUG_MSG(str) do { std::cout << str << std::endl; } while( false )
#else
#define DEBUG_MSG(str) do { } while ( false )
#endif

// definitions of a complete graph that implements the EdgeListGraph
// concept of Boost's graph library.
namespace boost {
	struct complete_graph {
		complete_graph(int n_vertices) : n_vertices(n_vertices) {}
		int n_vertices;

		struct edge_iterator : public input_iterator_helper<edge_iterator, int, std::ptrdiff_t, int const *, int>
		{
			int edge_idx, n_vertices;
			edge_iterator() : edge_idx(0), n_vertices(-1) {}
			edge_iterator(int n_vertices, int edge_idx) : edge_idx(edge_idx), n_vertices(n_vertices) {}
			edge_iterator &operator++()
			{
				if (edge_idx >= n_vertices * n_vertices)
					return *this;
				++edge_idx;
				if (edge_idx / n_vertices == edge_idx % n_vertices)
					++edge_idx;
				return *this;
			}
			int operator*() const {return edge_idx;}
			bool operator==(const edge_iterator &iter) const
			{
				return edge_idx == iter.edge_idx;
			}
		};
	};

	template<>
	struct graph_traits<complete_graph> {
		typedef int                             vertex_descriptor;
		typedef int                             edge_descriptor;
		typedef directed_tag                    directed_category;
		typedef disallow_parallel_edge_tag      edge_parallel_category;
		typedef edge_list_graph_tag             traversal_category;
		typedef complete_graph::edge_iterator   edge_iterator;
		typedef unsigned                        edges_size_type;

		static vertex_descriptor null_vertex() {return -1;}
	};

	pair<complete_graph::edge_iterator, complete_graph::edge_iterator>
	edges(const complete_graph &g)
	{
		return make_pair(complete_graph::edge_iterator(g.n_vertices, 1),
						 complete_graph::edge_iterator(g.n_vertices, g.n_vertices*g.n_vertices));
	}

	unsigned
	num_edges(const complete_graph &g)
	{
		return (g.n_vertices - 1) * (g.n_vertices - 1);
	}

	int
	source(int edge, const complete_graph &g)
	{
		return edge / g.n_vertices;
	}

	int
	target(int edge, const complete_graph &g)
	{
		return edge % g.n_vertices;
	}
}

typedef graph_traits<complete_graph>::edge_descriptor Edge;
typedef graph_traits<complete_graph>::vertex_descriptor Vertex;

int dirMST(string fin,string fout,string mode)
{
	int n_vertices;
	vector< tuple<int,int,float> > edge_list;
	tuple<int,int,float> g_edge;

	string line;
	int v1,v2,ctr = 0;
	float weight;
	ifstream inputf (fin.c_str());
	float* nodeWeights = NULL;
	if(inputf.is_open())
	{
		getline(inputf,line);
		sscanf(line.c_str(),"%d",&n_vertices);
		nodeWeights = new float[n_vertices];
		int ctr = 0;
		while(getline(inputf,line))
		{
			if(ctr<n_vertices)
			{
				sscanf(line.c_str(),"%f",&nodeWeights[ctr]);
				DEBUG_MSG("v|"<<ctr+1<<"|:"<<nodeWeights[ctr]);
				ctr++;
			}
			else
			{
				sscanf (line.c_str(),"%d,%d,%f",&v1,&v2,&weight);
				//Assumes vertices start from 0...N-1
				edge_list.push_back(make_tuple(v1,v2,weight));
			}
		}
		inputf.close();
	}
	else
	{
		cerr<<"Input file not found. Cannot be opened\n";
		exit(1);
	}

	//Read in the edgelist and set the weights to their values, all other weights will
	//be -Inf
	//2 dimensional array of integer weights -Converting to float, make sure ok

	//Build a complete graph on n vertices (hack)
	//TODO: Fix to build it based on the graph structure
	complete_graph g(n_vertices);
	multi_array<float, 2> weights(extents[n_vertices][n_vertices]);
	vector<Vertex> parent(n_vertices);
	//Vertex roots[] = {0, 1};
	Vertex* roots = new Vertex[n_vertices];
	for(int vi = 0;vi<n_vertices;vi++)
		roots[vi]=vi;

	vector<Edge> branching;
	//Initialize all weights to 0
	for(int i=0;i<n_vertices;i++)
	{
		for(int j=0;j<n_vertices;j++)
		{
			//If using Minimum Spanning Tree
			if(mode.compare("min")==0)
				weights[i][j] = numeric_limits<int>::max();
			else//Maximum spanning tree
				weights[i][j] = numeric_limits<int>::min();
		}
	}

	//Write out the indices of the edge set that form the directed spanning tree
	numeric::ublas::mapped_matrix<int> index_map (n_vertices,n_vertices);
	DEBUG_MSG("---Graph Read---\nN: "<<n_vertices);
	BOOST_FOREACH(g_edge,edge_list)
	{
		v1 = get<0>(g_edge);
		v2 = get<1>(g_edge);
		weight=get<2>(g_edge);
		DEBUG_MSG(v1<<" | "<<v2<<" | "<<weight);
		weights[v1][v2] = weight;
		index_map(v1,v2) = ctr++;
	}
	bool isMax = true;
	if(mode.compare("min")==0)
	{
		isMax = false;
		//TOptimum isMaximum set to false -> MinST
		//TOptimum isMaximum set to true -> MaxST
	}
	vector< vector< Edge > > rooted_branching(n_vertices);

	double optVal = numeric_limits<int>::max();
	if(isMax)
		optVal = numeric_limits<int>::min();
	int optRoot= -1;
	double branchingWeight;
	for(int vi=0;vi<n_vertices;vi++)
	{
		//Compute rooted branching
		if(isMax)
		{
			edmonds_optimum_branching<true, true, true>
			(g, identity_property_map(), weights.origin(),
			 roots+vi, roots + vi+1, back_inserter(rooted_branching[vi]));
		}
		else
		{
		edmonds_optimum_branching<false, true, true>
			(g, identity_property_map(), weights.origin(),
			 roots+vi, roots + vi+1, back_inserter(rooted_branching[vi]));
		}
		//Track the weight of the branching
		branchingWeight = nodeWeights[vi];
		BOOST_FOREACH(Edge e,rooted_branching[vi])
		{
			branchingWeight += weights[source(e, g)][target(e, g)];
		}
		//Track the max/min branching weight
		if(isMax && branchingWeight>optVal)
		{
			optVal = branchingWeight;
			optRoot = vi;
		}
		if(!isMax && branchingWeight<optVal)
		{
			optVal = branchingWeight;
			optRoot = vi;
		}
	}

	//Write result to file
	DEBUG_MSG("--Optimal ("<<mode<<") Branching--");
	ofstream outf(fout.c_str());
	if(outf==NULL)
	{
		cerr<<"Output file not created."<<endl;
		exit(1);
	}
	BOOST_FOREACH(Edge e ,rooted_branching[optRoot])
	{
		v1 =source(e, g);
		v2 =target(e, g);
		ctr = index_map(v1,v2);

		DEBUG_MSG(v1<<"->"<<v2<<" idx:"<<ctr);
		outf<<ctr<<endl;
	}
	delete roots;
	delete nodeWeights;
	return EXIT_SUCCESS;

}
string PNAME = "mstwrapper";
int main(int argc, char *argv[])
{
	if (argc != 4)
	{
		cerr << "Usage: " << PNAME
			 << " <input file name> <output file name> <min|max>\n";
		exit(1);
	}
	string mode = string(argv[3]);
	if(mode.compare("min")==0 || mode.compare("max")==0)//strcmp(argv[3],"min")==0 || strcmp(argv[3],"max"){
	{
		return dirMST(argv[1],argv[2],mode);
	}
	else
	{
		cerr<<"Final argument not <min> or <max>"<<endl;
		exit(1);
	}

}


#include <iostream>
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/common/common_headers.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/console/parse.h>
#include <boost/program_options.hpp>

#include <algorithm>
#include <functional>
#include <math.h>       /* pow */
#include <cstdlib>
#include <string>
#include <map>
//#include "../include/viz.h"
#include "../include/helper.h"
#include "../include/sampling.h"
#include "../include/segmentation.h"
#include "../include/voxel.h"
#include "../include/cycleTimer.h"

// Types
typedef pcl::PointXYZRGB PointT;

void showhelpinfo(char *s)
{
  cout<<"Usage:   "<<s<<" [-option] [argument]"<<endl;
  cout<<"option:  "<<"-h  show help information"<<endl;
  cout<<"         "<<"-n number of points"<<endl;
  cout<<"         "<<"-m number of samples"<<endl;
  cout<<"         "<<"-a voxelization grid size in sampling step"<<endl;
  cout<<"         "<<"-b voxelization grid size in segmentation step"<<endl;
}

int loadPC(pcl::PointCloud<pcl::PointXYZRGB>::Ptr pc, std::string filename){

  if (pcl::io::loadPCDFile<pcl::PointXYZRGB> (filename, *pc) == -1) //* load the file
  {
    PCL_ERROR ("Couldn't read file test_pcd.pcd \n");
    return (-1);
  }
  return (1);
}

int
main (int argc, char** argv)
{

  int num_samples_in;
  float grid_size_1;
  float grid_size_2;
  int num_pts_in;
  int help_bool = 0;
  int full_file = 1;
  // default:

  if(argc == 1)
    {
      printf("no args!, running default parameters \n");
      num_samples_in = 100000;
      grid_size_1 = 20;
      grid_size_2 = 0.12;
      full_file = 0;
      num_pts_in = 1000000;
      cout<<" number of pts input: "<<num_pts_in<<endl;
      cout<<" number of samples: "<<num_samples_in<<endl;
      cout<<" grid size in sampling: "<<grid_size_1<<endl;
      cout<<" grid size in segmentation: "<<grid_size_2<<endl;
    }
  else{
    char tmp;
    while((tmp=getopt(argc,argv,"hn:m:a:b:"))!=-1)
      {
	switch(tmp)
	  {
	    /*option h : help infomation*/
	  case 'h':
	    showhelpinfo(argv[0]);
	    help_bool = 1;
	    break;
	    /*option n : set number of input points to read */
	  case 'n':
	    num_pts_in = atoi(optarg);
	    full_file = 0;
	    cout<<" number of pts input: "<<optarg<<endl;
	    break;
	    /*option m : set number of samples */
	  case 'm':
	    num_samples_in = atoi(optarg);
	    cout<<" number of samples: "<<optarg<<endl;
	    break;
	    /*option a : set sampling grid size*/
	  case 'a':
	    grid_size_1 = atof(optarg);
	    cout<<" grid size in sampling: "<<optarg<<endl;
	    break;
	    /*option b: set segmentation grid size */
	  case 'b':
	    grid_size_2 = atof(optarg);
	    cout<<" grid size in segmentation: "<<optarg<<endl;
	    break;
	  default:
	    showhelpinfo(argv[0]);
	    break;
	  }
      }
  }
  if(help_bool == 1){
    return(0);
  }

  double readTime = 0.f;
  double samplingTime = 0.f;
  double segmentTime = 0.f;
  double totalTime = 0.f;

  double startTime = CycleTimer::currentSeconds();

  /************************** STAGE 0: INITIALIZATION ****************************************/

  printf("reading point cloud file! \n");
  //  std::string filename("/Users/rohan/Dropbox/Work/workspace/pointcloud/data/rgbd-scenes_aligned/kitchen_small/kitchen_small_1/kitchen_small_1.pcd");
  std::string filename("/afs/andrew.cmu.edu/usr18/rohanv/data/kitchen_small_1.pcd");
  pcl::PointCloud<pcl::PointXYZRGB>::Ptr pc (new pcl::PointCloud<pcl::PointXYZRGB>);

  if(loadPC(pc,filename)){
    std::cout << "Loaded "
    << pc->width * pc->height
    << " data points from kitchen_small_1.pcd"
    << std::endl;
  }

  if(full_file == 1){
    num_pts_in = pc->size();
    cout<<" number of pts_input: "<<num_pts_in<<endl;
  }
  int bool_viz_or = 0;
  int bool_viz_rs = 0;

  if(bool_viz_or){
    printf("visualization of original point cloud! \n");
    //  pc_viz(pc);
  }

  printf("finished reading point cloud file! \n");
  double endReadTime = CycleTimer::currentSeconds();

  /************* STAGE 1 : SAMPLING ************/

  /* STEP 1:
     For all i, points:
     In single step, compute (iterative sum/reduction) for all j neighbors of i, A_i,j * x_j, and sum. then || x_i - sum || is weight
     A_i,j is a kernel: RBF/Gaussian/Epachanikov. Store in importance vector. Normalize.
     Simultaneously compute density. (can do this later since only need this for sampled points)
  */
  // tuning_parameters

  float sigma_sq = 0.00005;
  //  int numNbrs = 50;
  int numNbrs = (int) grid_size_1;
  int num_pts = num_pts_in;
    // compute importance weights
  std::vector<float> imp_wt(num_pts,0.0);
  computeWeights(pc, num_pts, sigma_sq, numNbrs,imp_wt);
  printf("computed weights\n");

  /* STEP 2:
     Randomly Sample K points according to weights , and construct new resampled pointcloud.
   */

// get sample indices

  int total_samples = num_samples_in;
  std::vector<int> sampleIdx = weightedRandomSample(imp_wt,num_pts,total_samples);
  printf("randomly sampled %d points \n" ,total_samples);

// construct resampled point cloud from sample indices:
  pcl::PointCloud<pcl::PointXYZRGB>::Ptr pc_rs (new pcl::PointCloud<pcl::PointXYZRGB>);
  resamplePC(pc,pc_rs,sampleIdx,total_samples);
  printf("constructed resampled point cloud \n");

  printf("visualization of resampled point cloud!\n");
  if(bool_viz_rs){
    //    pc_viz(pc_rs);
  }

  double endSamplingTime = CycleTimer::currentSeconds();

  /*********** STAGE 2 : SEGMENTATION *************/
   // STEP 3:
   //   Compute P_n = sum A'_{i,j} for all i in resampled point cloud. A'_{i,j} is another kernel.

  /* STEP 4:
     Do segmentation procedure for each i and link to highest parent.
   */
  int num_pts_rs = total_samples;
  float sigma_sq_seg = 0.00005;
  int numNbrs_seg = 50;

  std::vector<float> p_density(num_pts_rs,0.0);
  segmentation_computeDensity(pc_rs,num_pts_rs,sigma_sq_seg,numNbrs_seg,p_density);
    printf("computed density \n");

  std::vector<int> parents(num_pts_rs,0);
  std::vector<float> distances(num_pts_rs,0.0);

 //segmentation parameter:
float  tau = grid_size_2;
  //  float tau = 0.1;
  segmentation_linkNeighbors(pc_rs,num_pts_rs, tau, p_density, parents,distances);
  constructSegments(pc_rs,num_pts_rs,parents,distances);

  //print parents:
  int num_trees = 0;
  for (int i =0 ; i<num_pts_rs; i++){
    //printf("i:%d , parent:%d \n",i,parents[i]);
    if(parents[i]==i){
      num_trees++;
    }
  }
  printf("finished segmentation! \n");
  printf("number of segments: %d \n", num_trees);

  double endTime = CycleTimer::currentSeconds();

  readTime               = 1000.f * (endReadTime - startTime);
  samplingTime           = 1000.f * (endSamplingTime - endReadTime);
  segmentTime            = 1000.f * (endTime - endSamplingTime);
  totalTime              = 1000.f * (endTime - startTime);

  printf("Time for reading input PCD file:              %.4f ms\n", readTime);
  printf("Time for sampling:                            %.4f ms\n", samplingTime);
  printf("Time for segmentation:                        %.4f ms\n", segmentTime);
  printf("Total time:                                   %.4f ms\n", totalTime);

/************* visualize segments: **********/

// construct segmented point cloud:

  pcl::PointCloud<pcl::PointXYZRGB>::Ptr pc_seg (new pcl::PointCloud<pcl::PointXYZRGB>);
  constructSegmentedPC(pc_seg,pc_rs,num_pts_rs,parents);

  int bool_viz_seg = 0;
  printf("visualization of segmented point cloud!\n");
  if(bool_viz_seg){
    //  pc_viz(pc_seg);
  }
  pcl::io::savePCDFileASCII ("seg_pcd.pcd", *pc_seg);

  return (0);

}


/* Generated from orogen/lib/orogen/templates/tasks/Task.cpp */

#include "PointCloudAggregator.hpp"
#include <fstream>
#include <boost/archive/polymorphic_binary_oarchive.hpp>

//#include <vizkit3d/Vizkit3DWidget.hpp>
//#include "MLSMapVisualization.hpp"
//#include <vizkit3d/GridVisualization.hpp>

using namespace ::maps;

PointCloudAggregator::PointCloudAggregator(std::string const& name)
    : PointCloudAggregatorBase(name)
{
}

PointCloudAggregator::PointCloudAggregator(std::string const& name, RTT::ExecutionEngine* engine)
    : PointCloudAggregatorBase(name, engine)
{
}

PointCloudAggregator::~PointCloudAggregator()
{
}

void PointCloudAggregator::bodyPoseCallback(const base::Time &ts, const ::base::samples::RigidBodyState &bodyPose_sample)
{
    //throw std::runtime_error("Aggregator callback for bodyPose not implemented");
    lastPose = bodyPose_sample;
    std::cout << "B" << std::flush;
}

void PointCloudAggregator::pointCloudCallback(const base::Time &ts, const ::base::samples::Pointcloud &pc_sample)
{
//    throw std::runtime_error("Aggregator callback for pointCloud not implemented");
    const std::vector<base::Point> &points = pc_sample.points;
    size_t N = points.size();
    std::cout << "C " << N << " at " << lastPose.getPose() << std::endl;

    base::Pose body2World = lastPose.getPose();
    Eigen::Affine3d scanner2World = body2World.toTransform() * _scanner_in_body.rvalue().getPose().toTransform();
    base::samples::Pointcloud out;
    ::maps::grid::PointCloud pc;
    pc.resize(N);
    out.points.reserve(N);
    for(size_t i=0; i<N; ++i)
    {
        out.points.push_back(scanner2World * points[i]);
        pc[i].getVector3fMap() = points[i].cast<float>();
    }
    _pointCloud_in_world.write(out);
    mls->mergePointCloud(pc, scanner2World);
    viz.updateData(*mls);

}

/// The following lines are template definitions for the various state machine
// hooks defined by Orocos::RTT. See PointCloudAggregator.hpp for more detailed
// documentation about them.

bool PointCloudAggregator::configureHook()
{
    if (! PointCloudAggregatorBase::configureHook())
        return false;
    using namespace ::maps::grid;
    // TODO get values from config
    Eigen::Vector2d res(0.1, 0.1);
    Vector2ui numCells(500, 500);

    MLSConfig mls_config;
    mls_config.updateModel = MLSConfig::SLOPE;
    mls_config.gapSize = 0.25f;
    mls_config.useNegativeInformation = !true;
    mls.reset(new MLSMapSloped(numCells, res, mls_config));
    mls->getLocalFrame().translation() << 0.5*mls->getSize()+Eigen::Vector2d(0.5,0.5), 0;

    return true;
}
bool PointCloudAggregator::startHook()
{
    if (! PointCloudAggregatorBase::startHook())
        return false;

//    app.start();
//
//    //create vizkit3d plugin
//    vizkit3d::MLSMapVisualization *mls_viz = new vizkit3d::MLSMapVisualization();
//    mls_viz->updateData(*mls);
//
//    //create vizkit3d widget
//    vizkit3d::Vizkit3DWidget *widget = app.getWidget();
//    // grid plugin
//    vizkit3d::GridVisualization *grid_viz = new vizkit3d::GridVisualization();
//    widget->addPlugin(grid_viz);
//    // add plugin
//    widget->addPlugin(mls_viz);

    return true;
}
void PointCloudAggregator::updateHook()
{
    PointCloudAggregatorBase::updateHook();
    return;
    std::cout << __PRETTY_FUNCTION__ << std::endl;
    base::samples::RigidBodyState rbs;
    while(_bodyPose.read(rbs)== RTT::NewData)
    {
        std::cout << ".";
    }
    base::samples::Pointcloud pc;
    while(_pointCloud.read(pc)== RTT::NewData)
    {
        std::cout << "Pointcloud\n";
    }
}
void PointCloudAggregator::errorHook()
{
    PointCloudAggregatorBase::errorHook();
}
void PointCloudAggregator::stopHook()
{
    std::cout << "stopHook, do serialization\n";
    std::ofstream stream("output_mls.bin", std::ios::binary);
    boost::archive::polymorphic_binary_oarchive oa(stream);
    oa << *mls;

    PointCloudAggregatorBase::stopHook();
}
void PointCloudAggregator::cleanupHook()
{
    PointCloudAggregatorBase::cleanupHook();
}


/* boost random/xor_combine.hpp header file
 *
 * Copyright Jens Maurer 2002
 * 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 for most recent version including documentation.
 *
 * $Id: xor_combine.hpp 60755 2010-03-22 00:45:06Z steven_watanabe $
 *
 */

#ifndef BOOST_RANDOM_XOR_COMBINE_HPP
#define BOOST_RANDOM_XOR_COMBINE_HPP

#include <iostream>
#include <cassert>
#include <algorithm> // for std::min and std::max
#include <boost/config.hpp>
#include <boost/limits.hpp>
#include <boost/static_assert.hpp>
#include <boost/cstdint.hpp>     // uint32_t
#include <boost/random/detail/config.hpp>

namespace boost {
namespace random {

/// \cond hide_private_members
#ifndef BOOST_NO_DEPENDENT_TYPES_IN_TEMPLATE_VALUE_PARAMETERS
  #define BOOST_RANDOM_VAL_TYPE typename URNG1::result_type
#else
  #define BOOST_RANDOM_VAL_TYPE uint32_t
#endif
/// \endcond

/**
 * Instantiations of @c xor_combine model a \pseudo_random_number_generator.
 * To produce its output it invokes each of the base generators, shifts
 * their results and xors them together.
 */
template<class URNG1, int s1, class URNG2, int s2
#ifndef BOOST_RANDOM_DOXYGEN
, BOOST_RANDOM_VAL_TYPE val = 0
#endif
>
class xor_combine
{
public:
  typedef URNG1 base1_type;
  typedef URNG2 base2_type;
  typedef typename base1_type::result_type result_type;

  BOOST_STATIC_CONSTANT(bool, has_fixed_range = false);
  BOOST_STATIC_CONSTANT(int, shift1 = s1);
  BOOST_STATIC_CONSTANT(int, shift2 = s2);

  /**
   * Constructors a @c xor_combine by default constructing
   * both base generators.
   */
  xor_combine() : _rng1(), _rng2()
  { }
  /**
   * Constructs a @c xor_combine by copying two base generators.
   */
  xor_combine(const base1_type & rng1, const base2_type & rng2)
    : _rng1(rng1), _rng2(rng2) { }
  /**
   * Constructs a @c xor_combine, seeding both base generators
   * with @c v.
   */
  xor_combine(const result_type & v)
    : _rng1(v), _rng2(v) { }
  /**
   * Constructs a @c xor_combine, seeding both base generators
   * with values from the iterator range [first, last) and changes
   * first to point to the element after the last one used.  If there
   * are not enough elements in the range to seed both generators,
   * throws @c std::invalid_argument.
   */
  template<class It> xor_combine(It& first, It last)
    : _rng1(first, last), _rng2( /* advanced by other call */ first, last) { }
  /**
   * Calls @c seed() for both base generators.
   */
  void seed() { _rng1.seed(); _rng2.seed(); }
  /**
   * @c seeds both base generators with @c v.
   */
  void seed(const result_type & v) { _rng1.seed(v); _rng2.seed(v); }
  /**
   * seeds both base generators with values from the iterator
   * range [first, last) and changes first to point to the element
   * after the last one used.  If there are not enough elements in
   * the range to seed both generators, throws @c std::invalid_argument.
   */
  template<class It> void seed(It& first, It last)
  {
    _rng1.seed(first, last);
    _rng2.seed(first, last);
  }

  /** Returns the first base generator. */
  const base1_type& base1() { return _rng1; }
  /** Returns the second base generator. */
  const base2_type& base2() { return _rng2; }

  /** Returns the next value of the generator. */
  result_type operator()()
  {
    // MSVC fails BOOST_STATIC_ASSERT with std::numeric_limits at class scope
#if !defined(BOOST_NO_LIMITS_COMPILE_TIME_CONSTANTS) && !(defined(BOOST_MSVC) && BOOST_MSVC <= 1300)
    BOOST_STATIC_ASSERT(std::numeric_limits<typename base1_type::result_type>::is_integer);
    BOOST_STATIC_ASSERT(std::numeric_limits<typename base2_type::result_type>::is_integer);
    BOOST_STATIC_ASSERT(std::numeric_limits<typename base1_type::result_type>::digits >= std::numeric_limits<typename base2_type::result_type>::digits);
#endif
    return (_rng1() << s1) ^ (_rng2() << s2);
  }

  /**
   * Returns the smallest value that the generator can produce.
   */
  result_type min BOOST_PREVENT_MACRO_SUBSTITUTION () const { return std::min BOOST_PREVENT_MACRO_SUBSTITUTION((_rng1.min)(), (_rng2.min)()); }
  /**
   * Returns the largest value that the generator can produce.
   */
  result_type max BOOST_PREVENT_MACRO_SUBSTITUTION () const { return std::max BOOST_PREVENT_MACRO_SUBSTITUTION((_rng1.min)(), (_rng2.max)()); }
  static bool validation(result_type x) { return val == x; }

#ifndef BOOST_NO_OPERATORS_IN_NAMESPACE

#ifndef BOOST_RANDOM_NO_STREAM_OPERATORS
  template<class CharT, class Traits>
  friend std::basic_ostream<CharT,Traits>&
  operator<<(std::basic_ostream<CharT,Traits>& os, const xor_combine& s)
  {
    os << s._rng1 << " " << s._rng2 << " ";
    return os;
  }

  template<class CharT, class Traits>
  friend std::basic_istream<CharT,Traits>&
  operator>>(std::basic_istream<CharT,Traits>& is, xor_combine& s)
  {
    is >> s._rng1 >> std::ws >> s._rng2 >> std::ws;
    return is;
  }
#endif

  friend bool operator==(const xor_combine& x, const xor_combine& y)
  { return x._rng1 == y._rng1 && x._rng2 == y._rng2; }
  friend bool operator!=(const xor_combine& x, const xor_combine& y)
  { return !(x == y); }
#else
  // Use a member function; Streamable concept not supported.
  bool operator==(const xor_combine& rhs) const
  { return _rng1 == rhs._rng1 && _rng2 == rhs._rng2; }
  bool operator!=(const xor_combine& rhs) const
  { return !(*this == rhs); }
#endif

private:
  base1_type _rng1;
  base2_type _rng2;
};

#ifndef BOOST_NO_INCLASS_MEMBER_INITIALIZATION
//  A definition is required even for integral static constants
template<class URNG1, int s1, class URNG2, int s2, BOOST_RANDOM_VAL_TYPE val>
const bool xor_combine<URNG1, s1, URNG2, s2, val>::has_fixed_range;
template<class URNG1, int s1, class URNG2, int s2, BOOST_RANDOM_VAL_TYPE val>
const int xor_combine<URNG1, s1, URNG2, s2, val>::shift1;
template<class URNG1, int s1, class URNG2, int s2, BOOST_RANDOM_VAL_TYPE val>
const int xor_combine<URNG1, s1, URNG2, s2, val>::shift2;
#endif

#undef BOOST_RANDOM_VAL_TYPE

} // namespace random
} // namespace boost

#endif // BOOST_RANDOM_XOR_COMBINE_HPP


// Describes an image using a bag of words search framework. The image
// is effectively a sparse array of "term" counts with optional sets
// of (x,y) coordinates in the image associated with that term.
//
// Author: Mark Desnoyer (markd@cmu.edu)
// Date: July 2010

#include "ImageDocument.h"

#include <math.h>
#include <ros/ros.h>
#include <limits>
#include <boost/math/special_functions/fpclassify.hpp>

using namespace std;
using boost::shared_ptr;

namespace species_id {

const string ImageDocument::EMPTY_STRING("");

// Add a single term to the document with optional (x,y)
// location. If the same term is already in the document, the value
// is added to the existing sum and the (x,y) is added to the list.
//
// Inputs:
// id - ID of the term to add
// val - weight of the term to add to the vector
// (x,y) - optional coordinates in the image where the term was found
void ImageDocument::AddTermImpl(TermID id, const Term* termPtr,
                                float val, float x, float y) {
  if ((termPtr && (fabs(termPtr->val()) < 10*numeric_limits<float>::epsilon() &&
                   (termPtr->coords() == NULL or termPtr->coords()->size() == 0))) ||
      (!termPtr && val < 10*numeric_limits<float>::epsilon() &&
       !boost::math::isfinite(x) && !boost::math::isfinite(y))) {
    // No point in adding a zero term
    return;
  }

  isNormalized_ = false;
  length_ = LENGTH_NOT_CALCULATED;

  TermMap::iterator loc = terms_.find(id);
  if (loc == terms_.end()) {
    // Term is not in the document, so add it
    if (termPtr) {
      terms_[id] = termPtr->copy();
    } else {
      terms_[id] = Term::Ptr(new Term(val, x, y));
    }
  } else {
    // Term is already in the document, so add to it
    if (termPtr) {
      loc->second->AddTerm(*termPtr);
    } else {
      loc->second->AddTerm(val, x, y);
    }
  }
}

void ImageDocument::SetTerm(TermID id, float val) {
  bool isZero = fabs(val) < 10*numeric_limits<float>::epsilon();

  isNormalized_ = false;
  length_ = LENGTH_NOT_CALCULATED;

  TermMap::iterator loc = terms_.find(id);
  if (loc == terms_.end()) {
    // Term is not in the document, so add it
    if (!isZero) {
      terms_[id] = shared_ptr<Term>(new Term(val));
    }
  } else {
    // Term is already in the document, so set it
    loc->second->SetVal(val);
  }
}

void ImageDocument::AddTermsFromDoc(const ImageDocument& doc,
                                    TermID offset,
                                    float minVal) {
  for (TermMap::const_iterator termI = doc.begin();
       termI != doc.end();
       ++termI) {
    if (termI->second->val() > minVal) {
      AddTerm(termI->first + offset, *termI->second);
    }
  }
}

// Normalizes the terms so that they sum to 1
void ImageDocument::Normalize() {
  if (isNormalized_) {
    return;
  }

  double sum = 0.0;

  // Figure out the total sum of the terms
  for (TermMap::const_iterator i = begin();
       i != end();
       ++i) {
    sum += i->second->val();
  }

  if (fabs(sum) < numeric_limits<float>::epsilon()*10) {
    // Sum is zero, so we'll get a nan. Leave things along
    isNormalized_ = true;
    length_ = LENGTH_NOT_CALCULATED;
    return;
  }

  // Now update the vector
  double factor = 1. / sum;
  (*this) *= factor;

  isNormalized_ = true;
  length_ = LENGTH_NOT_CALCULATED;
}

float ImageDocument::length() const {
  if (length_ < 0) {
    double sum = 0.0;

    // Figure out the term by term sum
    for (TermMap::const_iterator i = begin();
         i != end();
         ++i) {
      sum += (i->second->val() * i->second->val()) ;
    }
    length_ = sqrt(sum);
  }

  return length_;
}

// Term by term multiplication
ImageDocument& ImageDocument::operator*=(const vector<float>& v) {
  isNormalized_ = false;
  length_ = LENGTH_NOT_CALCULATED;

  // Now update the vector
  for (TermMap::iterator i = terms_.begin();
       i != terms_.end();
       ++i) {
    TermID id = i->first;
    double val = 0.0;
    if (id < v.size()) {
      val = v[id];
    } else {
      ROS_WARN_STREAM_ONCE(
        "Vector to multiply by is missing an entry for id: " << id);
    }
    *(i->second) *= val;
  }
}

ImageDocument& ImageDocument::operator*=(double v) {
  isNormalized_ = false;
  length_ = LENGTH_NOT_CALCULATED;

  for (TermMap::iterator i = terms_.begin();
       i != terms_.end();
       ++i) {
    *(i->second) *= v;
  }
}

float ImageDocument::GetVal(TermID id) const {
  Term* term = GetTerm(id);
  if (term == NULL) {
    return 0.0;
  }
  return term->val();
}

const std::deque<Term::Coord>* ImageDocument::GetCoords(TermID id) const {
  Term* term = GetTerm(id);
  if (term == NULL || term->coords() == NULL) {
    return NULL;
  }
  return term->coords();
}

shared_ptr<ImageDocument> ImageDocument::copy() const {
  shared_ptr<ImageDocument> copy(new ImageDocument());

  copy->isNormalized_ = isNormalized_;

  for (TermMap::const_iterator i = begin();
       i != end();
       ++i) {
    copy->terms_[i->first] = i->second->copy();
  }

  copy->label_ = label_;
  copy->metaData_ = metaData_;

  return copy;
}

Term* ImageDocument::GetTerm(TermID id) const {
  TermMap::const_iterator loc = terms_.find(id);
  if (loc == terms_.end()) {
    return NULL;
  }
  return loc->second.get();
}

void ImageDocument::RemoveTermsBelowThresh(float thresh) {
  for(TermMap::iterator i = terms_.begin();
      i != terms_.end();) {
    if (i->second->val() < thresh) {
      i = terms_.erase(i);
    } else {
      ++i;
    }
  }
}

}


// a sorting example that uses the worst-case distribution for spreadsort.
//
//  Copyright Steven Ross 2009-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/sort for library home page.

#include <boost/sort/spreadsort/spreadsort.hpp>
#include <time.h>
#include <stdio.h>
#include <stdlib.h>
#include <algorithm>
#include <vector>
#include <string>
#include <fstream>
#include <sstream>
#include <iostream>
using namespace boost::sort::spreadsort;
using namespace std;

#define DATA_TYPE boost::uint64_t

#define ALR_THRESHOLD 3

const unsigned max_count = ALR_THRESHOLD - 1;
const unsigned bit_shift = detail::rough_log_2_size(max_count) -
  detail::int_log_mean_bin_size;
const unsigned radix_threshold = detail::rough_log_2_size(max_count) + 1;

const DATA_TYPE typed_one = 1;

void
fill_vector(vector<DATA_TYPE> & input, const DATA_TYPE base_value,
            unsigned remaining_bits, const vector<unsigned> & indices,
            int index)
{
  if (index < 0) {
    for (unsigned u = 0; u < max_count; ++u)
      input.push_back((base_value << remaining_bits) +
                      (rand() % (1 << remaining_bits)));
  }
  else {
    unsigned shift = indices[index];
    fill_vector(input, (base_value << shift) + ((1 << shift) - 1),
                remaining_bits - shift, indices, index - 1);
    fill_vector(input, base_value << shift, remaining_bits - shift, indices,
                index - 1);
  }
}

//Generates a random index from 0 up to but not including count.
unsigned
get_index(unsigned count)
{
  unsigned result = unsigned((rand() % (1 << 16))*uint64_t(count)/(1 << 16));
  if (result >= count)
    return count - 1;
  return result;
}

//Tests std::sort vs boost::sort::spreadsort on boost::sort's worst distribution.
int main(int, const char **) {
  unsigned total_length = sizeof(DATA_TYPE) * 8;
  double std_sort_time = 0;
  double spreadsort_time = 0;
  for (int repetition = 0; repetition < 10; ++repetition) {
    vector<DATA_TYPE> input;
    vector<unsigned> offsets;
    unsigned bit_length = total_length - radix_threshold;
    unsigned bit_offset = bit_shift;
    for (; bit_length >= ++bit_offset; bit_length -= bit_offset)
      offsets.push_back(bit_offset);
    for (int ii = (1 << bit_length) - 1; ii >= 0; --ii)
      fill_vector(input, ii, total_length - bit_length,
                  offsets, offsets.size() - 1);

    //Randomize the inputs slightly so they aren't in reverse-sorted order, for
    //which std::sort is very fast.
    for (unsigned u = 0; u < input.size() / 10; ++u)
      std::swap(input[get_index(input.size())], input[get_index(input.size())]);

    //Run both std::sort and boost::sort::spreadsort.
    for (unsigned u = 0; u < 2; ++u) {
      vector<DATA_TYPE> array = input;
      clock_t start, end;
      double elapsed;
      start = clock();
      if (u)
        std::sort(array.begin(), array.end());
      else
        boost::sort::spreadsort::spreadsort(array.begin(), array.end());
      end = clock();
      elapsed = static_cast<double>(end - start);
      if (u)
        std_sort_time += elapsed / CLOCKS_PER_SEC;
      else
        spreadsort_time += elapsed / CLOCKS_PER_SEC;
      array.clear();
    }
  }

  printf("std::sort elapsed time %f\n", std_sort_time);
  printf("spreadsort elapsed time %f\n", spreadsort_time);
  return 0;
}


#include <ros/ros.h>
#include <Eigen/Eigen>

#include <prometheus_drl/agent_state.h>

#include "printf_utils.h"

prometheus_drl::agent_state agent_state;
Eigen::MatrixXd obstacle_matrix;
Eigen::MatrixXd agent_matrix;
Eigen::MatrixXd goal_matrix;
Eigen::MatrixXd nei_goal_matrix;

void state_cb(const prometheus_drl::agent_stateConstPtr& msg)
{
    agent_state = *msg;

    obstacle_matrix.resize(agent_state.matrix_size,agent_state.matrix_size);
    agent_matrix.resize(agent_state.matrix_size,agent_state.matrix_size);
    goal_matrix.resize(agent_state.matrix_size,agent_state.matrix_size);
    nei_goal_matrix.resize(agent_state.matrix_size,agent_state.matrix_size);

    for(int i = 0; i < agent_state.matrix_size; i++)
    {
        for(int j = 0; j < agent_state.matrix_size; j++)
        {
            obstacle_matrix(i,j) = agent_state.obstacle_matrix[i*agent_state.matrix_size+j];
            agent_matrix(i,j) = agent_state.agent_matrix[i*agent_state.matrix_size+j];
            goal_matrix(i,j) = agent_state.goal_matrix[i*agent_state.matrix_size+j];
            nei_goal_matrix(i,j) = agent_state.nei_goal_matrix[i*agent_state.matrix_size+j];
        }
    }
}

void printf_cb(const ros::TimerEvent &e)
{
    cout << GREEN << "obstacle_matrix: " << TAIL <<endl;
    cout << GREEN <<  obstacle_matrix  << TAIL <<endl;
    cout << GREEN << "agent_matrix: " << TAIL <<endl;
    cout << GREEN <<  agent_matrix  << TAIL <<endl;
    cout << GREEN << "goal_matrix: " << TAIL <<endl;
    cout << GREEN <<  goal_matrix  << TAIL <<endl;
    cout << GREEN << "nei_goal_matrix: " << TAIL <<endl;
    cout << GREEN <<  nei_goal_matrix  << TAIL <<endl;
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "read_agent_state");
    ros::NodeHandle nh("~");

    string agent_prefix;
    // 模型前缀 - 默认为 ugv，无人机则设置为 uav
    nh.param<string>("agent_prefix", agent_prefix, "/ugv");

    string agent_name = agent_prefix + std::to_string(1);

    ros::Subscriber agent_state_sub = nh.subscribe<prometheus_drl::agent_state>(agent_name + "/agent_state", 1, state_cb);

    ros::Timer printf_timer = nh.createTimer(ros::Duration(5.0), printf_cb);

    ros::spin();

    return 0;
}