#pragma once

#include "primitives.h"
#include "ant.h"
#include "world.h"
#include "utils/custom_console.h"

#include <boost/thread/thread.hpp>
#include <iostream>
#include <ostream>

namespace Ant
{
	Ant ant;
}

namespace World
{
	World world;
}

std::ostream &operator<<(std::ostream &os, World::World& world)
{
	for (char x = 0; x < world.trail.size(); ++x) {
		for (char y = 0; y < world.trail[x].size(); ++y) {
			os << world.trail[x][y] << " ";
		}

		os << "\n";
	}

	return os;
}

std::ostream &operator<<(std::ostream &os, Ant::Ant& ant)
{
	Console::SetCursorPosition(ant.x * 2, ant.y);
	os << ant.Draw();
	boost::this_thread::sleep(boost::posix_time::milliseconds(100));

	return os;
}

namespace Primitives
{
	Function tset[] = { Move, Right, Left }; // terminal set
	Function fset[] = { IfFoodAhead, Prog2, Prog3 }; // function set

	void SkipSubtree()
	{
		int not_end_of_subtree = 1;
		while (not_end_of_subtree > 0) {
			not_end_of_subtree += ArityMin1(*current_node++);
		}
	}

	int Primitives::IsFoodAhead()
	{
		char x = Ant::ant.x + Ant::ant.facing.first,
			 y = Ant::ant.y + Ant::ant.facing.second;

		if (x < 0)
			x = World::world.width - 1;
		else if (x > World::world.width - 1)
			x = 0;

		if (y < 0)
			y = World::world.height - 1;
		else if (y > World::world.height - 1)
			y = 0;

		return World::world.HasFood(x, y);
	}

	int IfFoodAhead()
	{
		if (IsFoodAhead()) {
			Next();
			++current_node;
		}
		else {
			SkipSubtree();
			Next();
		}

		return 0;
	}

	int Move()
	{
		if (display) {
			Console::SetCursorPosition(Ant::ant.x * 2, Ant::ant.y);
			std::cout << ".";
		}

		if (Ant::ant.steps_left-- > 0) {

			Ant::ant.x += Ant::ant.facing.first;
			Ant::ant.y += Ant::ant.facing.second;

			if (Ant::ant.x < 0)
				Ant::ant.x = World::world.width - 1;
			else if (Ant::ant.x > World::world.width - 1)
				Ant::ant.x = 0;

			if (Ant::ant.y < 0)
				Ant::ant.y = World::world.height - 1;
			else if (Ant::ant.y > World::world.height - 1)
				Ant::ant.y = 0;

			if (World::world.HasFood(Ant::ant.x, Ant::ant.y)) {
				World::world.RemovePiece(Ant::ant.x, Ant::ant.y);
				Ant::ant.food_eaten += 1;
			}

			if (display && Ant::ant.food_eaten < World::world.num_food_pieces)
				std::cout << Ant::ant;
		}

		return 0;
	}

	int Right()
	{
		if (Ant::ant.steps_left > 0)
			++Ant::ant;

		if (display && Ant::ant.food_eaten < World::world.num_food_pieces)
			std::cout << Ant::ant;

		return 0;
	}

	int Left()
	{
		if (Ant::ant.steps_left > 0)
			--Ant::ant;

		if (display && Ant::ant.food_eaten < World::world.num_food_pieces)
			std::cout << Ant::ant;

		return 0;
	}

	int Prog2()
	{
		Next();
		Next();

		return 0;
	}

	int Prog3()
	{
		Next();
		Next();
		Next();

		return 0;
	}

	float Evaluate(std::string program)
	{
		World::world.Refresh();
		Ant::ant.Spawn();

		int best_steps = Ant::ant.steps_left;
		int best_num_eaten = World::world.num_food_pieces;

		auto normalise_steps = [&best_steps](int steps_left)->float {return (float)steps_left / best_steps; };
		auto normalise_num_eaten = [&best_num_eaten](int num_pieces_eaten)->float {return (float)num_pieces_eaten / best_num_eaten; };

		while (Ant::ant.steps_left > 0) {
			current_node = program.begin();
			Next();
		}

		float food_weight = 0.7, steps_weight = 0.3;

		return 1 - food_weight * normalise_num_eaten(Ant::ant.food_eaten) - steps_weight * normalise_steps(Ant::ant.steps_left);
	}

	void Save(std::string program, std::string fullpath)
	{
		std::fstream program_file(fullpath, std::ios::out);

		std::unordered_map<char, std::string> tokens;
		tokens.emplace(MOVE, "MOVE");
		tokens.emplace(RIGHT, "RIGHT");
		tokens.emplace(LEFT, "LEFT");
		tokens.emplace(IF_FOOD_AHEAD, "IF-FOOD-AHEAD");
		tokens.emplace(PROG2, "PROG2");
		tokens.emplace(PROG3, "PROG3");

		for (int i = 0; i < program.size(); ++i)
			program_file << tokens[program[i]] << "\n";

		program_file.close();
	}

	std::string Load(std::string fullpath)
	{
		std::fstream program_file(fullpath);

		std::unordered_map<std::string, char> reverse_tokens;
		reverse_tokens.emplace("MOVE", MOVE);
		reverse_tokens.emplace("RIGHT", RIGHT);
		reverse_tokens.emplace("LEFT", LEFT);
		reverse_tokens.emplace("IF-FOOD-AHEAD", IF_FOOD_AHEAD);
		reverse_tokens.emplace("PROG2", PROG2);
		reverse_tokens.emplace("PROG3", PROG3);

		std::string line, program;
		while (true) {
			getline(program_file, line);
			if (program_file.eof())
				break;
			program += reverse_tokens[line];
		}

		program_file.close();

		return program;
	}

	void Test(std::string program_path, bool visualise)
	{
		display = visualise;

		Console::Maximise();
		Console::HideCursor();

		std::cout << World::world;

		Evaluate(Load(program_path));

		Console::SetCursorPosition(0, World::world.height);

		printf("\nNo. pieces available: %d", World::world.num_food_pieces);
		printf("\nNo. pieces eaten: %d", Ant::ant.food_eaten);
		printf("\nDone!");
		getchar();
	}

	int ArityMin1(char x, char dummy)
	{
		if (x < Primitives::FSET_START)
			return -1;

		if (x == PROG3)
			return 2;

		return 1;
	}

	std::unordered_map<char, Function> terminals;
	std::unordered_map<char, Function> jumptable;
	std::string::const_iterator current_node;

	int Next()
	{
		return jumptable[*current_node++]();
	}

	int Terminal()
	{
		return terminals[*(current_node - 1)]();
	}

	void SetUp()
	{
		char i = Primitives::TSET_START;

		for (; i <= Primitives::TSET_END; ++i) {
			terminals.emplace(i, Primitives::tset[i - Primitives::TSET_START]);
			jumptable.emplace(i, Terminal);
		}

		for (i = Primitives::FSET_START; i <= Primitives::FSET_END; ++i)
			jumptable.emplace(i, Primitives::fset[i - Primitives::FSET_START]);
	}

	bool display = false;
}


/*~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~\
|  Phycas: Python software for phylogenetic analysis                          |
|  Copyright (C) 2006 Mark T. Holder, Paul O. Lewis and David L. Swofford     |
|                                                                             |
|  This program is free software; you can redistribute it and/or modify       |
|  it under the terms of the GNU General Public License as published by       |
|  the Free Software Foundation; either version 2 of the License, or          |
|  (at your option) any later version.                                        |
|                                                                             |
|  This program is distributed in the hope that it will be useful,            |
|  but WITHOUT ANY WARRANTY; without even the implied warranty of             |
|  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the              |
|  GNU General Public License for more details.                               |
|                                                                             |
|  You should have received a copy of the GNU General Public License along    |
|  with this program; if not, write to the Free Software Foundation, Inc.,    |
|  51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.                |
\~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/

#ifndef SLICE_SAMPLER_HPP
#define SLICE_SAMPLER_HPP

#include <cstdlib>
#include <climits>
#include <cfloat>
#include "basic_lot.hpp"
#include "probability_distribution.hpp"
#include "xprobdist.hpp"
#include <boost/shared_ptr.hpp>
#include <boost/weak_ptr.hpp>
#if defined(PYTHON_ONLY)
#	include <boost/python/call_method.hpp>
#endif
typedef std::pair<double, double> ParamAndLnProb;
typedef std::pair<double, double> SliceInterval;

namespace phycas
{

struct SliceStats
	{
	SliceStats() : nsamples(0), value(0.0), width(0.0), diff(0.0), failed(0.0), evals(0.0) {}

	unsigned nsamples;
	double value;
	double width;
	double diff;
	double failed;
	double evals;
	};

// Note: AdHocDensity is defined in probablity_distribution.hpp
typedef boost::shared_ptr<AdHocDensity> FuncToSampleShPtr;

#undef WEAK_FUNCTOSAMPLE
#if defined(WEAK_FUNCTOSAMPLE)
typedef boost::weak_ptr<AdHocDensity> FuncToSampleWkPtr;
#endif

/*----------------------------------------------------------------------------------------------------------------------
|	Implements the univariate slice sampler described in Neal, Radford M. 2003. Slice sampling. Annals of Statistics
|	31:705-741.
*/
class SliceSampler
	{
	public:
								SliceSampler();
#if defined(WEAK_FUNCTOSAMPLE)
								SliceSampler(LotShPtr rnd, FuncToSampleWkPtr f);
#else
								SliceSampler(LotShPtr rnd, FuncToSampleShPtr f);
#endif
		virtual					~SliceSampler();

		double					Sample();
		std::vector<double>					DebugSample();

		double					OverrelaxedSample();
		std::vector<double>					DebugOverrelaxedSample();

#if defined(WEAK_FUNCTOSAMPLE)
		void					AttachFunc(FuncToSampleWkPtr f);
#else
		void					AttachFunc(FuncToSampleShPtr f);
#endif
		void					AttachRandomNumberGenerator(LotShPtr rnd);

		void					SetXValue(double x);
		void					SetMaxUnits(unsigned umax);

		void					SetSliceUnitWidth(double uwidth);
		double					GetSliceUnitWidth() const;

		double					AdaptSimple(double multiplier);
		double					AdaptNeal(double multiplier);
		void					AdaptYConditional(double from_ends, double multiplier);
		SliceInterval			FindSliceInterval(ParamAndLnProb x0, const double ln_y0, double tol, unsigned max_steps = UINT_MAX) const;
		double					CalcW(double y0) const;

		double					GetMode() const;
		double					GetLnDensityAtMode() const;

		double					GetLastSampledXValue();
		double					GetLastSampledYValue();
		double					GetSliceYValue();

		double					GetLnZero() const {return -DBL_MAX;}
        void                    UseDoublingMethod(bool d);

		// For diagnosing inefficiency
		//
		double					GetMinX();
		double					GetMaxX();
		double					GetOrigLeftEdgeOfSlice();
		double					GetOrigRightEdgeOfSlice();
		double					GetLeftEdgeOfSlice();
		double					GetRightEdgeOfSlice();
		unsigned				GetNumFuncEvals();
		unsigned				GetNumFailedSamples();
		unsigned				GetNumUnitsRequired();
		unsigned				GetNumSamples();

		SliceStats				SummarizeDiagnostics();
		void					ResetDiagnostics();

	protected:

		void					Init();
		ParamAndLnProb			GetNextSample(const ParamAndLnProb);
		ParamAndLnProb			GetNextOverrelaxedSample(const ParamAndLnProb);
		SliceInterval			BisectionSqueeze(double left, double lnf_left, double right, double lnf_right, const double ln_y0, double tol, unsigned max_steps) const;

#if defined(WEAK_FUNCTOSAMPLE)
		FuncToSampleWkPtr		func;				/**< is a functor representing the probability distribution to be sampled */
#else
		FuncToSampleShPtr		func;				/**< is a functor representing the probability distribution to be sampled */
#endif
		LotShPtr				r;					/**< is the random number generator */
		ParamAndLnProb			lastSampled;		/**< most recent valid sample and its relative density */
		double					w;					/**< unit size for interval I */
		unsigned				maxUnits;			/**< maximum number of units of size w to use for interval I (set to UINT_MAX for unlimited) */

		// These quantities are used for diagnostic purposes in GetNextSample()
		//
		double					orig_left_edge;		/**< x coordinate of left edge of most recent slice (before cropping) */
		double					orig_right_edge;	/**< x coordinate of right edge of most recent slice (before cropping) */
		double					left_edge;			/**< x coordinate of left edge of most recent slice (after cropping) */
		double					right_edge;			/**< x coordinate of right edge of most recent slice (after cropping) */
		double					ln_y;				/**< log of f(x) representing most recent slice */

		// For diagnosing inefficiency, all reset with call to ResetDiagnostics()
		//
		double					min_x;				/**< minimum x value tried */
		double					max_x;				/**< maximum x value tried */
		double					sumValues;			/**< sum of last `num_samples' sampled values */
		double					sumWidths;			/**< sum of last `num_samples' cropped slice widths (where slice width = right_edge - left_edge) */
		double					sumDiffs;			/**< sum of last `num_samples' differences between successively sampled values */
		unsigned				func_evals;			/**< counts number of function evaluations */
		unsigned				failed_samples;		/**< counts number of samples that failed because they were not in the slice */
		unsigned				realized_m;			/**< counts number of units, each of size w, that were required to bracket the slice */
		unsigned				num_samples;		/**< counts number of times GetNextSample called */
		std::vector<ParamAndLnProb>	most_recent;	/**< vector of all (x, lnx) pairs evaluated in most recent sampling effort */

		// These are needed for y-conditional adaptation
		bool					ycond_on;			/**< if true, w chosen anew for each sample based on y-coordinate of slice */
		double					ycond_a;
		double					ycond_b;
		double					ycond_multiplier;
		ParamAndLnProb			mode;

		// These are for overrelaxed sampling
		unsigned				num_overrelaxed_samples;	/**< counts number of times GetNextOverrelaxedSample called */

        bool                    doubling;           /**< if true, doubling method will be used to increase slice interval */
	};

typedef boost::shared_ptr<SliceSampler> SliceSamplerShPtr;

} // namespace phycas

#endif


/*--
    Combiner.cpp

    This file is part of the Cornucopia curve sketching library.
    Copyright (C) 2010 Ilya Baran (baran37@gmail.com)

    This program is free software: you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation, either version 3 of the License, or
    (at your option) any later version.

    This program is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details.

    You should have received a copy of the GNU General Public License
    along with this program.  If not, see <http://www.gnu.org/licenses/>.
*/

#include "Combiner.h"
#include "GraphConstructor.h"
#include "PrimitiveFitter.h"
#include "PrimitiveSequence.h"
#include "PathFinder.h"
#include "Preprocessing.h"
#include "Resampler.h"
#include "Polyline.h"
#include "Solver.h"
#include "ErrorComputer.h"
#include "Fitter.h"
#include "Oversketcher.h"
#include "PiecewiseLinearUtils.h"

#include <iterator>
#include <cstdio>

#include <Eigen/LU>
#include <Eigen/Cholesky>

using namespace std;
using namespace Eigen;
NAMESPACE_Cornu

#define SPARSE 1

class MulticurveDenseEvalData : public LSEvalData
{
public:
    //overrides
    double error() const { return _con.squaredNorm(); }

    void solveForDelta(double damping, Eigen::VectorXd &out, std::set<LSBoxConstraint> &constraints)
    {
        size_t vars = _errDer.cols();

        size_t size = vars + _con.size() + constraints.size();
        MatrixXd lhs = MatrixXd::Zero(size, size);
        VectorXd rhs = VectorXd::Zero(size);

        lhs.block(0, 0, vars, vars) = _errDer.transpose() * _errDer;
        rhs.segment(0, vars) = -_errDer.transpose() * _err;

        lhs.block(vars, 0, _conDer.rows(), _conDer.cols()) = _conDer;
        lhs.block(0, vars, _conDer.cols(), _conDer.rows()) = _conDer.transpose();
        rhs.segment(vars, _con.size()) = -_con;

        int cnt = 0;
        for(set<LSBoxConstraint>::const_iterator it = constraints.begin(); it != constraints.end(); ++it, ++cnt)
            lhs(vars + _conDer.rows() + cnt, it->index) = lhs(it->index, vars + _conDer.rows() + cnt) = 1.;

        //lhs += damping * MatrixXd::Identity(size, size);
        lhs.block(0, 0, vars, vars) += damping * MatrixXd::Identity(vars, vars);
        VectorXd result = lhs.lu().solve(rhs);

        out = result.segment(0, vars);

#if 0
        printf("Solve err = %lf\n", (lhs * result - rhs).norm());
        if(_conDer.size() > 0)
            printf("Con Solve err = %lf\n", (_conDer * out + _con).norm());
#endif

        //check which constraints we don't need
        cnt = 0;
        for(set<LSBoxConstraint>::iterator it = constraints.begin(); it != constraints.end(); ++cnt)
        {
            set<LSBoxConstraint>::iterator next = it;
            ++next;
            if(result(vars + _conDer.rows() + cnt) * it->sign > 0)
            {
                //printf("Unsetting constraint on variable at index %d\n", it->index);
                constraints.erase(it);
            }
            it = next;
        }
    }

    //for derivative verification, combine error and constraints
    VectorXd errVec() const
    {
        VectorXd out(_err.size() + _con.size());
        out << _err, _con;
        return out;
    }

    MatrixXd errVecDer() const
    {
        MatrixXd out(_err.size() + _con.size(), _errDer.cols());
        out << _errDer, _conDer;
        return out;
    }

    VectorXd &errVectorRef() { return _err; }
    MatrixXd &errDerRef() { return _errDer; }

    VectorXd &conVectorRef() { return _con; }
    MatrixXd &conDerRef() { return _conDer; }

private:
    Eigen::VectorXd _err;
    Eigen::MatrixXd _errDer;
    Eigen::VectorXd _con;
    Eigen::MatrixXd _conDer;
};

class MulticurveSparseEvalData : public LSEvalData
{
public:
    typedef Matrix<double, Dynamic, Dynamic, 0, 6, 6> BlockType;
    typedef vector<BlockType, aligned_allocator<BlockType> > BlockVectorType;
    typedef LLT<BlockType> BlockCholType;
    typedef vector<BlockCholType, aligned_allocator<BlockCholType> > BlockCholVectorType;

    //overrides
    double error() const { return _con.squaredNorm(); }

    void solveForDelta(double damping, Eigen::VectorXd &out, std::set<LSBoxConstraint> &constraints)
    {
        _computeIndices();

        size_t vars = _blockIndices.back();
        size_t cons = _con.size() + constraints.size();

        size_t size = vars + cons;
        VectorXd rhs = VectorXd::Zero(size);
        rhs.segment(0, vars) = _err;
        rhs.segment(vars, _con.size()) = -_con;

        BlockCholVectorType cholBlocks(_errDerBlocks.size());
        for(size_t i = 0; i < cholBlocks.size(); ++i)
            cholBlocks[i] = BlockCholType(_errDerBlocks[i] + damping * MatrixXd::Identity(_blockSizes[i], _blockSizes[i]));

        MatrixXd C = MatrixXd::Zero(cons, vars);
        C.block(0, 0, _con.size(), vars) = _conDer;

        int cnt = 0;
        for(set<LSBoxConstraint>::const_iterator it = constraints.begin(); it != constraints.end(); ++it, ++cnt)
            C(_conDer.rows() + cnt, it->index) = 1.;

        MatrixXd K(C.cols(), C.rows());
        _solveCholL(K, cholBlocks, C.transpose());
        LLT<MatrixXd> CholK(K.transpose() * K);

        VectorXd mid(size), result(size);

        //solve for mid
        _solveCholL(mid.segment(0, vars), cholBlocks, rhs.segment(0, vars));
        mid.segment(vars, cons) = CholK.matrixL().solve(rhs.segment(vars, cons) - K.adjoint() * mid.segment(0, vars));

        //solve for result
        result.segment(vars, cons) = -CholK.matrixU().solve(mid.segment(vars, cons));
        _solveCholU(result.segment(0, vars), cholBlocks, mid.segment(0, vars) - K * result.segment(vars, cons));

        out = result.segment(0, vars);
#if 0
        if(_conDer.size() > 0)
            printf("Con Solve err = %lf\n", (_conDer * out + _con).norm());
#endif

        //check which constraints we don't need
        cnt = 0;
        for(set<LSBoxConstraint>::iterator it = constraints.begin(); it != constraints.end(); ++cnt)
        {
            set<LSBoxConstraint>::iterator next = it;
            ++next;
            if(result(vars + _conDer.rows() + cnt) * it->sign > 0)
            {
                //printf("Unsetting constraint on variable at index %d\n", it->index);
                constraints.erase(it);
            }
            it = next;
        }
    }

    VectorXd &errVectorRef() { return _err; }
    BlockVectorType &errDerBlocksRef() { return _errDerBlocks; }

    VectorXd &conVectorRef() { return _con; }
    MatrixXd &conDerRef() { return _conDer; }

private:
    void _computeIndices()
    {
        _blockIndices.resize(_errDerBlocks.size() + 1);
        _blockSizes.resize(_errDerBlocks.size());
        _blockIndices[0] = 0;

        for(int i = 0; i < (int)_errDerBlocks.size(); ++i)
        {
            _blockSizes[i] = _errDerBlocks[i].rows();
            _blockIndices[i + 1] = _blockIndices[i] + _blockSizes[i];
        }
    }

    MatrixXd _blocksToMatrix(const BlockVectorType &blocks)
    {
        size_t sz = _blockIndices.back();
        MatrixXd out = MatrixXd::Zero(sz, sz);

        for(int i = 0; i < (int)blocks.size(); ++i)
            out.block(_blockIndices[i], _blockIndices[i], _blockSizes[i], _blockSizes[i]) = blocks[i];

        return out;
    }

    template<class M1, class M2> void _solveCholL(const M1 &out, const BlockCholVectorType &chols, const M2 &rhs)
    {
        for(int i = 0; i < (int)chols.size(); ++i)
            const_cast<M1 &>(out).block(_blockIndices[i], 0, _blockSizes[i], out.cols()) =
                chols[i].matrixL().solve(rhs.block(_blockIndices[i], 0, _blockSizes[i], rhs.cols()));
    }

    template<class M1, class M2> void _solveCholU(const M1 &out, const BlockCholVectorType &chols, const M2 &rhs)
    {
        for(int i = 0; i < (int)chols.size(); ++i)
            const_cast<M1 &>(out).block(_blockIndices[i], 0, _blockSizes[i], out.cols()) =
                chols[i].matrixU().solve(rhs.block(_blockIndices[i], 0, _blockSizes[i], rhs.cols()));
    }

    vector<size_t> _blockIndices, _blockSizes;
    BlockVectorType _errDerBlocks;

    Eigen::VectorXd _err;
    Eigen::VectorXd _con;
    Eigen::MatrixXd _conDer;
};

class MulticurveProblem : public LSProblem
{
public:
    MulticurveProblem(const Fitter &fitter)
        : _primitives(fitter.output<PRIMITIVE_FITTING>()->primitives), _iter(0)
    {
        smart_ptr<const AlgorithmOutput<GRAPH_CONSTRUCTION> > graph = fitter.output<GRAPH_CONSTRUCTION>();
        const vector<int> &path = fitter.output<PATH_FINDING>()->path;
        _errorComputer = fitter.output<ERROR_COMPUTER>()->errorComputer;
        _closed = fitter.output<CURVE_CLOSING>()->closed;
        _inflectionAccounting = fitter.params().get(Parameters::INFLECTION_COST) > 0.;

        for(int i = 0; i < (int)path.size(); ++i)
        {
            _primIdcs.push_back(graph->edges[path[i]].startVtx);
            _continuities.push_back(graph->edges[path[i]].continuity);
        }
        if(!_closed)
            _primIdcs.push_back(graph->edges[path.back()].endVtx);

        _curves = VectorC<CurvePrimitivePtr>((int)_primIdcs.size(), _closed ? CIRCULAR : NOT_CIRCULAR);
        _curveRanges = VectorC<pair<int, int> >((int)_primIdcs.size(), _curves.circular());
        for(int i = 0; i < (int)_primIdcs.size(); ++i)
        {
            _curveRanges[i] = make_pair(_primitives[_primIdcs[i]].startIdx, _primitives[_primIdcs[i]].endIdx);
            _curves[i] = _primitives[_primIdcs[i]].curve->clone();
        }

        //trim curves and ranges
        const int sampledPts = fitter.output<RESAMPLING>()->output->pts().size();
        for(int i = 0; i < (int)_continuities.size(); ++i)
        {
            if(_continuities[i] == 0)
                continue;

            //trim
            Vector2d trimPt = 0.5 * (_curves[i]->endPos() + _curves[i + 1]->startPos());
            if(!_primitives[_primIdcs[i]].isFixed())
                _curves[i]->trim(0, _curves[i]->project(trimPt));
            if(_curves.circular() || !_primitives[_primIdcs[i + 1]].isFixed())
                _curves[i + 1]->trim(_curves[i + 1]->project(trimPt), _curves[i + 1]->length());

            //the ranges over which error is computed should not overlap
            if(_continuities[i] == 1)
                swap(_curveRanges[i].second, _curveRanges[i + 1].first); //they overlap by 1 originally
            if(_continuities[i] == 2) //they overlap by 2
            {
                _curveRanges[i + 1].first = (_curveRanges[i + 1].first + 2) % sampledPts;
                _curveRanges[i].second = (_curveRanges[i].second + sampledPts - 2) % sampledPts;
            }
        }
    }

    vector<LSBoxConstraint> getConstraints() const
    {
        vector<LSBoxConstraint> out;
        VectorXd curParams = params();

        int curVar = 0;
        for(int i = 0; i < _curves.size(); ++i)
        {
            int primIdx = _primIdcs[i];
            if(_primitives[primIdx].isFixed())
            {
                int n = _curves[i]->numParams();
                for(int j = 0; j < n; ++j, ++curVar)
                    out.push_back(LSBoxConstraint(curVar, curParams[curVar], 0));

                continue;
            }

            //length must be at least half initial
            out.push_back(LSBoxConstraint(curVar + CurvePrimitive::LENGTH, _curves[i]->length() * 0.5, 1));

            //inflections
            //Debugging::get()->printf("idx = %d Type = %d, startSign = %d, endSign = %d", i, _curves[i]->getType(), _primitives[primIdx].startCurvSign, _primitives[primIdx].endCurvSign);
            if(_inflectionAccounting && _curves[i]->getType() >= CurvePrimitive::ARC)
            {
                int pi = _curves.toLinearIdx(i - 1);
                int ni = _curves.toLinearIdx(i + 1);
                bool c2toPrev = (pi >= 0 && _continuities[pi] == 2);
                bool c2toNext = (ni < _curves.size() && _continuities[i] == 2);
                bool line2Prev = c2toPrev && _curves[pi]->getType() == CurvePrimitive::LINE;
                bool line2Next = c2toNext && _curves[ni]->getType() == CurvePrimitive::LINE;

                int startSign = _primitives[primIdx].startCurvSign;
                int endSign = _primitives[primIdx].endCurvSign;

                if(!c2toPrev || (endSign == _primitives[_primIdcs[pi]].startCurvSign && startSign == endSign && !line2Prev))
                    out.push_back(LSBoxConstraint(curVar + CurvePrimitive::CURVATURE, 0., startSign));

                if(c2toPrev && c2toNext && startSign != endSign) //constrain the "easier one" of the endpoints
                {
                    double startViolation = -_curves[i]->startCurvature() * startSign;
                    double endViolation = -_curves[i]->endCurvature() * endSign;

                    //if we're C2 with a line, don't constrain that curvature
                    if(line2Prev)
                        startViolation = Parameters::infinity;
                    if(line2Next)
                        endViolation = Parameters::infinity;

                    if(startViolation < endViolation)
                        out.push_back(LSBoxConstraint(curVar + CurvePrimitive::CURVATURE, 0., startSign));
                    else
                        out.push_back(LSBoxConstraint(curVar + CurvePrimitive::DCURVATURE, 0., endSign));
                }

                if(!c2toNext && _curves[i]->getType() == CurvePrimitive::CLOTHOID)
                    out.push_back(LSBoxConstraint(curVar + CurvePrimitive::DCURVATURE, 0., endSign));
            }

            curVar += _curves[i]->numParams();
        }

        return out;
    }

#if SPARSE
    typedef MulticurveSparseEvalData EvalDataType;
#else
    typedef MulticurveDenseEvalData EvalDataType;
#endif

    int _iter;
    LSEvalData *createEvalData() { return new EvalDataType(); }
    void eval(const Eigen::VectorXd &x, LSEvalData *data)
    {
        setParams(x);
        EvalDataType *evalData = static_cast<EvalDataType *>(data);
        _evalError(evalData);
        _evalConstraints(evalData);
        //printf("Err: obj = %lf con = %lf\n", evalData->errVectorRef().norm(), evalData->conVectorRef().norm());
    }

    void setParams(const Eigen::VectorXd &x)
    {
        int curIdx = 0;
        for(int i = 0; i < (int)_curves.size(); ++i)
        {
            if(_curves[i]->getType() != CurvePrimitive::CLOTHOID)
            {
                _curves[i]->setParams(x.segment(curIdx, _curves[i]->numParams()));
            }
            else
            {
                VectorXd xm = x.segment(curIdx, _curves[i]->numParams());
                xm(CurvePrimitive::DCURVATURE) = (xm(CurvePrimitive::DCURVATURE) - xm(CurvePrimitive::CURVATURE)) / xm(CurvePrimitive::LENGTH);
                _curves[i]->setParams(xm);
            }
            curIdx += _curves[i]->numParams();
        }

        if(false)
        {
            char name[100];
            sprintf(name, "Out%d", _iter);
            for(int i = 0; i < _curves.size(); ++i)
                Debugging::get()->drawPrimitive(_curves[i], name, i, 2.);
        }

        ++_iter;
    }

    VectorXd params() const
    {
        int totParams = 0;
        for(int i = 0; i < (int)_curves.size(); ++i)
            totParams += _curves[i]->numParams();

        VectorXd out(totParams);

        int curIdx = 0;
        for(int i = 0; i < (int)_curves.size(); ++i)
        {
            if(_curves[i]->getType() != CurvePrimitive::CLOTHOID)
            {
                out.segment(curIdx, _curves[i]->numParams()) = _curves[i]->params();
            }
            else
            {
                VectorXd xm = _curves[i]->params();
                xm(CurvePrimitive::DCURVATURE) = xm(CurvePrimitive::CURVATURE) + xm(CurvePrimitive::DCURVATURE) * xm(CurvePrimitive::LENGTH);
                out.segment(curIdx, _curves[i]->numParams()) = xm;
            }
            curIdx += _curves[i]->numParams();
        }

        return out;
    }

    VectorC<CurvePrimitiveConstPtr> curves() const
    {
        VectorC<CurvePrimitiveConstPtr> out;
        copy(_curves.begin(), _curves.end(), back_inserter(out));
        return out;
    }

    double objective() const
    {
        double out = 0;

        for(int i = 0; i < (int)_curves.size(); ++i)
        {
            int csz = (int)_continuities.size();
            bool firstCorner = (!_closed && i == 0) || (_continuities[(i + csz - 1) % csz] == 0);
            bool lastCorner = (!_closed && i + 1 == (int)_curves.size()) || (_continuities[i] == 0);

            out += _errorComputer->computeError(_curves[i], _curveRanges[i].first, _curveRanges[i].second, firstCorner, lastCorner);
        }

        return out;
    }

private:
    void _evalError(EvalDataType *evalData)
    {
        vector<VectorXd> errVecs(_curves.size());
        vector<MatrixXd> errVecDers(_curves.size());

        size_t numErr = 0, numVar = 0;
        for(int i = 0; i < (int)_curves.size(); ++i)
        {
            int csz = (int)_continuities.size();
            bool firstCorner = (!_closed && i == 0) || (_continuities[(i + csz - 1) % csz] == 0);
            bool lastCorner = (!_closed && i + 1 == (int)_curves.size()) || (_continuities[i] == 0);

            _errorComputer->computeErrorVector(_curves[i], _curveRanges[i].first, _curveRanges[i].second,
                errVecs[i], &(errVecDers[i]), firstCorner, lastCorner);

            _curves[i]->toEndCurvatureDerivative(errVecDers[i]);

            numErr += errVecs[i].size();
            numVar += _curves[i]->numParams();
        }

        VectorXd &outErr = evalData->errVectorRef();

#if SPARSE
        EvalDataType::BlockVectorType &outErrDerBlocks = evalData->errDerBlocksRef();
        outErrDerBlocks.resize(_curves.size());
        outErr = VectorXd::Zero(numVar);
#else
        outErr = VectorXd::Zero(numErr);
        MatrixXd &outErrDer = evalData->errDerRef();
        outErrDer = MatrixXd::Zero(numErr, numVar);
#endif

        size_t curErr = 0, curVar = 0;
        for(int i = 0; i < (int)_curves.size(); ++i)
        {
            size_t nErr = errVecs[i].size();
            size_t nVar = errVecDers[i].cols();
#if SPARSE
            outErrDerBlocks[i] = errVecDers[i].transpose() * errVecDers[i];
            outErr.segment(curVar, nVar) = -errVecDers[i].transpose() * errVecs[i];
#else
            outErrDer.block(curErr, curVar, nErr, nVar) = errVecDers[i];
            outErr.segment(curErr, nErr) = errVecs[i];
#endif
            curErr += nErr;
            curVar += nVar;
        }
    }

    void _evalConstraints(EvalDataType *evalData)
    {
        VectorXd &outCon = evalData->conVectorRef();
        MatrixXd &outConDer = evalData->conDerRef();

        vector<VectorXd> conVecs(_continuities.size());
        vector<MatrixXd> conVecDers(_continuities.size());

        CurvePrimitive::EndDer endDer;

        size_t numCon = 0, numVar = 0;
        for(int i = 0; i < (int)_continuities.size(); ++i)
        {
            conVecs[i].resize(2 + _continuities[i]);
            conVecs[i].head<2>() = _curves[i]->endPos() - _curves[i + 1]->startPos();
            if(_continuities[i] >= 1)
                conVecs[i][2] = AngleUtils::toRange(_curves[i]->endAngle() - _curves[i + 1]->startAngle(), -PI);
            if(_continuities[i] == 2)
                conVecs[i][3] = _curves[i]->endCurvature() - _curves[i + 1]->startCurvature();

            _curves[i]->derivativeAtEnd(_continuities[i], endDer);
            conVecDers[i] = endDer;
            _curves[i]->toEndCurvatureDerivative(conVecDers[i]);

            numCon += conVecs[i].size();
            numVar += _curves[i]->numParams();
        }

        if(!_closed)
            numVar += _curves.back()->numParams();

        outCon = VectorXd::Zero(numCon);
        outConDer = MatrixXd::Zero(numCon, numVar);

        size_t curCon = 0, curVar = 0;
        for(int i = 0; i < (int)_continuities.size(); ++i)
        {
            size_t nCon = conVecs[i].size();
            size_t nVar = conVecDers[i].cols();
            outCon.segment(curCon, nCon) = conVecs[i];
            outConDer.block(curCon, curVar, nCon, nVar) = conVecDers[i];

            //now the derivatives for the second curve
            outConDer(curCon + 0, (curVar + nVar + CurvePrimitive::X) % numVar) = -1.;
            outConDer(curCon + 1, (curVar + nVar + CurvePrimitive::Y) % numVar) = -1.;
            if(nCon > 2)
                outConDer(curCon + 2, (curVar + nVar + CurvePrimitive::ANGLE) % numVar) = -1.;
            if(nCon > 3 && _curves[i + 1]->getType() != CurvePrimitive::LINE)
                outConDer(curCon + 3, (curVar + nVar + CurvePrimitive::CURVATURE) % numVar) = -1.;

            curCon += nCon;
            curVar += nVar;
        }
    }

    VectorC<CurvePrimitivePtr> _curves;
    const vector<FitPrimitive> &_primitives;
    vector<int> _primIdcs;
    vector<int> _continuities; //continuity[i] is between curves i and i + 1
    VectorC<pair<int, int> > _curveRanges;
    bool _closed;
    ErrorComputerConstPtr _errorComputer;
    bool _inflectionAccounting;
};

class DefaultCombiner : public Algorithm<COMBINING>
{
public:
    string name() const { return "Default"; }

protected:
    void _run(const Fitter &fitter, AlgorithmOutput<COMBINING> &out)
    {
        smart_ptr<const AlgorithmOutput<GRAPH_CONSTRUCTION> > graph = fitter.output<GRAPH_CONSTRUCTION>();
        const vector<FitPrimitive> &primitives = fitter.output<PRIMITIVE_FITTING>()->primitives;
        const vector<int> &path = fitter.output<PATH_FINDING>()->path;
        bool closed = fitter.output<CURVE_CLOSING>()->closed;

        if(path.empty())
            return; //no path

        VectorC<CurvePrimitiveConstPtr> outV;

        //if a single primitive
        if(graph->edges[path[0]].continuity == -1)
        {
            outV = VectorC<CurvePrimitiveConstPtr>(1, NOT_CIRCULAR);
            outV[0] = primitives[graph->edges[path[0]].startVtx].curve;
        }
        else //solve the nonlinear problem
        {
            MulticurveProblem problem(fitter);
            vector<LSBoxConstraint> constraints = problem.getConstraints();
            LSSolver solver(&problem, constraints);
            solver.setDefaultDamping(fitter.params().get(Parameters::COMBINE_DAMPING));
            solver.setMaxIter(50);
            solver.setIncreaseDampingAfter(5);
            solver.setDampingIncreaseFactor(1.5);

            VectorXd result = solver.solve(problem.params());
            problem.setParams(result);
            Debugging::get()->printf("Final objective = %lf", sqrt(problem.objective()));

            outV = problem.curves();
        }

        //==== track what happens to parameters ====
        out.parameters = fitter.output<RESAMPLING>()->parameters;
        PolylineConstPtr resampledCurve = fitter.output<RESAMPLING>()->output;
        const VectorC<Vector2d> &resampled = resampledCurve->pts();

        vector<int> finalPrimitives; //gather the indices of the graph vertices corresponding to the primitives
        for(int i = 0; i < (int)path.size(); ++i)
            finalPrimitives.push_back(graph->edges[path[i]].startVtx);
        if(outV.size() > (int)finalPrimitives.size())
            finalPrimitives.push_back(graph->edges[path.back()].endVtx);

        assert(outV.size() == finalPrimitives.size());

        vector<double> idxToParam(resampled.size()); //idx is the index into the resampled array
        vector<double> idxToDistSq(resampled.size(), 1e10);

        double lenSoFar = 0;
        for(int i = 0; i < outV.size(); ++i) //for each primitive see what projects to it
        {
            const FitPrimitive &primitive = primitives[graph->vertices[finalPrimitives[i]].primitiveIdx];
            for(int j = primitive.startIdx; ; ++j) //project each associated resampled point onto this primitive
            {
                if(j == (int)resampled.size()) //be careful with starts and ends of oversketched primitives
                {
                    if(closed) j = 0;
                    else
                        break;
                }

                if(j < 0)
                    continue;

                double proj = outV[i]->project(resampled[j]);
                double distSq = (resampled[j] - outV[i]->pos(proj)).squaredNorm();
                if(distSq < idxToDistSq[j])
                {
                    idxToDistSq[j] = distSq;
                    idxToParam[j] = proj + lenSoFar;
                }

                if(j == primitive.endIdx)
                    break;
            }
            lenSoFar += outV[i]->length();
        }
        double outputLength = lenSoFar;

        if(!closed)
        {
            idxToParam[0] = 0;
            idxToParam.back() = outputLength;
        }

        int minParamSample = min_element(idxToParam.begin(), idxToParam.end()) - idxToParam.begin();

        PiecewiseLinearMonotone prevToFinal(PiecewiseLinearMonotone::POSITIVE);
        //populate prevToFinal -- don't forget duplicating first point if closed
        prevToFinal.add(0, idxToParam[minParamSample]);
        lenSoFar = 0;
        for(VectorC<Vector2d>::Circulator ci = resampled.circulator(minParamSample + 1); !ci.done(); ++ci)
        {
            lenSoFar += (*ci - *(ci - 1)).norm();
            double finalParam = 0;
            if(ci.index() == minParamSample)
                finalParam += outputLength;
            else
                idxToParam[ci.index()] = max(idxToParam[ci.index()], idxToParam[(ci - 1).index()]); //ensure monotonicity
            finalParam += idxToParam[ci.index()];
            prevToFinal.add(lenSoFar, finalParam);
        }
        //adjust parameters into range
        for(int i = 0; i < (int)out.parameters.size(); ++i)
        {
            out.parameters[i] -= resampledCurve->idxToParam(minParamSample);
            if(out.parameters[i] < 0)
                out.parameters[i] += resampledCurve->length();
        }
        prevToFinal.batchEval(out.parameters);

        //==== combine with what needs to be done w.r.t. oversketching ====
        smart_ptr<const AlgorithmOutput<OVERSKETCHING> > osOutput = fitter.output<OVERSKETCHING>();
        VectorC<CurvePrimitiveConstPtr> outFinal(0, osOutput->finallyClose ? CIRCULAR : NOT_CIRCULAR);

        if(osOutput->toPrepend)
        {
            outFinal.insert(outFinal.end(), osOutput->toPrepend->primitives().begin(), osOutput->toPrepend->primitives().end() - 1);
            for(int i = 0; i < (int)out.parameters.size(); ++i)
                out.parameters[i] += (osOutput->toPrepend->length() - osOutput->toPrepend->primitives().back()->length());
        }
        outFinal.insert(outFinal.end(), outV.begin(), outV.end());

        if(osOutput->toAppend)
        {
            if(!osOutput->finallyClose)
            {
                outFinal.insert(outFinal.end(), osOutput->toAppend->primitives().begin() + 1, osOutput->toAppend->primitives().end());
            }
            else
            {
                if(osOutput->toAppend->primitives().size() >= 2)
                {
                    outFinal.insert(outFinal.end(), osOutput->toAppend->primitives().begin() + 1, osOutput->toAppend->primitives().end() - 1);
                }
                else //start and end curve is the same curve -- its original length is the one toAppend curve
                {
                    //get rid of last curve and possibly extend the first one
                    double lastCurveLen = outFinal.back()->length();
                    double firstCurveLen = outFinal[0]->length();
                    double origLen = osOutput->toAppend->primitives()[0]->length();
                    outFinal.pop_back();
                    //now extend the first curve to the combined length
                    outFinal[0] = outFinal[0]->trimmed(origLen - lastCurveLen, firstCurveLen);
                    for(int i = 0; i < (int)out.parameters.size(); ++i)
                        out.parameters[i] -= (origLen - lastCurveLen);
                }
            }
        }

        out.output = new PrimitiveSequence(outFinal);

#if 1
        for(int i = 0; i < (int)out.parameters.size(); ++i)
        {
            double paramOrig = fitter.originalSketch()->idxToParam(i);
            Debugging::get()->drawLine(fitter.originalSketch()->pts()[i], out.output->pos(out.parameters[i]), Vector3d(1, 0, 1), "Correspondence");
        }
#endif

    }
};

void Algorithm<COMBINING>::_initialize()
{
    new DefaultCombiner();
}

END_NAMESPACE_Cornu


#include <target_object_detector.hpp>
#include <Eigen/Core>
#include <Eigen/Eigen>
#include <Eigen/Dense>
#include <algorithm>
#include "libsvm/svm.h"
#include <ros/package.h>
using namespace pcl;

EuclideanCluster::EuclideanCluster(ros::NodeHandle nh, ros::NodeHandle n)
  : nh_(nh),
    rate_(n.param("loop_rate", 10)),
    frame_id_(n.param<std::string>("clustering_frame_id", "base_link"))
{
  source_pc_sub_ = nh_.subscribe(n.param<std::string>("source_pc_topic_name", "/hokuyo3d/hokuyo_cloud2"), 1, &EuclideanCluster::EuclideanCallback, this);
  fileterd_cloud_pub_ = nh_.advertise<sensor_msgs::PointCloud2>(n.param<std::string>("filtered_pc_topic_name", "/filtered_pointcloud"), 1);
  euclidean_cluster_pub_ = nh_.advertise<jsk_recognition_msgs::BoundingBoxArray>(n.param<std::string>("box_name", "/clustering_result"), 1);

  // クラスタリングのパラメータを初期化
  n.param<double>("clusterTolerance", clusterTolerance_, 0.02);
  n.param<int>("minSize", minSize_, 100);
  n.param<int>("maxSize", maxSize_, 25000);
  // clopboxを当てはめるエリアを定義
  n.param<float>("crop_x_min", crop_min_.x, 5.5);
  n.param<float>("crop_x_max", crop_max_.x, -5.5);
  n.param<float>("crop_y_min", crop_min_.y, -5.5);
  n.param<float>("crop_y_max", crop_max_.y, 5.5);
  n.param<float>("crop_z_min", crop_min_.z, -0.1);
  n.param<float>("crop_z_max", crop_max_.z, 0.5);
  n.param<std::string>("svm_model_path", svm_model_path_,
                       ros::package::getPath("target_object_detector")
                       + "/model/train.csv.model");
  if((model_=svm_load_model(svm_model_path_.c_str()))==0)
  {
    fprintf(stderr,"can't open model file %s\n",svm_model_path_.c_str());
    exit(1);
  }
  n.param<std::string>("svm_range_path", svm_range_path_,
                       ros::package::getPath("target_object_detector")
                       + "/model/train.csv.range");
  read_scaling_parameters(svm_range_path_);
}

void EuclideanCluster::EuclideanCallback(
    const sensor_msgs::PointCloud2::ConstPtr &source_pc) {

  //点群をKinect座標系からWorld座標系に変換
  //変換されたデータはtrans_pcに格納される．
  sensor_msgs::PointCloud2 trans_pc;
  try {
    pcl_ros::transformPointCloud(frame_id_, *source_pc, trans_pc, tf_);
  } catch (tf::ExtrapolationException e) {
    ROS_ERROR("pcl_ros::transformPointCloud %s", e.what());
  }

  // sensor_msgs::PointCloud2 → pcl::PointCloud
  pcl::PointCloud<PointXYZI> pcl_source;
  pcl::fromROSMsg(trans_pc, pcl_source);
  pcl::PointCloud<PointXYZI>::Ptr pcl_source_ptr(new pcl::PointCloud<PointXYZI>(pcl_source));

  // 点群の中からnanを消す
  // std::vector<int> dummy;
  // pcl::removeNaNFromPointCloud(*pcl_source_ptr, *pcl_source_ptr, dummy);

  // Create the filtering object
  // pcl::StatisticalOutlierRemoval<pcl::PointXYZI> sor;
  // sor.setInputCloud (pcl_source_ptr);
  // sor.setMeanK (100);
  // sor.setStddevMulThresh (0.1);
  // sor.filter (*pcl_source_ptr);

  // 平面をしきい値で除去する→Cropboxで
  CropBox(pcl_source_ptr, crop_min_, crop_max_);

  // 処理後の点群をpublish
  // sensor_msgs::PointCloud2 filtered_pc2;
  // pcl::toROSMsg(*pcl_source_ptr, filtered_pc2);
  // filtered_pc2.header.stamp = ros::Time::now();
  // filtered_pc2.header.frame_id = "base_link";
  // fileterd_cloud_pub_.publish(filtered_pc2);

  // Creating the KdTree object for the search method of the extraction
  Clustering(pcl_source_ptr);
}

void EuclideanCluster::CropBox(pcl::PointCloud<pcl::PointXYZI>::Ptr cloud,
                               pcl::PointXYZ min, pcl::PointXYZ max) {
  Eigen::Vector4f minPoint;

  minPoint[0] = min.x; // define minimum point x
  minPoint[1] = min.y; // define minimum point y
  minPoint[2] = min.z; // define minimum point z

  Eigen::Vector4f maxPoint;
  maxPoint[0] = max.x; // define max point x
  maxPoint[1] = max.y; // define max point y
  maxPoint[2] = max.z; // define max point z

  Eigen::Vector3f boxTranslatation;
  boxTranslatation[0] = 0;
  boxTranslatation[1] = 0;
  boxTranslatation[2] = 0;

  Eigen::Vector3f boxRotation;
  boxRotation[0] = 0; // rotation around x-axis
  boxRotation[1] = 0; // rotation around y-axis
  boxRotation[2] = 0; // in radians rotation around z-axis. this rotates your

  Eigen::Affine3f boxTransform;

  pcl::CropBox<pcl::PointXYZI> cropFilter;
  cropFilter.setInputCloud(cloud);
  cropFilter.setMin(minPoint);
  cropFilter.setMax(maxPoint);
  cropFilter.setTranslation(boxTranslatation);
  cropFilter.setRotation(boxRotation);

  pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZI>());
  cropFilter.filter(*cloud_filtered);
  pcl::copyPointCloud<pcl::PointXYZI, pcl::PointXYZI>(*cloud_filtered, *cloud);
}

void EuclideanCluster::Clustering(pcl::PointCloud<pcl::PointXYZI>::Ptr cloud) {
  pcl::search::KdTree<pcl::PointXYZI>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZI>);
  tree->setInputCloud(cloud);

  std::vector<pcl::PointIndices> cluster_indices;
  pcl::EuclideanClusterExtraction<pcl::PointXYZI> ec;
  ec.setClusterTolerance(clusterTolerance_);
  ec.setMinClusterSize(minSize_);
  ec.setMaxClusterSize(maxSize_);
  ec.setSearchMethod(tree);
  ec.setInputCloud(cloud);
  ec.extract(cluster_indices);

  int j = 0;
  jsk_recognition_msgs::BoundingBoxArray box_array; // clustering結果をぶち込む配列

  for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin(); it != cluster_indices.end(); ++it) {
    pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_cluster(new pcl::PointCloud<pcl::PointXYZI>);
    for (std::vector<int>::const_iterator pit = it->indices.begin(); pit != it->indices.end(); ++pit){
      cloud_cluster->points.push_back(cloud->points[*pit]);
    }
    cloud_cluster->width = cloud_cluster->points.size();
    cloud_cluster->height = 1;
    cloud_cluster->is_dense = true;

    //特徴量の計算
    svm_node features[14];
    feature_calculation(cloud_cluster, features);
    ROS_INFO_STREAM("features are extracted");
    int is_target_object = static_cast<int>( svm_predict( model_, features ) );
    ROS_INFO_STREAM("is_target_object : " << is_target_object);
    if (is_target_object == 1) {
      // 一つのclusterをpushback
      jsk_recognition_msgs::BoundingBox box;
      box = MinAreaRect(cloud_cluster, j);
      box_array.boxes.push_back(box);
      j++;
    }
  }

  // int clusterLength = clusterIndices.size();
  ROS_INFO("Found %lu clusters:", cluster_indices.size());

  // publish
  box_array.header.stamp = ros::Time::now();
  box_array.header.frame_id = frame_id_;
  euclidean_cluster_pub_.publish(box_array);

  // Empty Buffer
  cluster_indices.clear();
}

jsk_recognition_msgs::BoundingBox EuclideanCluster::MinAreaRect(pcl::PointCloud<pcl::PointXYZI>::Ptr cloud, int cluster_cnt){
  // PCLによる点群の最大最小エリア取得
  pcl::MomentOfInertiaEstimation<pcl::PointXYZI> feature_extractor;
  feature_extractor.setInputCloud(cloud);
  feature_extractor.compute();

  std::vector<float> moment_of_inertia;
  std::vector<float> eccentricity;
  pcl::PointXYZI min_point_AABB;
  pcl::PointXYZI max_point_AABB;

  geometry_msgs::Pose pose;
  geometry_msgs::Vector3 size;

  feature_extractor.getMomentOfInertia(moment_of_inertia);
  feature_extractor.getEccentricity(eccentricity);
  feature_extractor.getAABB(min_point_AABB, max_point_AABB);

  // OpenCVで最小矩形を当てはめる
  std::vector<cv::Point2f> points;
  for(unsigned int i = 0; i < cloud->points.size(); i++){
    cv::Point2f p2d;
    p2d.x = cloud->points[i].x;
    p2d.y = cloud->points[i].y;
    points.push_back(p2d);
  }
  cv::Mat points_mat(points);
  cv::RotatedRect rrect = cv::minAreaRect(points_mat);

  // ROS_INFO("Center of mass (x, y) = (%f, %f)", rrect.center.x, rrect.center.y);
  // ROS_INFO("Height = %f Width =  %f", rrect.size.height, rrect.size.width);
  // ROS_INFO("Angle = %f [deg]", rrect.angle);

  // jsk_recognition_msgs::BoundingBoxの型に合わせて代入していく
  pose.position.x = rrect.center.x;
  pose.position.y = rrect.center.y;
  pose.position.z = (min_point_AABB.z + max_point_AABB.z) / 2.0;

  Eigen::Matrix3f AxisAngle;
  Eigen::Vector3f axis(0,0,1); //z 軸を指定
  AxisAngle = Eigen::AngleAxisf(rrect.angle*M_PI/180.0, axis); // z軸周りに90度反時計回りに回転
  Eigen::Quaternionf quat(AxisAngle); // クォータニオンに変換
  pose.orientation.x = quat.x();
  pose.orientation.y = quat.y();
  pose.orientation.z = quat.z();
  pose.orientation.w = quat.w();

  size.x = rrect.size.width;
  size.y = rrect.size.height;
  size.z = max_point_AABB.z - min_point_AABB.z;

  // TFの名前付け
  std::stringstream ss;
  std::string object_name;
  ss << cluster_cnt;
  object_name = "object_" + ss.str();

  br_.sendTransform(tf::StampedTransform(
      tf::Transform(
          tf::Quaternion(pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w),
          tf::Vector3(pose.position.x, pose.position.y, max_point_AABB.z)),
          ros::Time::now(), "world", object_name));

  jsk_recognition_msgs::BoundingBox box;
  box.header.frame_id = frame_id_;
  box.pose = pose;
  box.dimensions = size;
  box.label = cluster_cnt;

  return box;
}

void EuclideanCluster::run()
{
  while(nh_.ok()){
    ros::spinOnce();
    rate_.sleep();
  }
}

void EuclideanCluster::feature_calculation(pcl::PointCloud<pcl::PointXYZI>::Ptr cloud,
                         svm_node *features)
  {
    int n;
    float f21,f22,f23,f24,f25,f26;
    float v1, v2, v3;
    double v11, v22, v33;
    float f31,f32,f33,f34,f35,f36,f37;
    pcl::PointXYZI min_point;
    pcl::PointXYZI max_point;
    std::vector<float> moment_of_inertia;
    std::vector<float> eccentricity;
    Eigen::Matrix3f cmat;
    Eigen::Vector4f xyz_centroid;
    EIGEN_ALIGN16 Eigen::Matrix3f covariance_matrix;
    pcl::MomentOfInertiaEstimation <pcl::PointXYZI> feature_extractor;
    feature_extractor.setInputCloud (cloud);
    feature_extractor.compute ();
    feature_extractor.getMomentOfInertia (moment_of_inertia);
    feature_extractor.getEccentricity (eccentricity);
    feature_extractor.getAABB (min_point, max_point);
    pcl::compute3DCentroid (*cloud, xyz_centroid);
    pcl::computeCovarianceMatrix (*cloud, xyz_centroid, covariance_matrix);
    n = cloud->points.size();
    f21 = covariance_matrix (0,0) / (n - 1);
    f22 = covariance_matrix (0,1) / (n - 1);
    f23 = covariance_matrix (0,2) / (n - 1);
    f24 = covariance_matrix (1,1) / (n - 1);
    f25 = covariance_matrix (1,2) / (n - 1);
    f26 = covariance_matrix (2,2) / (n - 1);
    cmat (0,0) = f21;
    cmat (0,1) = f22;
    cmat (0,2) = f23;
    cmat (1,0) = f22;
    cmat (1,1) = f24;
    cmat (1,2) = f25;
    cmat (2,0) = f23;
    cmat (2,1) = f25;
    cmat (2,2) = f26;
    Eigen::EigenSolver<Eigen::MatrixXf> es(cmat, false);
    std::complex<double> lambda1 = es.eigenvalues()[0];
    std::complex<double> lambda2 = es.eigenvalues()[1];
    std::complex<double> lambda3 = es.eigenvalues()[2];
    v11 = lambda1.real();
    v22 = lambda2.real();
    v33 = lambda3.real();
    std::vector<double> v;
    v.push_back (v11);
    v.push_back (v22);
    v.push_back (v33);
    std::sort(v.begin(), v.end() );
    v3 = v[0];
    v2 = v[1];
    v1 = v[2];
    v.clear();

    double nnn = v1 * v2 * v3;
    f31 = (v1 - v2) / v1;
    f32 = (v2 - v3) / v1;
    f33 = v3 / v1;
    f34 = pow(nnn, 1.0 / 3.0);
    f35 = (v1 - v3) / v1;
    f36 = -(v1 * log(v1) + v2 * log(v2) + v3 * log(v3));
    f37 = v3 / (v1 + v2 + v3);

    features[0].value = f21;
    features[1].value = f22;
    features[2].value = f23;
    features[3].value = f24;
    features[4].value = f25;
    features[5].value = f26;
    features[6].value = f31;
    features[7].value = f32;
    features[8].value = f33;
    features[9].value = f34;
    features[10].value = f35;
    features[11].value = f36;
    features[12].value = f37;
    features[0].index = 1;
    features[1].index = 2;
    features[2].index = 3;
    features[3].index = 4;
    features[4].index = 5;
    features[5].index = 6;
    features[6].index = 7;
    features[7].index = 8;
    features[8].index = 9;
    features[9].index = 10;
    features[10].index = 11;
    features[11].index = 12;
    features[12].index = 13;
    features[13].index = -1;
    normlize_features(features);
    for (int i = 0; i < 13; ++i) {
      std::cout << features[i].value << ", ";
    }
    std::cout << "\n";

  }

void EuclideanCluster::normlize_features(svm_node *features)
{
  for (int i = 0; i < 13; ++i) {
    if (feature_max_[i] == feature_min_[i]) {
      continue;
    }
    if(features[i].value == feature_min_[i]){
      features[i].value = feature_lower_;
    }else if(features[i].value == feature_max_[i]){
      features[i].value = feature_upper_;
    }else{
      features[i].value = feature_lower_ + (feature_upper_ - feature_lower_)*
        (features[i].value - feature_min_[i])/
        (feature_max_[i] - feature_min_[i]);
    }
  }
}

//スケーリングバリューは今は直書きしてるけど余裕があれば読み込めるようにする
void EuclideanCluster::read_scaling_parameters(std::string scaling_parameter_file)
{
  //scaling_parameter_file = "/home/morita/Documents/dev/ros/tc2016_ws/src/TC2016_for_thirdrobot/target_obejct_detector/model/train.csv.range";

  std::ifstream ifs(scaling_parameter_file.c_str());
  std::string str;
  if (ifs.fail())
  {
      ROS_ERROR("failed to read scaling parameter file");
      return;
  }

  // first line
  if(!getline(ifs, str))
  {
      ROS_ERROR("failed to read first line on scaling parameter file");
      return;
  }

  // second line
  if(!getline(ifs, str))
  {
      ROS_ERROR("failed to read second line on scaling parameter file");
      return;
  }

  std::vector<std::string> splited = split(str, ' ');

  feature_lower_ = atof(splited[0].c_str());
  feature_upper_ = atof(splited[1].c_str());

  // after second line
  while(getline(ifs, str))
  {
    std::vector<std::string> splited = split(str, ' ');

    feature_min_.push_back(atof(splited[1].c_str()));
    feature_max_.push_back(atof(splited[2].c_str()));
  }
}

std::vector<std::string> EuclideanCluster::split(const std::string &s, char delim)
{
    std::vector<std::string> elems;
    std::stringstream ss(s);
    std::string item;
    while (getline(ss, item, delim)) {
    if (!item.empty()) {
            elems.push_back(item);
        }
    }
    return elems;
}


//
// Created by AIYOJ on 2019-03-04.
//

#include <gtest/gtest.h>
#include "calibration/calibration_daemon.h"
#include <boost/function.hpp>
#include "calibration/isotonic_regression.h"
#include "calibration/prior_correction.h"
#include <spdlog/spdlog.h>
#include <boost/filesystem/operations.hpp>
#include <calibration/hybrid.h>
#include "log/algocomp_log.h"

namespace algocomp_test {
    class TestCalibrationDaemon : public ::testing::Test {
    protected:
        std::unique_ptr<algocomp::CalibrationDaemon> _daemon;
        std::shared_ptr<spdlog::logger> _logger;

        TestCalibrationDaemon() {
            _logger = spdlog::get(algocomp::ACLog::_logger_name);
        }

        ~TestCalibrationDaemon() {
        }

    public:
        void thread_content() {
            bool continue_loop = true;
            while (continue_loop) {
                _logger->debug("test_calibration_daemon: pass one loop in thread content...");
                boost::this_thread::sleep(boost::posix_time::seconds(10));
            }
        }

    };

    TEST_F(TestCalibrationDaemon, load_prior_correction) {
        std::string dname = "test/test-data";
        std::string fname = "1-calibrations/1-prior-correction.txt";
        std::string type = "prior-correction";
        int update_interval = 1;
        _daemon.reset(new algocomp::CalibrationDaemon(dname, fname, type, update_interval));

        auto calibration = _daemon->load();
        auto const prior_calibration = dynamic_cast<algocomp::PriorCorrection *>(calibration.get());
        EXPECT_EQ(prior_calibration->_neg_smp_r, 0.5);
    }

    TEST_F(TestCalibrationDaemon, load_isotonic_regression) {
        std::string dname = "test/test-data";
        std::string fname = "1-calibrations/1-isotonic-regression.txt";
        std::string type = "isotonic-regression";
        int update_interval = 1;
        _daemon.reset(new algocomp::CalibrationDaemon(dname, fname, type, update_interval));

        auto calibration = _daemon->load();
        auto const isotonic_regression = dynamic_cast<algocomp::IsotonicRegression *>(calibration.get());
        EXPECT_NEAR(isotonic_regression->get(0.8), 0.15296713764868813, 1E-4);
        EXPECT_NEAR(isotonic_regression->get(0.9), 0.29661971830985917, 1E-4);
        EXPECT_NEAR(isotonic_regression->get(0.4), 0.02298094617097653, 1E-4);
    }

    TEST_F(TestCalibrationDaemon, load_hybrid) {
        std::string dname = "test/test-data";
        std::string fname = "1-calibrations/1-prior-correction.txt;1-calibrations/1-isotonic-regression.txt";
        std::string type = "hybrid";
        int update_interval = 1;
        _daemon.reset(new algocomp::CalibrationDaemon(dname, fname, type, update_interval));

        auto calibration = _daemon->load();
        auto const hybrid = dynamic_cast<algocomp::Hybrid *>(calibration.get());

        std::vector<float> scores;
        scores.push_back(0.8);
        scores.push_back(0.9);
        scores.push_back(0.4);

        std::vector<float> correct_calibrated_scores;
        std::for_each(scores.begin(), scores.end(), [this, &correct_calibrated_scores, &hybrid](const float &v) {
            float score = v / (v + (1 - v) * 2);
            correct_calibrated_scores.push_back(hybrid->get(score));
        });

        std::vector<float> calibrated_scores = calibration->calibrate(scores);
        for (int i = 0; i < scores.size(); ++i) {
            EXPECT_EQ(calibrated_scores[i], correct_calibrated_scores[i]);
        }
    }

    TEST_F(TestCalibrationDaemon, is_modified) {
        std::string dname = "test/test-data";
        std::string fname = "1-calibrations/1-isotonic-regression.txt";
        std::string type = "isotonic-regression";
        int update_interval = 1;
        _daemon.reset(new algocomp::CalibrationDaemon(dname, fname, type, update_interval));

        bool first_touch = _daemon->is_modified();
        EXPECT_FALSE(first_touch);
        boost::filesystem::remove("test/test-data/1-calibrations/1-isotonic-regression.txt.swap");
        boost::filesystem::copy("test/test-data/1-calibrations/1-isotonic-regression.txt",
                                "test/test-data/1-calibrations/1-isotonic-regression.txt.swap");
        boost::filesystem::remove("test/test-data/1-calibrations/1-isotonic-regression.txt");
        boost::filesystem::copy("test/test-data/1-calibrations/1-isotonic-regression.txt.swap",
                                "test/test-data/1-calibrations/1-isotonic-regression.txt");
        boost::filesystem::remove("test/test-data/1-calibrations/1-isotonic-regression.txt.swap");
        auto out_model = _daemon->load();
        bool second_touch = _daemon->is_modified();
        EXPECT_TRUE(second_touch);
    }

    TEST_F(TestCalibrationDaemon, boost_thread) {
        boost::function0<void> f = boost::bind(&TestCalibrationDaemon::thread_content, this);
        boost::thread thrd(f);
        _logger->debug("test_calibration_daemon: sleep 2 seconds starts...");
        boost::this_thread::sleep(boost::posix_time::seconds(2));
        _logger->debug("test_calibration_daemon: sleep 2 seconds ends...");
        thrd.interrupt();
        thrd.join();
        _logger->debug("test_calibration_daemon: terminate thread!");
    }

    TEST_F(TestCalibrationDaemon, calibration_success_file) {
        std::string dname = "test/test-data";
        std::string fname = "1-calibrations/1-isotonic-regression.txt";
        std::string type = "isotonic-regression";
        int update_interval = 1;
        _daemon.reset(new algocomp::CalibrationDaemon(dname, fname, type, update_interval));

        _daemon->is_modified();
        EXPECT_TRUE(_daemon->load());
        boost::filesystem::rename(_daemon->_succ_file_paths[0], _daemon->_succ_file_paths[0] + ".bak");
        EXPECT_FALSE(_daemon->is_modified());
        EXPECT_FALSE(_daemon->load());
        boost::filesystem::rename(_daemon->_succ_file_paths[0] + ".bak", _daemon->_succ_file_paths[0]);
        EXPECT_FALSE(_daemon->is_modified());
        EXPECT_TRUE(_daemon->load());
    }

}


/******************************************************************************
 * Copyright (C) 2015 by Jerome Maye                                          *
 * jerome.maye@gmail.com                                                      *
 ******************************************************************************/

#include "aslam/calibration/egomotion/design-variables/DesignVariables.h"

#include <sstream>
#include <string>

#include <boost/make_shared.hpp>

#include <sm/kinematics/EulerAnglesYawPitchRoll.hpp>
#include <sm/kinematics/quaternion_algebra.hpp>

#include <sm/PropertyTree.hpp>

#include <aslam/backend/EuclideanPoint.hpp>
#include <aslam/backend/RotationQuaternion.hpp>
#include <aslam/backend/GenericScalar.hpp>
#include <aslam/backend/FixedPointNumber.hpp>

#include <aslam/calibration/core/OptimizationProblem.h>
#include <aslam/calibration/exceptions/OutOfBoundException.h>

using namespace sm;
using namespace sm::kinematics;
using namespace aslam::backend;

namespace aslam {
  namespace calibration {

/******************************************************************************/
/* Constructors and Destructor                                                */
/******************************************************************************/

    DesignVariables::DesignVariables(const PropertyTree& config) {
      auto numSensors = config.getInt("num");
      calibrationVariables_.reserve(numSensors);
      for (int i = 0; i < numSensors; ++i) {
        std::stringstream tagStream;
        tagStream << "sensor_" << i << "/";
        const auto tag = tagStream.str();
        auto r = boost::make_shared<EuclideanPoint>(Eigen::Vector3d(
          config.getDouble(tag + "extrinsics/translation/x"),
          config.getDouble(tag + "extrinsics/translation/y"),
          config.getDouble(tag + "extrinsics/translation/z")));
        if (config.getBool(tag + "extrinsics/translation/active"))
          r->setActive(true);
        else
          r->setActive(false);
        const EulerAnglesYawPitchRoll ypr;
        auto R = boost::make_shared<RotationQuaternion>(
          ypr.parametersToRotationMatrix(
          Eigen::Vector3d(config.getDouble(tag + "extrinsics/rotation/yaw"),
          config.getDouble(tag + "extrinsics/rotation/pitch"),
          config.getDouble(tag + "extrinsics/rotation/roll"))));
        if (config.getBool(tag + "extrinsics/rotation/active"))
          R->setActive(true);
        else
          R->setActive(false);
        auto t = boost::make_shared<TimeDesignVariable>(
          config.getInt(tag + "intrinsics/timeDelay"));
        if (config.getBool(tag + "intrinsics/timeDelayActive"))
          t->setActive(true);
        else
          t->setActive(false);
        auto idx = config.getInt(tag + "idx");
        calibrationVariables_[idx] = std::make_tuple(t, r, R);
      }
    }

/******************************************************************************/
/* Methods                                                                    */
/******************************************************************************/

    void DesignVariables::addToBatch(const BatchSP& batch, size_t groupId) {
      for (const auto& designVariable : calibrationVariables_) {
        batch->addDesignVariable(std::get<0>(designVariable.second), groupId);
        batch->addDesignVariable(std::get<1>(designVariable.second), groupId);
        batch->addDesignVariable(std::get<2>(designVariable.second), groupId);
      }
    }

    Eigen::VectorXd DesignVariables::getParameters(size_t idx) const {
      if (!calibrationVariables_.count(idx))
        throw OutOfBoundException<size_t>(idx,
          "DesignVariables::getParameters(): inexistent design variable");
      const auto& designVariable = calibrationVariables_.at(idx);
      Eigen::VectorXd params(7);
      params(0) = std::get<0>(designVariable)->getParameters()(0);
      params.segment<3>(1) = std::get<1>(designVariable)->toEuclidean();
      const EulerAnglesYawPitchRoll ypr;
      params.segment<3>(4) = ypr.rotationMatrixToParameters(
        quat2r(std::get<2>(designVariable)->getQuaternion()));
      return params;
    }

/******************************************************************************/
/* Stream methods                                                             */
/******************************************************************************/

    void DesignVariables::read(std::istream& /*stream*/) {
    }

    void DesignVariables::write(std::ostream& stream) const {
      for (const auto& designVariable : calibrationVariables_) {
        stream << "t_" << designVariable.first << ": "
          << std::get<0>(designVariable.second)->getParameters() << std::endl;
        stream << "r_" << designVariable.first << ": "
          << std::get<1>(designVariable.second)->toEuclidean().transpose()
          << std::endl;
        const EulerAnglesYawPitchRoll ypr;
        stream << "R_" << designVariable.first << ": "
          << ypr.rotationMatrixToParameters(quat2r(std::get<2>(
          designVariable.second)->getQuaternion())).transpose() << std::endl;
      }
    }

    void DesignVariables::read(std::ifstream& /*stream*/) {
    }

    void DesignVariables::write(std::ofstream& /*stream*/) const {
    }

  }
}


#ifndef INCLUDED_STDDEFX
#include "stddefx.h"
#define INCLUDED_STDDEFX
#endif

#ifndef INCLUDED_CALC_ACCUIMPL
#include "calc_accuimpl.h"
#define INCLUDED_CALC_ACCUIMPL
#endif

// Library headers.
#ifndef INCLUDED_CMATH
#include <cmath>
#define INCLUDED_CMATH
#endif
#ifndef INCLUDED_BOOST_NONCOPYABLE
#include <boost/noncopyable.hpp>
#define INCLUDED_BOOST_NONCOPYABLE
#endif
// PCRaster library headers.
#ifndef INCLUDED_COM_CSFCELL
#include "com_csfcell.h"
#define INCLUDED_COM_CSFCELL
#endif
#ifndef INCLUDED_COM_MVOP
#include "com_mvop.h"
#define INCLUDED_COM_MVOP
#endif
// Module headers.
#ifndef INCLUDED_CALC_EXECARGUMENTS
#include "calc_execarguments.h"
#define INCLUDED_CALC_EXECARGUMENTS
#endif
#ifndef INCLUDED_CALC_DOMAINERROR
#include "calc_domainerror.h"
#define INCLUDED_CALC_DOMAINERROR
#endif
#ifndef INCLUDED_CALC_DOWNSTREAMVISITOR
#include "calc_downstreamvisitor.h"
#define INCLUDED_CALC_DOWNSTREAMVISITOR
#endif
#ifndef INCLUDED_CALC_VFIELD
#include "calc_vfield.h"
#define INCLUDED_CALC_VFIELD
#endif
#ifndef INCLUDED_CALC_SCOPEDLDDGRAPH
#include "calc_scopedlddgraph.h"
#define INCLUDED_CALC_SCOPEDLDDGRAPH
#endif
#ifndef INCLUDED_CALC_LDDGRAPH
#include "calc_lddgraph.h"
#define INCLUDED_CALC_LDDGRAPH
#endif
#ifndef INCLUDED_CALC_FIELD
#include "calc_field.h"
#define INCLUDED_CALC_FIELD
#endif

/*!
  \file
  This file contains the implementation of the AccuImpl class.
*/

//------------------------------------------------------------------------------

namespace calc {

//! function object for AccuImpl
struct AccumulateFlux {
  // virtual ~AccumulateFlux() {};
  virtual float operator()(float   interimState,
                           size_t  pos) const=0;
  virtual bool mv(size_t /* pos */) const
  { return false; }
};

//! function object for AccuImpl with 1 parameter
struct AccumulateFluxPar:
  public AccumulateFlux
  // , public boost::noncopyable, if enable results in pure virtual function
  // called on linux/gcc
{
  const VField<float>& d_par;
  AccumulateFluxPar(const VField<float>& par):
    d_par(par)
    {}
  virtual bool mv(size_t pos) const {
    return pcr::isMV(d_par[pos]);
  }
};

AccuAll accuAll;

class Capacity : public AccumulateFluxPar {
public:
  Capacity(const VField<float>& capacity):
    AccumulateFluxPar(capacity)
    {}
  float operator()(float interimState, size_t pos) const {
    return std::min(interimState,d_par[pos]);
  }
};
AccuCapacity accuCapacity;

class Threshold : public AccumulateFluxPar {
public:
  Threshold(const VField<float>& threshold):
    AccumulateFluxPar(threshold)
    {}
  float operator()(float interimState, size_t pos) const {
    float threshold=d_par[pos];
    if (threshold < 0)
      throw DomainError("threshold < 0");
    if(interimState <= threshold)
       return 0;
    return interimState - threshold;
  }
};
AccuThreshold accuThreshold;

class Fraction : public AccumulateFluxPar {
public:
  Fraction(const VField<float>& fraction):
    AccumulateFluxPar(fraction)
    {}
  float operator()(float interimState, size_t pos) const {
    float fraction=d_par[pos];
    if (0 > fraction || fraction > 1)
      throw DomainError("fraction not in [0,1] range");
    return interimState * fraction;
  }
};
AccuFraction accuFraction;

class Trigger : public AccumulateFluxPar {
public:
  Trigger(const VField<float>& trigger):
    AccumulateFluxPar(trigger)
    {}
  float operator()(float interimState, size_t pos) const {
    float trigger=d_par[pos];
    if (trigger < 0)
      throw DomainError("trigger < 0");
    if (trigger <= interimState)
      return interimState;
    return 0;
  }
};
AccuTrigger accuTrigger;

//! Accumulation through a directed graph, like LddGraph
class AccuImpl : public DownstreamVisitor
{
private:
   AccuImpl&          operator=           (AccuImpl const& rhs);
                   AccuImpl               (AccuImpl const& rhs);

   //! the result state
   float*                 d_newState;
   //! the result flux
   float*                 d_flux;

   const  VField<float>&  d_oldState;
   const  AccumulateFlux& d_fluxFo;

   void visitEdge        (size_t up, size_t down);
   void finishVertex     (size_t v);

public:

                   AccuImpl               (float* newState, float* flux,
                                           const  LddGraph& lg,
                                           const  VField<float>&  oldState,
                                           const  AccumulateFlux& fluxFo);

  /* virtual */    ~AccuImpl              ();

};

template<class AccuStateFo>
static void accuStateFlux(
    RunTimeEnv*            rte,
    const Operator&        op)
  {
    ExecArguments arg(op,rte,3);
    arg.createResults();
    Field& newState(arg.result(0));
    Field& flux    (arg.result(1));

    size_t size(flux.nrValues());

    ScopedLddGraph      lg(rte,arg[0]);
    // TODO void calc::ExecutorTest::testRunTimeErrors()
    //  swap value of arg[1] and arg[2] error description is not
    //  correct it contains _mrf
    const VField<REAL4> oldState(arg[1],size);
    const VField<REAL4>      par(arg[2],size);

    AccuImpl ag(newState.dest_f(), flux.dest_f(),
                       lg.current(),oldState,AccuStateFo(par));
    ag.visitEntireLdd();

    arg.pushResults();
  }

} // namespace calc

//------------------------------------------------------------------------------
// DEFINITION OF STATIC ACCUIMPL MEMBERS
//------------------------------------------------------------------------------

//------------------------------------------------------------------------------
// DEFINITION OF ACCUIMPL MEMBERS
//------------------------------------------------------------------------------

calc::AccuImpl::AccuImpl(
    float* newState, float* flux,
    const  LddGraph& lg,
    const  VField<float>&  oldState,
    const  AccumulateFlux& fluxFo):
     DownstreamVisitor(lg),
     d_newState(newState),
     d_flux(flux),
     d_oldState(oldState),
     d_fluxFo(fluxFo)
{
    graph().initField<float>(d_flux,0.0);
}

calc::AccuImpl::~AccuImpl()
{
}

/*
 * void calc::AccuImpl::discoverVertex(size_t v)
 * {
 * d_flux[v]=0;
 * }
 */

void calc::AccuImpl::visitEdge(size_t up, size_t down)
{
  // send calculated flux to down
  com::inplace_add(d_flux[down],d_flux[up]);
}

void calc::AccuImpl::finishVertex(size_t v)
{
  if (com::oneIsMV(d_oldState[v],d_flux[v])|d_fluxFo.mv(v)) {
    pcr::setMV(d_flux[v]);
    pcr::setMV(d_newState[v]);
  } else {
    // up has al its in-fluxes now summed in flux
    float interimState = d_oldState[v] + d_flux[v];

    float newFlux = d_fluxFo(interimState,v);
        d_flux[v] = newFlux;
    d_newState[v] = interimState-newFlux;
  }
}

//------------------------------------------------------------------------------
// DEFINITION OF FREE OPERATORS
//------------------------------------------------------------------------------

//------------------------------------------------------------------------------
// DEFINITION OF FREE FUNCTIONS
//------------------------------------------------------------------------------

/* BE WARE \a op may NOT the op of the mrf, but they all
 * share the same args see OneOfMRF::exec
 */

#define ACCU_CLASS(Name)                                     \
void calc::Accu##Name::exec(                                 \
    RunTimeEnv* rte, const Operator& op,size_t nrArgs) const \
{ PRECOND(nrArgs==3); accuStateFlux<Name>(rte,op); }

ACCU_CLASS(Capacity)
ACCU_CLASS(Threshold)
ACCU_CLASS(Fraction)
ACCU_CLASS(Trigger)

void calc::AccuAll::exec(
    RunTimeEnv*     rte,
    const Operator& op, size_t) const
  {
     struct All : public AccumulateFlux {
       All() {}
     float operator()(float interimState, size_t /* pos */) const
      { return interimState; }
      ~All() {}
     };

    ExecArguments arg(op,rte,2);
    arg.createResult();
    Field& newState(arg.result(0));
    Field& flux    (arg.result(1));

    size_t size(flux.nrValues());

    ScopedLddGraph lg(rte,arg[0]);
    const VField<REAL4> oldState(arg[1],size);

    AccuImpl ag(newState.dest_f(), flux.dest_f(),lg.current(),oldState,All());
    ag.visitEntireLdd();

    arg.pushResults();
  }


#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE crypto_AES

#include <algorithm>
#include <boost/predef.h>
#include <boost/test/unit_test.hpp>
#include <openssl/aes.h>
#include <openssl/err.h>
#include <openssl/evp.h>
#include "crypto/crypto_AES.hpp"

BOOST_AUTO_TEST_CASE(test_crypto_aes_1)
{
    OpenSSL_add_all_algorithms();
    ERR_load_crypto_strings();

    unsigned char key[32];
    BOOST_REQUIRE(my::crypto::Rand_Key_AES(key) == 0);

    unsigned char key2[32];
    BOOST_REQUIRE(my::crypto::Rand_Key_AES(key2) == 0);
    BOOST_REQUIRE(strcmp((char *)key, (char *)key2) != 0);

    unsigned char iv[16];
    BOOST_REQUIRE(my::crypto::Rand_IV_AES(iv) == 0);

    unsigned char iv2[16];
    BOOST_REQUIRE(my::crypto::Rand_IV_AES(iv2) == 0);
    BOOST_REQUIRE(strcmp((char *)iv, (char *)iv2) != 0);

    unsigned char plaintext[] = "The quick brown fox jumps over the lazy dog";
    unsigned char aad[] = "Some AAD data";

    unsigned char ciphertext[128];
    unsigned char decryptedtext[128];

    unsigned char tag[16];

    int decryptedtext_len = 0, ciphertext_len = 0;
    ciphertext_len
        = my::crypto::Encrypt_AES(plaintext, strlen(reinterpret_cast<char *>(plaintext)), aad, strlen(reinterpret_cast<char *>(aad)), key, iv, ciphertext, tag);

    decryptedtext_len = my::crypto::Decrypt_AES(ciphertext, ciphertext_len, aad, strlen(reinterpret_cast<char *>(aad)), tag, key, iv, decryptedtext);

    BOOST_REQUIRE(decryptedtext_len >= 0);
    decryptedtext[decryptedtext_len] = '\0';

    BOOST_REQUIRE(strlen(reinterpret_cast<char *>(decryptedtext)) == strlen(reinterpret_cast<char *>(plaintext)));
    BOOST_REQUIRE(strcmp((char *)decryptedtext, (char *)plaintext) == 0);

    ERR_free_strings();
}

BOOST_AUTO_TEST_CASE(test_crypto_aes_2)
{
    OpenSSL_add_all_algorithms();
    ERR_load_crypto_strings();

    unsigned char key[32];
    BOOST_REQUIRE(my::crypto::Rand_Key_AES(key) == 0);

    unsigned char key2[32];
    BOOST_REQUIRE(my::crypto::Rand_Key_AES(key2) == 0);
    BOOST_REQUIRE(strcmp((char *)key, (char *)key2) != 0);

    unsigned char iv[16];
    BOOST_REQUIRE(my::crypto::Rand_IV_AES(iv) == 0);

    unsigned char iv2[16];
    BOOST_REQUIRE(my::crypto::Rand_IV_AES(iv2) == 0);
    BOOST_REQUIRE(strcmp((char *)iv, (char *)iv2) != 0);

    unsigned char plaintext[] = "Bonjour, je suis là depuis des années, transformée en chatton";
    unsigned char aad[] = "";

    unsigned char ciphertext[128];
    unsigned char decryptedtext[128];

    unsigned char tag[16];

    int decryptedtext_len = 0, ciphertext_len = 0;
    ciphertext_len
        = my::crypto::Encrypt_AES(plaintext, strlen(reinterpret_cast<char *>(plaintext)), aad, strlen(reinterpret_cast<char *>(aad)), key, iv, ciphertext, tag);

    decryptedtext_len = my::crypto::Decrypt_AES(ciphertext, ciphertext_len, aad, strlen(reinterpret_cast<char *>(aad)), tag, key, iv, decryptedtext);

    BOOST_REQUIRE(decryptedtext_len >= 0);
    decryptedtext[decryptedtext_len] = '\0';

    BOOST_REQUIRE(strlen(reinterpret_cast<char *>(decryptedtext)) == strlen(reinterpret_cast<char *>(plaintext)));
    BOOST_REQUIRE(strcmp((char *)decryptedtext, (char *)plaintext) == 0);

    ERR_free_strings();
}

BOOST_AUTO_TEST_CASE(test_crypto_aes_3)
{
    OpenSSL_add_all_algorithms();
    ERR_load_crypto_strings();

    unsigned char key[32];
    BOOST_REQUIRE(my::crypto::Rand_Key_AES(key) == 0);

    unsigned char key2[32];
    BOOST_REQUIRE(my::crypto::Rand_Key_AES(key2) == 0);
    BOOST_REQUIRE(strcmp((char *)key, (char *)key2) != 0);

    unsigned char iv[16];
    BOOST_REQUIRE(my::crypto::Rand_IV_AES(iv) == 0);

    unsigned char iv2[16];
    BOOST_REQUIRE(my::crypto::Rand_IV_AES(iv2) == 0);
    BOOST_REQUIRE(strcmp((char *)iv, (char *)iv2) != 0);

    unsigned char plaintext[] = "Les chaussettes de l'archiduchesse sont-elles sèches?  Archi-sèches ?";
    unsigned char aad[] = "";

    unsigned char ciphertext[128];
    unsigned char decryptedtext[128];

    unsigned char tag[16];

    int decryptedtext_len = 0, ciphertext_len = 0;
    ciphertext_len
        = my::crypto::Encrypt_AES(plaintext, strlen(reinterpret_cast<char *>(plaintext)), aad, strlen(reinterpret_cast<char *>(aad)), key, iv, ciphertext, tag);

    decryptedtext_len = my::crypto::Decrypt_AES(ciphertext, ciphertext_len, aad, strlen(reinterpret_cast<char *>(aad)), tag, key, iv, decryptedtext);

    BOOST_REQUIRE(decryptedtext_len >= 0);
    decryptedtext[decryptedtext_len] = '\0';

    BOOST_REQUIRE(strlen(reinterpret_cast<char *>(decryptedtext)) == strlen(reinterpret_cast<char *>(plaintext)));
    BOOST_REQUIRE(strcmp((char *)decryptedtext, (char *)plaintext) == 0);

    ERR_free_strings();
}

BOOST_AUTO_TEST_CASE(test_crypto_aes_4)
{
    OpenSSL_add_all_algorithms();
    ERR_load_crypto_strings();

    unsigned char key[32];
    BOOST_REQUIRE(my::crypto::Rand_Key_AES(key) == 0);

    unsigned char key2[32];
    BOOST_REQUIRE(my::crypto::Rand_Key_AES(key2) == 0);
    BOOST_REQUIRE(strcmp((char *)key, (char *)key2) != 0);

    unsigned char iv[16];
    BOOST_REQUIRE(my::crypto::Rand_IV_AES(iv) == 0);

    unsigned char iv2[16];
    BOOST_REQUIRE(my::crypto::Rand_IV_AES(iv2) == 0);
    BOOST_REQUIRE(strcmp((char *)iv, (char *)iv2) != 0);

    unsigned char plaintext[] = "Un chasseur sachant chasser doit savoir chasser sans son chien.";
    unsigned char aad[] = "Cinq chiens chassent six chats.";

    unsigned char ciphertext[128];
    unsigned char decryptedtext[128];

    unsigned char tag[16];

    int decryptedtext_len = 0, ciphertext_len = 0;
    ciphertext_len
        = my::crypto::Encrypt_AES(plaintext, strlen(reinterpret_cast<char *>(plaintext)), aad, strlen(reinterpret_cast<char *>(aad)), key, iv, ciphertext, tag);

    decryptedtext_len = my::crypto::Decrypt_AES(ciphertext, ciphertext_len, aad, strlen(reinterpret_cast<char *>(aad)), tag, key, iv, decryptedtext);

    BOOST_REQUIRE(decryptedtext_len >= 0);
    decryptedtext[decryptedtext_len] = '\0';

    BOOST_REQUIRE(strlen(reinterpret_cast<char *>(decryptedtext)) == strlen(reinterpret_cast<char *>(plaintext)));
    BOOST_REQUIRE(strcmp((char *)decryptedtext, (char *)plaintext) == 0);

    ERR_free_strings();
}

BOOST_AUTO_TEST_CASE(test_crypto_aes_5)
{
    OpenSSL_add_all_algorithms();
    ERR_load_crypto_strings();

    unsigned char key[32];
    BOOST_REQUIRE(my::crypto::Rand_Key_AES(key) == 0);

    unsigned char key2[32];
    BOOST_REQUIRE(my::crypto::Rand_Key_AES(key2) == 0);
    BOOST_REQUIRE(strcmp((char *)key, (char *)key2) != 0);

    unsigned char iv[16];
    BOOST_REQUIRE(my::crypto::Rand_IV_AES(iv) == 0);

    unsigned char iv2[16];
    BOOST_REQUIRE(my::crypto::Rand_IV_AES(iv2) == 0);
    BOOST_REQUIRE(strcmp((char *)iv, (char *)iv2) != 0);

    unsigned char plaintext[] = "lol";
    unsigned char aad[] = "Chattons";

    unsigned char ciphertext[128];
    unsigned char decryptedtext[128];

    unsigned char tag[16];

    int decryptedtext_len = 0, ciphertext_len = 0;
    ciphertext_len
        = my::crypto::Encrypt_AES(plaintext, strlen(reinterpret_cast<char *>(plaintext)), aad, strlen(reinterpret_cast<char *>(aad)), key, iv, ciphertext, tag);

    decryptedtext_len = my::crypto::Decrypt_AES(ciphertext, ciphertext_len, aad, strlen(reinterpret_cast<char *>(aad)), tag, key, iv, decryptedtext);

    BOOST_REQUIRE(decryptedtext_len >= 0);
    decryptedtext[decryptedtext_len] = '\0';

    BOOST_REQUIRE(strlen(reinterpret_cast<char *>(decryptedtext)) == strlen(reinterpret_cast<char *>(plaintext)));
    BOOST_REQUIRE(strcmp((char *)decryptedtext, (char *)plaintext) == 0);

    ERR_free_strings();
}

BOOST_AUTO_TEST_CASE(test_crypto_aes_6)
{
    OpenSSL_add_all_algorithms();
    ERR_load_crypto_strings();

    unsigned char key[32];
    BOOST_REQUIRE(my::crypto::Rand_Key_AES(key) == 0);

    unsigned char key2[32];
    BOOST_REQUIRE(my::crypto::Rand_Key_AES(key2) == 0);
    BOOST_REQUIRE(strcmp((char *)key, (char *)key2) != 0);

    unsigned char iv[16];
    BOOST_REQUIRE(my::crypto::Rand_IV_AES(iv) == 0);

    unsigned char iv2[16];
    BOOST_REQUIRE(my::crypto::Rand_IV_AES(iv2) == 0);
    BOOST_REQUIRE(strcmp((char *)iv, (char *)iv2) != 0);

    unsigned char plaintext[] = "lol";
    unsigned char aad[] = "";

    unsigned char ciphertext[128];
    unsigned char decryptedtext[128];

    unsigned char tag[16];

    int decryptedtext_len = 0, ciphertext_len = 0;
    ciphertext_len
        = my::crypto::Encrypt_AES(plaintext, strlen(reinterpret_cast<char *>(plaintext)), aad, strlen(reinterpret_cast<char *>(aad)), key, iv, ciphertext, tag);

    decryptedtext_len = my::crypto::Decrypt_AES(ciphertext, ciphertext_len, aad, strlen(reinterpret_cast<char *>(aad)), tag, key, iv, decryptedtext);

    BOOST_REQUIRE(decryptedtext_len >= 0);
    decryptedtext[decryptedtext_len] = '\0';

    BOOST_REQUIRE(strlen(reinterpret_cast<char *>(decryptedtext)) == strlen(reinterpret_cast<char *>(plaintext)));
    BOOST_REQUIRE(strcmp((char *)decryptedtext, (char *)plaintext) == 0);

    ERR_free_strings();
}

BOOST_AUTO_TEST_CASE(test_crypto_aes_7)
{
    OpenSSL_add_all_algorithms();
    ERR_load_crypto_strings();

    unsigned char key[32];
    BOOST_REQUIRE(my::crypto::Rand_Key_AES(key) == 0);

    unsigned char key2[32];
    BOOST_REQUIRE(my::crypto::Rand_Key_AES(key2) == 0);
    BOOST_REQUIRE(strcmp((char *)key, (char *)key2) != 0);

    unsigned char iv[16];
    BOOST_REQUIRE(my::crypto::Rand_IV_AES(iv) == 0);

    unsigned char iv2[16];
    BOOST_REQUIRE(my::crypto::Rand_IV_AES(iv2) == 0);
    BOOST_REQUIRE(strcmp((char *)iv, (char *)iv2) != 0);

    unsigned char plaintext[] = "lol";
    unsigned char aad[] = "";

    unsigned char ciphertext[128];
    unsigned char decryptedtext[128];

    unsigned char tag[16];

    int decryptedtext_len = 0, ciphertext_len = 0;
    ciphertext_len
        = my::crypto::Encrypt_AES(plaintext, strlen(reinterpret_cast<char *>(plaintext)), aad, strlen(reinterpret_cast<char *>(aad)), key, iv, ciphertext, tag);

    decryptedtext_len = my::crypto::Decrypt_AES(ciphertext, ciphertext_len, aad, strlen(reinterpret_cast<char *>(aad)), tag, key, iv, decryptedtext);

    BOOST_REQUIRE(decryptedtext_len >= 0);
    decryptedtext[decryptedtext_len] = '\0';

    BOOST_REQUIRE(strlen(reinterpret_cast<char *>(decryptedtext)) == strlen(reinterpret_cast<char *>(plaintext)));
    BOOST_REQUIRE(strcmp((char *)decryptedtext, (char *)plaintext) == 0);

    ERR_free_strings();
}


#ifndef OSRM_ENGINE_ROUTING_BASE_MLD_HPP
#define OSRM_ENGINE_ROUTING_BASE_MLD_HPP

#include "engine/algorithm.hpp"
#include "engine/datafacade/contiguous_internalmem_datafacade.hpp"
#include "engine/routing_algorithms/routing_base.hpp"
#include "engine/search_engine_data.hpp"

#include "util/typedefs.hpp"

#include <boost/assert.hpp>

namespace osrm
{
namespace engine
{
namespace routing_algorithms
{
namespace mld
{

namespace
{
// Unrestricted search (Args is const PhantomNodes &):
//   * use partition.GetQueryLevel to find the node query level based on source and target phantoms
//   * allow to traverse all cells
LevelID getNodeQureyLevel(const partition::MultiLevelPartitionView &partition,
                          NodeID node,
                          const PhantomNodes &phantom_nodes)
{
    auto level = [&partition, node](const SegmentID &source, const SegmentID &target) {
        if (source.enabled && target.enabled)
            return partition.GetQueryLevel(source.id, target.id, node);
        return INVALID_LEVEL_ID;
    };
    return std::min(std::min(level(phantom_nodes.source_phantom.forward_segment_id,
                                   phantom_nodes.target_phantom.forward_segment_id),
                             level(phantom_nodes.source_phantom.forward_segment_id,
                                   phantom_nodes.target_phantom.reverse_segment_id)),
                    std::min(level(phantom_nodes.source_phantom.reverse_segment_id,
                                   phantom_nodes.target_phantom.forward_segment_id),
                             level(phantom_nodes.source_phantom.reverse_segment_id,
                                   phantom_nodes.target_phantom.reverse_segment_id)));
}

bool checkParentCellRestriction(CellID, const PhantomNodes &) { return true; }

// Restricted search (Args is LevelID, CellID):
//   * use the fixed level for queries
//   * check if the node cell is the same as the specified parent onr
LevelID getNodeQureyLevel(const partition::MultiLevelPartitionView &, NodeID, LevelID level, CellID)
{
    return level;
}

bool checkParentCellRestriction(CellID cell, LevelID, CellID parent) { return cell == parent; }
}

template <bool DIRECTION, typename... Args>
void routingStep(const datafacade::ContiguousInternalMemoryDataFacade<algorithm::MLD> &facade,
                 SearchEngineData::MultiLayerDijkstraHeap &forward_heap,
                 SearchEngineData::MultiLayerDijkstraHeap &reverse_heap,
                 NodeID &middle_node,
                 EdgeWeight &path_upper_bound,
                 Args... args)
{
    const auto &partition = facade.GetMultiLevelPartition();
    const auto &cells = facade.GetCellStorage();

    const auto node = forward_heap.DeleteMin();
    const auto weight = forward_heap.GetKey(node);

    // Upper bound for the path source -> target with
    // weight(source -> node) = weight weight(to -> target) ≤ reverse_weight
    // is weight + reverse_weight
    // More tighter upper bound requires additional condition reverse_heap.WasRemoved(to)
    // with weight(to -> target) = reverse_weight and all weights ≥ 0
    if (reverse_heap.WasInserted(node))
    {
        auto reverse_weight = reverse_heap.GetKey(node);
        auto path_weight = weight + reverse_weight;
        if (path_weight >= 0 && path_weight < path_upper_bound)
        {
            middle_node = node;
            path_upper_bound = path_weight;
        }
    }

    const auto level = getNodeQureyLevel(partition, node, args...);

    if (level >= 1 && !forward_heap.GetData(node).from_clique_arc)
    {
        if (DIRECTION == FORWARD_DIRECTION)
        {
            // Shortcuts in forward direction
            const auto &cell = cells.GetCell(level, partition.GetCell(level, node));
            auto destination = cell.GetDestinationNodes().begin();
            for (auto shortcut_weight : cell.GetOutWeight(node))
            {
                BOOST_ASSERT(destination != cell.GetDestinationNodes().end());
                const NodeID to = *destination;
                if (shortcut_weight != INVALID_EDGE_WEIGHT && node != to)
                {
                    const EdgeWeight to_weight = weight + shortcut_weight;
                    if (!forward_heap.WasInserted(to))
                    {
                        forward_heap.Insert(to, to_weight, {node, true});
                    }
                    else if (to_weight < forward_heap.GetKey(to))
                    {
                        forward_heap.GetData(to) = {node, true};
                        forward_heap.DecreaseKey(to, to_weight);
                    }
                }
                ++destination;
            }
        }
        else
        {
            // Shortcuts in backward direction
            const auto &cell = cells.GetCell(level, partition.GetCell(level, node));
            auto source = cell.GetSourceNodes().begin();
            for (auto shortcut_weight : cell.GetInWeight(node))
            {
                BOOST_ASSERT(source != cell.GetSourceNodes().end());
                const NodeID to = *source;
                if (shortcut_weight != INVALID_EDGE_WEIGHT && node != to)
                {
                    const EdgeWeight to_weight = weight + shortcut_weight;
                    if (!forward_heap.WasInserted(to))
                    {
                        forward_heap.Insert(to, to_weight, {node, true});
                    }
                    else if (to_weight < forward_heap.GetKey(to))
                    {
                        forward_heap.GetData(to) = {node, true};
                        forward_heap.DecreaseKey(to, to_weight);
                    }
                }
                ++source;
            }
        }
    }

    // Boundary edges
    for (const auto edge : facade.GetBorderEdgeRange(level, node))
    {
        const auto &edge_data = facade.GetEdgeData(edge);
        if (DIRECTION == FORWARD_DIRECTION ? edge_data.forward : edge_data.backward)
        {
            const NodeID to = facade.GetTarget(edge);

            if (checkParentCellRestriction(partition.GetCell(level + 1, to), args...))
            {
                BOOST_ASSERT_MSG(edge_data.weight > 0, "edge_weight invalid");
                const EdgeWeight to_weight = weight + edge_data.weight;

                if (!forward_heap.WasInserted(to))
                {
                    forward_heap.Insert(to, to_weight, {node, false});
                }
                else if (to_weight < forward_heap.GetKey(to))
                {
                    forward_heap.GetData(to) = {node, false};
                    forward_heap.DecreaseKey(to, to_weight);
                }
            }
        }
    }
}

template <typename... Args>
std::tuple<EdgeWeight, NodeID, NodeID, std::vector<EdgeID>>
search(const datafacade::ContiguousInternalMemoryDataFacade<algorithm::MLD> &facade,
       SearchEngineData::MultiLayerDijkstraHeap &forward_heap,
       SearchEngineData::MultiLayerDijkstraHeap &reverse_heap,
       Args... args)
{

    const auto &partition = facade.GetMultiLevelPartition();

    BOOST_ASSERT(!forward_heap.Empty() && forward_heap.MinKey() < INVALID_EDGE_WEIGHT);
    BOOST_ASSERT(!reverse_heap.Empty() && reverse_heap.MinKey() < INVALID_EDGE_WEIGHT);

    // run two-Target Dijkstra routing step.
    NodeID middle = SPECIAL_NODEID;
    EdgeWeight weight = INVALID_EDGE_WEIGHT;
    EdgeWeight forward_heap_min = forward_heap.MinKey();
    EdgeWeight reverse_heap_min = reverse_heap.MinKey();
    while (forward_heap.Size() + reverse_heap.Size() > 0 &&
           forward_heap_min + reverse_heap_min < weight)
    {
        if (!forward_heap.Empty())
        {
            routingStep<FORWARD_DIRECTION>(
                facade, forward_heap, reverse_heap, middle, weight, args...);
            if (!forward_heap.Empty())
                forward_heap_min = forward_heap.MinKey();
        }
        if (!reverse_heap.Empty())
        {
            routingStep<REVERSE_DIRECTION>(
                facade, reverse_heap, forward_heap, middle, weight, args...);
            if (!reverse_heap.Empty())
                reverse_heap_min = reverse_heap.MinKey();
        }
    };

    // No path found for both target nodes?
    if (weight == INVALID_EDGE_WEIGHT || SPECIAL_NODEID == middle)
    {
        return std::make_tuple(
            INVALID_EDGE_WEIGHT, SPECIAL_NODEID, SPECIAL_NODEID, std::vector<EdgeID>());
    }

    // Get packed path as edges {from node ID, to node ID, edge ID}
    std::vector<std::tuple<NodeID, NodeID, bool>> packed_path;
    NodeID current_node = middle, parent_node = forward_heap.GetData(middle).parent;
    while (parent_node != current_node)
    {
        const auto &data = forward_heap.GetData(current_node);
        packed_path.push_back(std::make_tuple(parent_node, current_node, data.from_clique_arc));
        current_node = parent_node;
        parent_node = forward_heap.GetData(parent_node).parent;
    }
    std::reverse(std::begin(packed_path), std::end(packed_path));
    const NodeID source_node = current_node;

    current_node = middle, parent_node = reverse_heap.GetData(middle).parent;
    while (parent_node != current_node)
    {
        const auto &data = reverse_heap.GetData(current_node);
        packed_path.push_back(std::make_tuple(current_node, parent_node, data.from_clique_arc));
        current_node = parent_node;
        parent_node = reverse_heap.GetData(parent_node).parent;
    }
    const NodeID target_node = current_node;

    // Unpack path
    std::vector<EdgeID> unpacked_path;
    unpacked_path.reserve(packed_path.size());
    for (auto const &packed_edge : packed_path)
    {
        NodeID source, target;
        bool overlay_edge;
        std::tie(source, target, overlay_edge) = packed_edge;
        if (!overlay_edge)
        { // a base graph edge
            unpacked_path.push_back(facade.FindEdge(source, target));
        }
        else
        { // an overlay graph edge
            LevelID level = getNodeQureyLevel(partition, source, args...);
            CellID parent_cell_id = partition.GetCell(level, source);
            BOOST_ASSERT(parent_cell_id == partition.GetCell(level, target));

            LevelID sublevel = level - 1;

            // Here heaps can be reused, let's go deeper!
            forward_heap.Clear();
            reverse_heap.Clear();
            forward_heap.Insert(source, 0, {source});
            reverse_heap.Insert(target, 0, {target});

            // TODO: when structured bindings will be allowed change to
            // auto [subpath_weight, subpath_source, subpath_target, subpath] = ...
            EdgeWeight subpath_weight;
            NodeID subpath_source, subpath_target;
            std::vector<EdgeID> subpath;
            std::tie(subpath_weight, subpath_source, subpath_target, subpath) =
                search(facade, forward_heap, reverse_heap, sublevel, parent_cell_id);
            BOOST_ASSERT(!subpath.empty());
            BOOST_ASSERT(subpath_source == source);
            BOOST_ASSERT(subpath_target == target);
            unpacked_path.insert(unpacked_path.end(), subpath.begin(), subpath.end());
        }
    }

    return std::make_tuple(weight, source_node, target_node, std::move(unpacked_path));
}

} // namespace mld
} // namespace routing_algorithms
} // namespace engine
} // namespace osrm

#endif // OSRM_ENGINE_ROUTING_BASE_MLD_HPP