#include "interpolate.h"
#include "geoutil.h"
#include "lambert_conformal_grid.h"
#include "latitude_longitude_grid.h"
#include "numerical_functions.h"
#include "point_list.h"
#include "reduced_gaussian_grid.h"
#include "stereographic_grid.h"
#include "util.h"

#include "plugin_factory.h"
#include <Eigen/Dense>

#define HIMAN_AUXILIARY_INCLUDE

#include "querydata.h"

#undef HIMAN_AUXILIARY_INCLUDE

using namespace himan;
using namespace Eigen;

namespace himan
{
namespace interpolate
{
// Function to return identifier for each supported datatype
// In the interpolation weight cache we store weights separately
// for each data type.

template <typename T>
int DataTypeId();

template <>
int DataTypeId<float>()
{
	return 1;
}

template <>
int DataTypeId<double>()
{
	return 2;
}

template <>
int DataTypeId<short>()
{
	return 3;
}
template <>
int DataTypeId<unsigned char>()
{
	return 4;
}

bool IsSupportedGridForRotation(HPGridType type)
{
	switch (type)
	{
		case kRotatedLatitudeLongitude:
		case kStereographic:
		case kLambertConformalConic:
			return true;
		default:
			return false;
	}
}

template <typename T>
bool InterpolateArea(const grid* baseGrid, std::shared_ptr<info<T>> source)
{
	if (!source)
	{
		return false;
	}

#ifdef HAVE_CUDA
	if (source->PackedData()->HasData())
	{
		util::Unpack<T>({source});
	}
#endif

	base<T> target;
	target.grid = std::shared_ptr<himan::grid>(baseGrid->Clone());

	if (baseGrid->Class() == kRegularGrid)
	{
		target.data.Resize(dynamic_cast<const regular_grid*>(baseGrid)->Ni(),
		                   dynamic_cast<const regular_grid*>(baseGrid)->Nj());
	}
	else if (baseGrid->Class() == kIrregularGrid)
	{
		target.data.Resize(baseGrid->Size(), 1);
	}

	auto method = InterpolationMethod(source->Param().Name(), source->Param().InterpolationMethod());

	logger logr("interpolation");
	logr.Trace(fmt::format("Grid interpolation with method '{}'", HPInterpolationMethodToString.at(method)));

	if (interpolate::interpolator<T>().Interpolate(*source->Base(), target, method))
	{
		auto interpGrid = std::shared_ptr<grid>(baseGrid->Clone());

		interpGrid->UVRelativeToGrid(source->Grid()->UVRelativeToGrid());

		source->Base()->grid = interpGrid;
		source->Base()->data = std::move(target.data);

		return true;
	}

	return false;
}

template bool InterpolateArea<double>(const grid*, std::shared_ptr<info<double>>);
template bool InterpolateArea<float>(const grid*, std::shared_ptr<info<float>>);
template bool InterpolateArea<short>(const grid*, std::shared_ptr<info<short>>);
template bool InterpolateArea<unsigned char>(const grid*, std::shared_ptr<info<unsigned char>>);

template <typename T>
bool ReorderPoints(const grid* baseGrid, std::shared_ptr<info<T>> info)
{
	if (!info)
	{
		return false;
	}

	// Worst case: cartesian product ie O(n^2)

	auto targetStations = dynamic_cast<const point_list*>(baseGrid)->Stations();
	auto sourceStations = std::dynamic_pointer_cast<point_list>(info->Grid())->Stations();
	const auto& sourceData = info->Data();
	matrix<T> newData(targetStations.size(), 1, 1, MissingValue<T>());

	if (targetStations.size() == 0 || sourceStations.size() == 0)
	{
		return false;
	}

	std::vector<station> newStations;

	for (size_t i = 0; i < targetStations.size(); i++)
	{
		station s1 = targetStations[i];

		bool found = false;

		for (size_t j = 0; j < sourceStations.size() && !found; j++)
		{
			station s2 = sourceStations[j];

			if (s1.Id() == s2.Id())
			{
				newStations.push_back(s1);
				newData.Set(i, sourceData.At(j));

				found = true;
			}
		}

		if (!found)
		{
			// throw std::runtime_error("Failed, source data does not contain all the same points as target");
			return false;
		}
	}

	std::dynamic_pointer_cast<point_list>(info->Grid())->Stations(newStations);

	auto b = info->Base();
	b->data = std::move(newData);

	return true;
}

template bool ReorderPoints<double>(const grid*, std::shared_ptr<info<double>>);
template bool ReorderPoints<float>(const grid*, std::shared_ptr<info<float>>);
template bool ReorderPoints<short>(const grid*, std::shared_ptr<info<short>>);
template bool ReorderPoints<unsigned char>(const grid*, std::shared_ptr<info<unsigned char>>);

template <typename T>
bool Interpolate(const grid* baseGrid, const std::vector<std::shared_ptr<info<T>>>& infos)
{
	for (const auto& info : infos)
	{
		bool needInterpolation = false;
		bool needPointReordering = false;

		/*
		 * Possible scenarios:
		 * 1. from regular to regular (basic area&grid interpolation)
		 * 2. from regular to irregular (area to point)
		 * 3. from irregular to irregular (limited functionality, basically just point reordering)
		 * 4. from irregular to regular, not supported, except if source is gaussian
		 */

		// 1.

		if (baseGrid->Class() == kRegularGrid && info->Grid()->Class() == kRegularGrid)
		{
			if (*baseGrid != *info->Grid())
			{
				needInterpolation = true;
			}

			// == operator does not test scanning mode !

			else if (dynamic_cast<const regular_grid*>(baseGrid)->ScanningMode() !=
			         std::dynamic_pointer_cast<regular_grid>(info->Grid())->ScanningMode())
			{
#ifdef HAVE_CUDA
				if (info->PackedData()->HasData())
				{
					// must unpack before swapping
					util::Unpack<T>({info});
				}
#endif
				util::Flip<T>(info->Data());
				auto base = info->Base();
				const bool uv = base->grid->UVRelativeToGrid();
				base->grid = std::shared_ptr<himan::grid>(baseGrid->Clone());
				base->grid->UVRelativeToGrid(uv);
			}
		}

		// 2.

		else if (baseGrid->Class() == kIrregularGrid && info->Grid()->Class() == kRegularGrid)
		{
			needInterpolation = true;
		}

		// 3.

		else if (baseGrid->Class() == kIrregularGrid && info->Grid()->Class() == kIrregularGrid)
		{
			if (*baseGrid != *info->Grid())
			{
				needPointReordering = true;
			}
		}

		// 4.

		else if (baseGrid->Class() == kRegularGrid && info->Grid()->Class() == kIrregularGrid)
		{
			if (info->Grid()->Type() == kReducedGaussian)
			{
				needInterpolation = true;
			}
			else
			{
				throw std::runtime_error("Unable to extrapolate from points to grid");
			}
		}

		if (needInterpolation && InterpolateArea<T>(baseGrid, info) == false)
		{
			return false;
		}
		else if (needPointReordering && ReorderPoints<T>(baseGrid, info) == false)
		{
			return false;
		}
		else if (needInterpolation == false && needPointReordering == false)
		{
			logger logr("interpolate");
			logr.Trace("Areas are equal, no need to interpolate");
		}
	}

	return true;
}

template bool Interpolate<double>(const grid*, const std::vector<std::shared_ptr<info<double>>>&);
template bool Interpolate<float>(const grid*, const std::vector<std::shared_ptr<info<float>>>&);
template bool Interpolate<short>(const grid*, const std::vector<std::shared_ptr<info<short>>>&);
template bool Interpolate<unsigned char>(const grid*, const std::vector<std::shared_ptr<info<unsigned char>>>&);

bool IsVectorComponent(const std::string& paramName)
{
	if (paramName == "U-MS" || paramName == "V-MS" || paramName == "WGU-MS" || paramName == "WGV-MS" ||
	    paramName == "IVELU-MS" || paramName == "IVELV-MS" || paramName == "WVELU-MS" || paramName == "WVELV-MS")
	{
		return true;
	}

	return false;
}

bool IsNumericCodeTable(const std::string& paramName)
{
	if (paramName.find("NWCSAF_CLDTYPE") != std::string::npos ||
	    paramName.find("NWCSAF_CLDMASK") != std::string::npos || paramName == "NWCSAF_CTTH_QC-N" ||
	    paramName == "CLDTYPE-N")
	{
		return true;
	}
	return false;
}

HPInterpolationMethod InterpolationMethod(const std::string& paramName, HPInterpolationMethod interpolationMethod)
{
	// Later we'll add this information to radon directly
	if (interpolationMethod == kBiLinear &&
	    (
	        // vector parameters
	        IsVectorComponent(paramName) || paramName == "DD-D" || paramName == "FF-MS" ||
	        // precipitation
	        paramName == "RR-KGM2" || paramName == "SNR-KGM2" || paramName == "GRI-KGM2" || paramName == "RRR-KGM2" ||
	        paramName == "RRRC-KGM2" || paramName == "RRRL-KGM2" || paramName == "SNRC-KGM2" ||
	        paramName == "SNRL-KGM2" || paramName == "RRRS-KGM2" || paramName == "RR-1-MM" || paramName == "RR-3-MM" ||
	        paramName == "RR-6-MM" || paramName == "RRI-KGM2" || paramName == "SNRI-KGM2" ||
	        paramName == "SNACC-KGM2" ||
	        // symbols
	        paramName == "CLDSYM-N" || paramName == "PRECFORM-N" || paramName == "PRECFORM2-N" ||
	        paramName == "FOGSYM-N" || paramName == "ICING-N" || paramName == "POTPRECT-N" ||
	        paramName == "POTPRECF-N" || paramName == "FOGINT-N" || paramName == "PRECTYPE-N" ||
	        // code tables
	        IsNumericCodeTable(paramName)))
	{
#ifdef DEBUG
		std::cout << "Debug::interpolation Switching interpolation method from bilinear to nearest point" << std::endl;
#endif
		return kNearestPoint;  // nearest point in himan and newbase
	}

	return interpolationMethod;
}

template <typename T>
void RotateVectorComponentsCPU(const grid* from, const grid* to, himan::matrix<T>& U, himan::matrix<T>& V)
{
	// First convert to earth relative

	logger log("interpolate");

	if (from->UVRelativeToGrid() && from->Type() != kLatitudeLongitude)
	{
		log.Trace("Rotating from " + HPGridTypeToString.at(from->Type()) + " to earth relative");

		switch (from->Type())  // source type
		{
			case kLatitudeLongitude:
				break;
			case kRotatedLatitudeLongitude:
			{
				const auto rll = dynamic_cast<const rotated_latitude_longitude_grid*>(from);
				point southPole = rll->SouthPole();

				if (southPole.Y() > 0)
				{
					southPole.Y(-southPole.Y());
					southPole.X(0);
				}

				for (size_t i = 0; i < U.Size(); i++)
				{
					T u = U[i];
					T v = V[i];

					const point rotPoint = rll->RotatedLatLon(i);
					const point regPoint = rll->LatLon(i);

					// Algorithm by J.E. HAUGEN (HIRLAM JUNE -92), modified by K. EEROLA
					// Algorithm originally defined in hilake/TURNDD.F

					const double southPoleY = constants::kDeg * (southPole.Y() + 90);

					double sinPoleY, cosPoleY;
					sincos(southPoleY, &sinPoleY, &cosPoleY);

					const double cosRegY = cos(constants::kDeg * regPoint.Y());  // zcyreg
					const double zxmxc = constants::kDeg * (regPoint.X() - southPole.X());

					double sinxmxc, cosxmxc;
					sincos(zxmxc, &sinxmxc, &cosxmxc);

					const double rotXRad = constants::kDeg * rotPoint.X();
					const double rotYRad = constants::kDeg * rotPoint.Y();

					double sinRotX, cosRotX;
					sincos(rotXRad, &sinRotX, &cosRotX);

					double sinRotY, cosRotY;
					sincos(rotYRad, &sinRotY, &cosRotY);

					const double PA = cosxmxc * cosRotX + cosPoleY * sinxmxc * sinRotX;
					const double PB = cosPoleY * sinxmxc * cosRotX * sinRotY + sinPoleY * sinxmxc * cosRotY -
					                  cosxmxc * sinRotX * sinRotY;
					const double PC = (-sinPoleY) * sinRotX / cosRegY;
					const double PD = (cosPoleY * cosRotY - sinPoleY * cosRotX * sinRotY) / cosRegY;

					U[i] = static_cast<T>(PA * u + PB * v);
					V[i] = static_cast<T>(PC * u + PD * v);
				}
			}
			break;

			case kLambertConformalConic:
			{
				auto lcc = dynamic_cast<const lambert_conformal_grid*>(from);
				const double cone = lcc->Cone();
				const double orientation = lcc->Orientation();

				for (size_t i = 0; i < U.Size(); i++)
				{
					T u = U[i];
					T v = V[i];

					// http://www.mcs.anl.gov/~emconsta/wind_conversion.txt

					double angle = from->LatLon(i).X() - orientation;
					ASSERT(angle >= -180 && angle <= 180);

					const double anglex = cone * angle * constants::kDeg;
					double sinx, cosx;
					sincos(anglex, &sinx, &cosx);

					U[i] = static_cast<T>(cosx * u + sinx * v);
					V[i] = static_cast<T>(-1 * sinx * u + cosx * v);
				}
			}
			break;

			case kStereographic:
			{
				// The same as lambert but with cone = 1

				const double orientation = dynamic_cast<const stereographic_grid*>(from)->Orientation();

				for (size_t i = 0; i < U.Size(); i++)
				{
					T u = U[i];
					T v = V[i];

					const double angle = (from->LatLon(i).X() - orientation) * constants::kDeg;
					double sinx, cosx;

					sincos(angle, &sinx, &cosx);

					U[i] = static_cast<T>(cosx * u + sinx * v);
					V[i] = static_cast<T>(-1 * sinx * u + cosx * v);
				}
			}
			break;
			default:
				throw std::runtime_error("Unable to rotate from " + HPGridTypeToString.at(from->Type()) + " to " +
				                         HPGridTypeToString.at(to->Type()));
		}
	}

	if (to->UVRelativeToGrid() == false)
	{
		log.Trace("Result grid has UVRelativeToGrid=false, no need for further rotation");
		return;
	}

	switch (to->Type())
	{
		case kLatitudeLongitude:
			break;

		case kRotatedLatitudeLongitude:
		{
			const auto rll = dynamic_cast<const rotated_latitude_longitude_grid*>(to);
			point southPole = rll->SouthPole();

			if (southPole.Y() > 0)
			{
				southPole.Y(-southPole.Y());
				southPole.X(0);
			}

			for (size_t i = 0; i < U.Size(); i++)
			{
				T u = U[i];
				T v = V[i];

				const point rotPoint = rll->RotatedLatLon(i);
				const point regPoint = rll->LatLon(i);

				// Algorithm by J.E. HAUGEN (HIRLAM JUNE -92), modified by K. EEROLA
				// Algorithm originally defined in hilake/TURNDD.F

				const double southPoleY = constants::kDeg * (southPole.Y() + 90);

				double sinPoleY, cosPoleY;
				sincos(southPoleY, &sinPoleY, &cosPoleY);

				const double sinRegY = sin(constants::kDeg * regPoint.Y());  // zsyreg
				const double cosRegY = cos(constants::kDeg * regPoint.Y());  // zcyreg

				double zxmxc = constants::kDeg * (regPoint.X() - southPole.X());

				double sinxmxc, cosxmxc;
				sincos(zxmxc, &sinxmxc, &cosxmxc);

				const double rotXRad = constants::kDeg * rotPoint.X();

				double sinRotX, cosRotX;
				sincos(rotXRad, &sinRotX, &cosRotX);

				const double cosRotY = cos(constants::kDeg * rotPoint.Y());  // zcyrot

				const double PA = cosPoleY * sinxmxc * sinRotX + cosxmxc * cosRotX;
				const double PB =
				    cosPoleY * cosxmxc * sinRegY * sinRotX - sinPoleY * cosRegY * sinRotX - sinxmxc * sinRegY * cosRotX;
				const double PC = sinPoleY * sinxmxc / cosRotY;
				const double PD = (sinPoleY * cosxmxc * sinRegY + cosPoleY * cosRegY) / cosRotY;

				U[i] = static_cast<T>(PA * u + PB * v);
				V[i] = static_cast<T>(PC * u + PD * v);
			}
		}
		break;

		case kLambertConformalConic:
		{
			auto lcc = dynamic_cast<const lambert_conformal_grid*>(to);
			const double cone = lcc->Cone();
			const double orientation = lcc->Orientation();

			for (size_t i = 0; i < U.Size(); i++)
			{
				T u = U[i];
				T v = V[i];

				// http://www.mcs.anl.gov/~emconsta/wind_conversion.txt

				const double angle = to->LatLon(i).X() - orientation;
				ASSERT(angle >= -180 && angle <= 180);

				const double anglex = cone * angle * constants::kDeg;
				double sinx, cosx;
				sincos(anglex, &sinx, &cosx);

				U[i] = static_cast<T>(cosx * u - sinx * v);
				V[i] = static_cast<T>(sinx * u + cosx * v);
			}
		}
		break;
		case kStereographic:
		{
			const double orientation = dynamic_cast<const stereographic_grid*>(to)->Orientation();

			for (size_t i = 0; i < U.Size(); i++)
			{
				T u = U[i];
				T v = V[i];

				const double angle = (to->LatLon(i).X() - orientation) * constants::kDeg;
				double sinx, cosx;

				sincos(angle, &sinx, &cosx);

				U[i] = static_cast<T>(cosx * u - sinx * v);
				V[i] = static_cast<T>(sinx * u + cosx * v);
			}
		}
		break;

		default:
			throw std::runtime_error("Unable to rotate from " + HPGridTypeToString.at(from->Type()) + " to " +
			                         HPGridTypeToString.at(to->Type()));
	}
}

template void RotateVectorComponentsCPU<double>(const grid*, const grid*, himan::matrix<double>&,
                                                himan::matrix<double>&);
template void RotateVectorComponentsCPU<float>(const grid*, const grid*, himan::matrix<float>&, himan::matrix<float>&);
template void RotateVectorComponentsCPU<short>(const grid*, const grid*, himan::matrix<short>&, himan::matrix<short>&);
template void RotateVectorComponentsCPU<unsigned char>(const grid*, const grid*, himan::matrix<unsigned char>&,
                                                       himan::matrix<unsigned char>&);

template <typename T>
void RotateVectorComponents(const grid* from, const grid* to, himan::info<T>& UInfo, himan::info<T>& VInfo,
                            bool useCuda)
{
	ASSERT(UInfo.Grid()->UVRelativeToGrid() == VInfo.Grid()->UVRelativeToGrid());

	logger log("interpolate");
	if (!UInfo.Grid()->UVRelativeToGrid())
	{
		log.Trace("Source data is not relative to grid -- skipping rotation");
		return;
	}

#ifdef HAVE_CUDA
	if (useCuda)
	{
		if (UInfo.PackedData()->HasData() || VInfo.PackedData()->HasData())
		{
			throw std::runtime_error("Packed data needs to be unpacked before rotation on GPU");
		}

		cudaStream_t stream;
		CUDA_CHECK(cudaStreamCreate(&stream));
		RotateVectorComponentsGPU<T>(from, to, UInfo.Data(), VInfo.Data(), stream, 0, 0, 0);
		CUDA_CHECK(cudaStreamSynchronize(stream));
	}
	else
#endif
	{
		RotateVectorComponentsCPU<T>(from, to, UInfo.Data(), VInfo.Data());
	}

	UInfo.Grid()->UVRelativeToGrid(false);
	VInfo.Grid()->UVRelativeToGrid(false);
}

template void RotateVectorComponents<double>(const grid*, const grid*, info<double>&, info<double>&, bool);
template void RotateVectorComponents<float>(const grid*, const grid*, info<float>&, info<float>&, bool);
template void RotateVectorComponents<short>(const grid*, const grid*, info<short>&, info<short>&, bool);
template void RotateVectorComponents<unsigned char>(const grid*, const grid*, info<unsigned char>&,
                                                    info<unsigned char>&, bool);

template <typename T>
std::pair<std::vector<size_t>, std::vector<T>> InterpolationWeights(reduced_gaussian_grid& source, point target)
{
	// target lon 0 <= lon < 360
	if (target.X() < 0.0)
		target.X(target.X() + 360.0);
	else if (target.X() >= 360.0)
		target.X(target.X() - 360.0);

	const auto lats = source.Latitudes();

	// check if point is inside domain
	if (target.Y() >= lats.front())
	{
		// point north of domain
		return std::make_pair(std::vector<size_t>{0}, std::vector<T>{MissingValue<T>()});
	}

	if (target.Y() <= lats.back())
	{
		// point south of domain
		return std::make_pair(std::vector<size_t>{0}, std::vector<T>{MissingValue<T>()});
	}

	// find y-indices
	auto south = std::lower_bound(lats.begin(), lats.end(), target.Y(), std::greater_equal<double>());
	auto north = south;
	north--;

	size_t y_north = static_cast<size_t>(std::distance(lats.begin(), north));
	size_t y_south = static_cast<size_t>(std::distance(lats.begin(), south));

	// find x-indices
	size_t x_north_west = static_cast<size_t>(
	    std::floor(static_cast<double>(source.NumberOfPointsAlongParallels()[y_north]) * target.X() / 360.));
	size_t x_north_east =
	    x_north_west < static_cast<size_t>(source.NumberOfPointsAlongParallels()[y_north] - 1) ? x_north_west + 1 : 0;
	size_t x_south_west = static_cast<size_t>(
	    std::floor(static_cast<double>(source.NumberOfPointsAlongParallels()[y_south]) * target.X() / 360.));
	size_t x_south_east =
	    x_south_west < static_cast<size_t>(source.NumberOfPointsAlongParallels()[y_south] - 1) ? x_south_west + 1 : 0;

	/*
	 *
	 *  a---------------b
	 *  |               |
	 *   \     p        |
	 *   |              |
	 *   c--------------d
	 *
	 *  Now we know the indices x|y of the four points a,b,c,d that surround p
	 */

	std::vector<size_t> idxs;
	idxs.reserve(4);
	idxs.push_back(source.LocationIndex(x_north_west, y_north));
	idxs.push_back(source.LocationIndex(x_north_east, y_north));
	idxs.push_back(source.LocationIndex(x_south_west, y_south));
	idxs.push_back(source.LocationIndex(x_south_east, y_south));

	// calculate weights by bilinear interpolation to point target (p) surrounded by points A|B|C|D
	const point& p = target;
	const point a = source.LatLon(idxs[0]);
	const point b = source.LatLon(idxs[1]);
	const point c = source.LatLon(idxs[2]);
	const point d = source.LatLon(idxs[3]);

	std::vector<T> weights(4);

	// Matrices and Vectors
	Eigen::Matrix4d A(4, 4);
	Eigen::MatrixXd xi(4, 2);
	Eigen::Vector4d x(4);
	Eigen::Vector2d phi(2);

	// Construct linear system of equations to be solved
	A(0, 0) = 1;
	A(1, 0) = 1;
	A(2, 0) = 1;
	A(3, 0) = 1;
	A(0, 1) = a.X();
	A(1, 1) = b.X() == 0.0 ? 360 : b.X();

	A(2, 1) = c.X();
	A(3, 1) = d.X() == 0.0 ? 360 : d.X();

	A(0, 2) = a.Y();
	A(1, 2) = b.Y();
	A(2, 2) = c.Y();
	A(3, 2) = d.Y();
	A(0, 3) = a.X() * a.Y();
	A(1, 3) = b.X() == 0.0 ? 360 * b.Y() : b.X() * b.Y();
	A(2, 3) = c.X() * c.Y();
	A(3, 3) = d.X() == 0.0 ? 360 * d.Y() : d.X() * d.Y();

	xi(0, 0) = 0;
	xi(1, 0) = 1;
	xi(2, 0) = 0;
	xi(3, 0) = 1;
	xi(0, 1) = 1;
	xi(1, 1) = 1;
	xi(2, 1) = 0;
	xi(3, 1) = 0;

	// Solve linear system
	xi = A.colPivHouseholderQr().solve(xi);

	x[0] = 1;
	x[1] = p.X();
	x[2] = p.Y();
	x[3] = p.X() * p.Y();

	xi.transposeInPlace();
	phi = xi * x;

	weights[0] = static_cast<T>((1 - phi[0]) * phi[1]);
	weights[1] = static_cast<T>(phi[0] * phi[1]);
	weights[2] = static_cast<T>((1 - phi[0]) * (1 - phi[1]));
	weights[3] = static_cast<T>(phi[0] * (1 - phi[1]));

	return std::make_pair(idxs, weights);
}

template <typename T>
std::pair<std::vector<size_t>, std::vector<T>> InterpolationWeights(const regular_grid& source, const point& xy)
{
	if (IsMissing(xy.X()) || IsMissing(xy.Y()))
		return std::make_pair(std::vector<size_t>{0}, std::vector<T>{MissingValue<T>()});

	std::vector<size_t> idxs{static_cast<size_t>(xy.X()) + source.Ni() * static_cast<size_t>(xy.Y()),
	                         static_cast<size_t>(xy.X()) + source.Ni() * static_cast<size_t>(xy.Y()) + 1 -
	                             (static_cast<size_t>(xy.X()) == source.Ni() - 1 ? source.Ni() : 0),
	                         static_cast<size_t>(xy.X()) + source.Ni() * static_cast<size_t>(xy.Y() + 1),
	                         static_cast<size_t>(xy.X()) + source.Ni() * static_cast<size_t>(xy.Y() + 1) + 1 -
	                             (static_cast<size_t>(xy.X()) == source.Ni() - 1 ? source.Ni() : 0)};

	std::vector<T> weights(4);

	weights[0] = static_cast<T>((1 - std::fmod(xy.X(), 1)) * (1 - std::fmod(xy.Y(), 1)));
	weights[1] = static_cast<T>(std::fmod(xy.X(), 1) * (1 - std::fmod(xy.Y(), 1)));
	weights[2] = static_cast<T>((1 - std::fmod(xy.X(), 1)) * std::fmod(xy.Y(), 1));
	weights[3] = static_cast<T>(std::fmod(xy.X(), 1) * std::fmod(xy.Y(), 1));

	// Index is outside grid. This happens when target point is located on bottom edge
	// Set index to first grid point
	for (auto& idx : idxs)
	{
		if (idx > source.Size() - 1)
		{
			idx = 0;
		}
	}

	return std::make_pair(idxs, weights);
}

template <typename T>
std::vector<std::pair<std::vector<size_t>, std::vector<T>>> InterpolationWeights(const regular_grid& source,
                                                                                const std::vector<point>& targets)
{
	std::vector<std::pair<std::vector<size_t>, std::vector<T>>> ret;
	ret.reserve(targets.size());

	for (const auto& xy : targets)
	{
		ret.push_back(InterpolationWeights<T>(source, xy));
	}

	return ret;
}

template <typename T>
std::pair<size_t, T> NearestPoint(reduced_gaussian_grid& source, point target)
{
	// target lon 0 <= lon < 360
	if (target.X() < 0.0)
		target.X(target.X() + 360.0);
	else if (target.X() >= 360.0)
		target.X(target.X() - 360.0);

	const auto lats = source.Latitudes();

	// find y-indices
	auto south = std::lower_bound(lats.begin(), lats.end(), target.Y(), std::greater_equal<double>());
	auto north = south;
	north--;

	size_t y_north = std::distance(lats.begin(), north);
	size_t y_south = std::distance(lats.begin(), south);

	// find x-indices
	size_t x_north_west = static_cast<size_t>(
	    std::floor(static_cast<double>(source.NumberOfPointsAlongParallels()[y_north]) * target.X() / 360.));
	size_t x_north_east =
	    x_north_west < static_cast<size_t>(source.NumberOfPointsAlongParallels()[y_north] - 1) ? x_north_west + 1 : 0;
	size_t x_south_west = static_cast<size_t>(
	    std::floor(static_cast<double>(source.NumberOfPointsAlongParallels()[y_south]) * target.X() / 360.));
	size_t x_south_east =
	    x_south_west < static_cast<size_t>(source.NumberOfPointsAlongParallels()[y_south] - 1) ? x_south_west + 1 : 0;

	size_t nearest = x_north_west;
	for (auto p : {x_north_east, x_south_west, x_south_east})
	{
		if (geoutil::Distance(source.LatLon(p), target) < geoutil::Distance(source.LatLon(nearest), target))
			nearest = p;
	}

	return std::make_pair(nearest, 1.0);
}

template <typename T>
std::pair<size_t, T> NearestPoint(const regular_grid& source, const point& xy)
{
	if (IsMissing(xy.X()) || IsMissing(xy.Y()))
	{
		return std::make_pair(0, MissingValue<T>());
	}
	// In case of point in wrap-around region on global grid
	if (static_cast<size_t>(std::round(xy.X())) == source.Ni())
	{
		return std::make_pair(source.Ni() * static_cast<size_t>(std::round(xy.Y())), 1.0);
	}
	return std::make_pair(
	    static_cast<size_t>(std::round(xy.X())) + source.Ni() * static_cast<size_t>(std::round(xy.Y())), 1.0);
}

template <typename T>
std::vector<std::pair<size_t, T>> NearestPoint(const regular_grid& source, const std::vector<point>& targets)
{
	std::vector<std::pair<size_t, T>> ret;
	ret.reserve(targets.size());

	for (const auto& xy : targets)
	{
		ret.push_back(NearestPoint<T>(source, xy));
	}

	return ret;
}

// area_interpolation class member functions definitions
template <typename T>
area_interpolation<T>::area_interpolation(grid& source, grid& target, HPInterpolationMethod method)
    : itsInterpolation(target.Size(), source.Size())
{
	std::vector<Triplet<T>> coefficients;

	std::string useOldMethod = "no";
	try
	{
		useOldMethod = util::GetEnv("HIMAN_USE_OLD_PROJECTION_METHOD");
	}
	catch (...)
	{
	}

	// default to using new 'bulk' method
	if (source.Class() == target.Class() && source.Class() == kRegularGrid && useOldMethod == "no")
	{
		const regular_grid& sg = dynamic_cast<regular_grid&>(source);
		const regular_grid& tg = dynamic_cast<regular_grid&>(target);

		std::vector<point> xy = sg.XY(tg);
		std::vector<std::pair<std::vector<size_t>, std::vector<T>>> ws;

		if (method == kBiLinear)
		{
			ws = InterpolationWeights<T>(sg, xy);
		}
		else if (method == kNearestPoint)
		{
			auto np = NearestPoint<T>(sg, xy);

			for (size_t i = 0; i < np.size(); i++)
			{
				std::pair<std::vector<size_t>, std::vector<T>> w;
				w.first.push_back(np[i].first);
				w.second.push_back(np[i].second);
				ws.push_back(w);
			}
		}
		else
		{
			throw std::bad_typeid();
		}

		for (size_t i = 0; i < ws.size(); i++)
		{
			const auto& w = ws[i];
			for (size_t j = 0; j < w.first.size(); ++j)
			{
				coefficients.push_back(Triplet<T>(static_cast<int>(i), static_cast<int>(w.first[j]), w.second[j]));
			}
		}
	}
	else
	{
		// compute weights in the interpolation matrix line by line, i.e. point by point on target grid
		for (size_t i = 0; i < target.Size(); ++i)
		{
			std::pair<std::vector<size_t>, std::vector<T>> w;
			switch (source.Type())
			{
				case kLatitudeLongitude:
				case kRotatedLatitudeLongitude:
				case kStereographic:
				case kLambertConformalConic:
				case kLambertEqualArea:
				case kTransverseMercator:
					if (method == kBiLinear)
					{
						w = InterpolationWeights<T>(dynamic_cast<regular_grid&>(source),
						                            dynamic_cast<regular_grid&>(source).XY(target.LatLon(i)));
					}
					else if (method == kNearestPoint)
					{
						auto np = NearestPoint<T>(dynamic_cast<regular_grid&>(source),
						                          dynamic_cast<regular_grid&>(source).XY(target.LatLon(i)));
						w.first.push_back(np.first);
						w.second.push_back(np.second);
					}
					else
					{
						throw std::bad_typeid();
					}
					break;
				case kReducedGaussian:
					if (method == kBiLinear)
					{
						w = InterpolationWeights<T>(dynamic_cast<reduced_gaussian_grid&>(source), target.LatLon(i));
					}
					else if (method == kNearestPoint)
					{
						auto np = NearestPoint<T>(dynamic_cast<reduced_gaussian_grid&>(source), target.LatLon(i));
						w.first.push_back(np.first);
						w.second.push_back(np.second);
					}
					else
					{
						throw std::bad_typeid();
					}
					break;
				default:
					// what to throw?
					throw std::bad_typeid();
					break;
			}

			for (size_t j = 0; j < w.first.size(); ++j)
			{
				coefficients.push_back(Triplet<T>(static_cast<int>(i), static_cast<int>(w.first[j]), w.second[j]));
			}
		}
	}

	itsInterpolation.setFromTriplets(coefficients.begin(), coefficients.end());
}

template <typename T>
void area_interpolation<T>::Interpolate(base<T>& source, base<T>& target)
{
	Map<Matrix<T, Dynamic, Dynamic>> srcValues(source.data.ValuesAsPOD(), source.data.Size(), 1);
	Map<Matrix<T, Dynamic, Dynamic>> trgValues(target.data.ValuesAsPOD(), target.data.Size(), 1);

	trgValues = itsInterpolation * srcValues;
}

template <typename T>
size_t area_interpolation<T>::SourceSize() const
{
	return itsInterpolation.cols();
}

template <typename T>
size_t area_interpolation<T>::TargetSize() const
{
	return itsInterpolation.rows();
}

// Interpolator member functions
template <typename T>
std::map<size_t, area_interpolation<T>> interpolator<T>::cache;

template <typename T>
std::mutex interpolator<T>::interpolatorAccessMutex;

template <typename T>
bool interpolator<T>::Insert(const base<T>& source, const base<T>& target, HPInterpolationMethod method)
{
	std::lock_guard<std::mutex> guard(interpolatorAccessMutex);

	std::pair<size_t, himan::interpolate::area_interpolation<T>> insertValue;

	try
	{
		std::vector<size_t> hashes{method, source.grid->Hash(), target.grid->Hash()};
		insertValue.first = boost::hash_range(hashes.begin(), hashes.end());

		// area_interpolation is already present in cache
		if (cache.count(insertValue.first) > 0)
		{
			return true;
		}

		insertValue.second = himan::interpolate::area_interpolation<T>(*source.grid, *target.grid, method);
	}
	catch (const std::exception& e)
	{
		std::cerr << "Creating interpolation coefficients failed with " << e.what() << "\n";
		himan::Abort();
	}

	return cache.insert(std::move(insertValue)).second;
}

// template bool interpolator::Insert<double>(const base<double>&, const base<double>&, HPInterpolationMethod);
// template bool interpolator::Insert<float>(const base<float>&, const base<float>&, HPInterpolationMethod);

template <typename T>
bool interpolator<T>::Interpolate(base<T>& source, base<T>& target, HPInterpolationMethod method)
{
	std::vector<size_t> hashes{method, source.grid->Hash(), target.grid->Hash()};
	auto it = cache.find(boost::hash_range(hashes.begin(), hashes.end()));

	if (it != cache.end())
	{
		try
		{
			it->second.Interpolate(source, target);
			return true;
		}
		catch (const boost::bad_get& e)
		{
			std::cout << e.what() << std::endl;
			return false;
		}
	}

	else
	{
		Insert(source, target, method);
		return Interpolate(source, target, method);
	}
}

// template bool interpolator::Interpolate<double>(base<double>&, base<double>&, HPInterpolationMethod);
// template bool interpolator::Interpolate<float>(base<float>&, base<float>&, HPInterpolationMethod);

}  // namespace interpolate
}  // namespace himan


#ifndef TVLQR_CONTROL_HPP
#define TVLQR_CONTROL_HPP

/*
 * Implements a Time Varying Linear Quadratic Regulator using trajectories
 *
 * Author: Andrew Barry, <abarry@csail.mit.edu> 2015
 *
 */

#include <iostream>
#include <string>
#include <fstream>
#include <vector>
#include <sstream>

#include <bot_core/rotations.h>
#include <bot_frames/bot_frames.h>

#include "../TrajectoryLibrary/Trajectory.hpp"
#include "../../LCM/mav_pose_t.h"

#include <Eigen/Core>

#include "../../utils/utils/RealtimeUtils.hpp"

#include "../../utils/ServoConverter/ServoConverter.hpp"

class TvlqrControl
{

    public:
        TvlqrControl(const ServoConverter *converter, const Trajectory &stable_controller);

        void SetTrajectory(const Trajectory &trajectory);

        bool HasTrajectory() const { return current_trajectory_ != nullptr; }

        Eigen::VectorXi GetControl(const mav_pose_t *msg);

        void SetStateEstimatorInitialized();

        bool IsTimeInvariant() const { return HasTrajectory() && current_trajectory_->IsTimeInvariant(); }

    private:

        void InitializeState(const mav_pose_t *msg);
        double GetTNow() const;
        Eigen::VectorXd GetStateMinusInit(const mav_pose_t *msg);

        const Trajectory *current_trajectory_;
        const Trajectory *stable_controller_;

        Eigen::VectorXd initial_state_;
        Eigen::VectorXd last_state_; // keep so we can do angle unwrapping
        Eigen::Matrix3d Mz_; // rotation matrix that transforms global state into local state by removing yaw

        bool state_initialized_;

        const ServoConverter *converter_;

        int64_t t0_;
        double last_ti_state_estimator_reset_;

};

#endif


/*
 * Copyright (c) 2019, The Regents of the University of California (Regents).
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are
 * met:
 *
 *    1. Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *
 *    2. Redistributions in binary form must reproduce the above
 *       copyright notice, this list of conditions and the following
 *       disclaimer in the documentation and/or other materials provided
 *       with the distribution.
 *
 *    3. Neither the name of the copyright holder nor the names of its
 *       contributors may be used to endorse or promote products derived
 *       from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS AS IS
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 * Please contact the author(s) of this library if you have any questions.
 * Authors: David Fridovich-Keil   ( dfk@eecs.berkeley.edu )
 */

///////////////////////////////////////////////////////////////////////////////
//
// 14D quadrotor dynamics.
//
///////////////////////////////////////////////////////////////////////////////

#include <quads/quadrotor14d.h>
#include <quads/types.h>

#include <ros/ros.h>
#include <Eigen/Dense>
#include <string>
#include <iostream>

namespace quads {

Vector14d Quadrotor14D::operator()(const Vector14d& x,
                                   const Vector4d& u) const {
  ROS_ASSERT(initialized_);

  // Precompute sines/consines.
  const double cpsi = std::cos(x(kPsiIdx));
  const double spsi = std::sin(x(kPsiIdx));
  const double cphi = std::cos(x(kPhiIdx));
  const double sphi = std::sin(x(kPhiIdx));
  const double ctheta = std::cos(x(kThetaIdx));
  const double stheta = std::sin(x(kThetaIdx));

  const double gx = (cpsi * stheta * cphi + spsi * sphi) / m_;
  const double gy = (spsi * stheta * cphi - cpsi * sphi) / m_;
  const double gz = cphi * ctheta / m_;

  Vector14d xdot;
  xdot << x(kDxIdx), x(kDyIdx), x(kDzIdx), x(kQIdx), x(kRIdx), x(kPIdx),
      gx * x(kZetaIdx), gy * x(kZetaIdx), gz * x(kZetaIdx) - 9.81, x(kXiIdx),
      u(0), u(2) / Iy_, u(1) / Ix_, u(3) / Iz_;

  return xdot;
}

Matrix14x14d Quadrotor14D::StateJacobian(const Vector14d& x,
                                         const Vector4d& u) const {
  const double theta = x(kThetaIdx);
  const double psi = x(kPsiIdx);
  const double phi = x(kPhiIdx);
  const double zeta = x(kZetaIdx);

  Matrix14x14d F;
  F << 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0,
      0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
      0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0,
      0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0,
      zeta * std::cos(phi) * std::cos(psi) * std::cos(theta),
      zeta * (std::cos(phi) * std::sin(psi) -
              std::cos(psi) * std::sin(phi) * std::sin(theta)),
      zeta * (std::cos(psi) * std::sin(phi) -
              std::cos(phi) * std::sin(psi) * std::sin(theta)),
      0, 0, 0, std::sin(phi) * std::sin(psi) +
                   std::cos(phi) * std::cos(psi) * std::sin(theta),
      0, 0, 0, 0, 0, 0, 0,
      zeta * std::cos(phi) * std::cos(theta) * std::sin(psi),
      -zeta * (std::cos(phi) * std::cos(psi) +
               std::sin(phi) * std::sin(psi) * std::sin(theta)),
      zeta * (std::sin(phi) * std::sin(psi) +
              std::cos(phi) * std::cos(psi) * std::sin(theta)),
      0, 0, 0, std::cos(phi) * std::sin(psi) * std::sin(theta) -
                   std::cos(psi) * std::sin(phi),
      0, 0, 0, 0, 0, 0, 0, -zeta * std::cos(phi) * std::sin(theta),
      -zeta * std::cos(theta) * std::sin(phi), 0, 0, 0, 0,
      std::cos(phi) * std::cos(theta), 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
      1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
      0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
      0, 0, 0, 0, 0, 0, 0, 0, 0, 0;

  return F;
}

Matrix6x14d Quadrotor14D::OutputJacobian(const Vector14d& x) const {
  Matrix6x14d H(Matrix6x14d::Zero());
  H(0, 0) = 1.0;
  H(1, 1) = 1.0;
  H(2, 2) = 1.0;
  H(3, 3) = 1.0;
  H(4, 4) = 1.0;
  H(5, 5) = 1.0;
  return H;
}

bool Quadrotor14D::Initialize(const ros::NodeHandle& n) {
  name_ = ros::names::append(n.getNamespace(), "quadrotor14d");

  if (!LoadParameters(n)) {
    ROS_ERROR("%s: Failed to load parameters.", name_.c_str());
    return false;
  }

  initialized_ = true;
  return true;
}

bool Quadrotor14D::LoadParameters(const ros::NodeHandle& n) {
  ros::NodeHandle nl(n);

  // Mass and inertia.
  if (!nl.getParam("dynamics/m", m_)) return false;
  if (!nl.getParam("dynamics/Ix", Ix_)) return false;
  if (!nl.getParam("dynamics/Iy", Iy_)) return false;
  if (!nl.getParam("dynamics/Iz", Iz_)) return false;

  return true;
}

}  // namespace quads


#include <CGAL/Three/Polyhedron_demo_plugin_interface.h>
#include <CGAL/Three/Scene_interface.h>
#include <QApplication>
#include <QObject>
#include <QAction>
#include <QMainWindow>
#include <QInputDialog>
#include <QMessageBox>
#include <QMap>
#include "Messages_interface.h"
#ifdef USE_SURFACE_MESH
#include "Kernel_type.h"
#include "Scene_surface_mesh_item.h"
#else
#include "Scene_polyhedron_item.h"
#include "Polyhedron_type.h"
#endif
#include "Color_ramp.h"
#include "triangulate_primitive.h"
#include <CGAL/Polygon_mesh_processing/bbox.h>
#include <CGAL/Polygon_mesh_processing/distance.h>
#include <CGAL/Polygon_mesh_processing/compute_normal.h>
#include <boost/iterator/counting_iterator.hpp>
#include <CGAL/Search_traits_3.h>
#include <CGAL/Spatial_sort_traits_adapter_3.h>
#include <CGAL/property_map.h>
#include <boost/container/flat_map.hpp>

using namespace CGAL::Three;
namespace PMP = CGAL::Polygon_mesh_processing;

#ifdef USE_SURFACE_MESH
typedef Scene_surface_mesh_item Scene_face_graph_item;
#else
typedef Scene_polyhedron_item Scene_face_graph_item;
#endif

typedef Scene_face_graph_item::Face_graph Face_graph;

#if defined(CGAL_LINKED_WITH_TBB)
template <class AABB_tree, class Point_3>
struct Distance_computation{
  const AABB_tree& tree;
  const std::vector<Point_3>& sample_points;
  Point_3 initial_hint;
  tbb::atomic<double>* distance;
  std::vector<double>& output;

  Distance_computation(const AABB_tree& tree,
                       const Point_3 p,
                       const std::vector<Point_3>& sample_points,
                       tbb::atomic<double>* d,
                       std::vector<double>& out )
    : tree(tree)
    , sample_points(sample_points)
    , initial_hint(p)
    , distance(d)
    , output(out)
  {
  }
  void
  operator()(const tbb::blocked_range<std::size_t>& range) const
  {
    Point_3 hint = initial_hint;
    double hdist = 0;
    for( std::size_t i = range.begin(); i != range.end(); ++i)
    {
      hint = tree.closest_point(sample_points[i], hint);
      Kernel::FT dist = squared_distance(hint,sample_points[i]);
      double d = CGAL::sqrt(dist);
      output[i] = d;
      if (d>hdist) hdist=d;
    }

    if (hdist > distance->load())
      distance->store(hdist);
  }
};
#endif

class Scene_distance_polyhedron_item: public Scene_item
{
  Q_OBJECT
public:
  Scene_distance_polyhedron_item(Face_graph* poly, Face_graph* polyB, QString other_name, int sampling_pts)
    :Scene_item(NbOfVbos,NbOfVaos),
      poly(poly),
      poly_B(polyB),
      are_buffers_filled(false),
      other_poly(other_name)
  {
    nb_pts_per_face = sampling_pts;
    this->setRenderingMode(FlatPlusEdges);
    thermal_ramp.build_thermal();
  }
  bool supportsRenderingMode(RenderingMode m) const {
    return (m == Flat || m == FlatPlusEdges);
  }
  Scene_item* clone() const {return 0;}
  QString toolTip() const {return QString("Item %1 with color indicating distance with %2").arg(this->name()).arg(other_poly);}
  void draw(Viewer_interface *viewer) const
  {
    if(!are_buffers_filled)
    {
      computeElements();
      initializeBuffers(viewer);
      compute_bbox();
    }
    vaos[Facets]->bind();
    attribBuffers(viewer, PROGRAM_WITH_LIGHT);
    program = getShaderProgram(PROGRAM_WITH_LIGHT);
    program->bind();
    program->setUniformValue("is_selected", false);
    viewer->glDrawArrays(GL_TRIANGLES, 0, static_cast<GLsizei>(nb_pos/3));
    program->release();
    vaos[Facets]->release();
  }
  void drawEdges(Viewer_interface* viewer) const
  {
    vaos[Edges]->bind();

    attribBuffers(viewer, PROGRAM_WITHOUT_LIGHT);
    program = getShaderProgram(PROGRAM_WITHOUT_LIGHT);
    program->bind();
    //draw the edges
    program->setAttributeValue("colors", QColor(Qt::black));
    program->setUniformValue("is_selected", false);
    viewer->glDrawArrays(GL_LINES, 0, static_cast<GLsizei>(nb_edge_pos/3));
    vaos[Edges]->release();
    program->release();
  }

  void compute_bbox() const {
    _bbox = PMP::bbox(*poly);
  }

private:
  Face_graph* poly;
  Face_graph* poly_B;
  mutable bool are_buffers_filled;
  QString other_poly;
  mutable std::vector<float> m_vertices;
  mutable std::vector<float> edge_vertices;
  mutable std::vector<float> normals;
  mutable std::vector<float> colors;
  Color_ramp thermal_ramp;
  int nb_pts_per_face;

  enum VAOs {
    Facets=0,
    Edges,
    NbOfVaos};

  enum VBOs {
    Vertices=0,
    Edge_vertices,
    Normals,
    Colors,
    NbOfVbos};

  mutable std::size_t nb_pos;
  mutable std::size_t nb_edge_pos;
  mutable QOpenGLShaderProgram *program;

  //fills 'out' and returns the hausdorff distance for calibration of the color_ramp.

  double compute_distances(const Face_graph& m, const std::vector<Kernel::Point_3>& sample_points,
                           std::vector<double>& out)const
  {
    typedef CGAL::AABB_face_graph_triangle_primitive<Face_graph> Primitive;
    typedef CGAL::AABB_traits<Kernel, Primitive> Traits;
    typedef CGAL::AABB_tree< Traits > Tree;

    Tree tree( faces(m).first, faces(m).second, m);
    tree.accelerate_distance_queries();
    tree.build();
    boost::graph_traits<Face_graph>::vertex_descriptor vd = *(vertices(m).first);
    Traits::Point_3 hint = get(CGAL::vertex_point,*poly, vd);

#if !defined(CGAL_LINKED_WITH_TBB)
    double hdist = 0;
    for(std::size_t i = 0; i<sample_points.size(); ++i)
    {
      hint = tree.closest_point(sample_points[i], hint);
      Kernel::FT dist = squared_distance(hint,sample_points[i]);
      double d = CGAL::sqrt(dist);
      out[i]= d;
      if (d>hdist) hdist=d;
    }
      return hdist;
#else
    tbb::atomic<double> distance;
    distance.store(0);
    Distance_computation<Tree, Kernel::Point_3> f(tree, hint, sample_points, &distance, out);
    tbb::parallel_for(tbb::blocked_range<std::size_t>(0, sample_points.size()), f);
    return distance;
#endif
  }

  void computeElements()const
  {
    QApplication::setOverrideCursor(Qt::WaitCursor);
    m_vertices.resize(0);
    edge_vertices.resize(0);
    normals.resize(0);
    colors.resize(0);

    typedef Kernel::Vector_3	    Vector;
    typedef boost::graph_traits<Face_graph>::face_descriptor   face_descriptor;
    typedef boost::graph_traits<Face_graph>::vertex_descriptor vertex_descriptor;

    typedef boost::property_map<Face_graph,CGAL::vertex_point_t>::type VPmap;
    VPmap vpmap = get(CGAL::vertex_point,*poly);

    //facets
    {
      boost::container::flat_map<face_descriptor, Vector> face_normals_map;
      boost::associative_property_map< boost::container::flat_map<face_descriptor, Vector> >
          nf_pmap(face_normals_map);
      boost::container::flat_map<vertex_descriptor, Vector> vertex_normals_map;
      boost::associative_property_map< boost::container::flat_map<vertex_descriptor, Vector> >
          nv_pmap(vertex_normals_map);

      PMP::compute_normals(*poly, nv_pmap, nf_pmap);
      std::vector<Kernel::Point_3> total_points(0);

      BOOST_FOREACH(boost::graph_traits<Face_graph>::face_descriptor f, faces(*poly)) {
        Vector nf = get(nf_pmap, f);
        typedef FacetTriangulator<Face_graph, Kernel, boost::graph_traits<Face_graph>::vertex_descriptor> FT;
        double diagonal;
        if(this->diagonalBbox() != std::numeric_limits<double>::infinity())
          diagonal = this->diagonalBbox();
        else
          diagonal = 0.0;

        //compute distance with other polyhedron
        //sample facet
        std::vector<Kernel::Point_3> sampled_points;
        std::size_t nb_points =  (std::max)((int)std::ceil(nb_pts_per_face * PMP::face_area(f,*poly,PMP::parameters::geom_traits(Kernel()))),
                                            1);
        Kernel::Point_3 &p = get(vpmap,target(halfedge(f,*poly),*poly));
        Kernel::Point_3 &q = get(vpmap,target(next(halfedge(f,*poly),*poly),*poly));
        Kernel::Point_3 &r = get(vpmap,target(next(next(halfedge(f,*poly),*poly),*poly),*poly));
        CGAL::Random_points_in_triangle_3<Kernel::Point_3> g(p, q, r);
        CGAL::cpp11::copy_n(g, nb_points, std::back_inserter(sampled_points));
        sampled_points.push_back(p);
        sampled_points.push_back(q);
        sampled_points.push_back(r);

        //triangle facets with sample points for color display
        FT triangulation(f,sampled_points,nf,poly,diagonal);

        if(triangulation.cdt->dimension() != 2 )
        {
          qDebug()<<"Error : cdt not right (dimension != 2). Facet not displayed";
          continue;
        }

        //iterates on the internal faces to add the vertices to the positions
        //and the normals to the appropriate vectors

        for(FT::CDT::Finite_faces_iterator
            ffit = triangulation.cdt->finite_faces_begin(),
            end = triangulation.cdt->finite_faces_end();
            ffit != end; ++ffit)
        {
          if(ffit->info().is_external)
            continue;

          for (int i = 0; i<3; ++i)
          {
            total_points.push_back(ffit->vertex(i)->point());
            m_vertices.push_back(ffit->vertex(i)->point().x());
            m_vertices.push_back(ffit->vertex(i)->point().y());
            m_vertices.push_back(ffit->vertex(i)->point().z());

            normals.push_back(nf.x());
            normals.push_back(nf.y());
            normals.push_back(nf.z());
          }
        }
      }
      //compute the distances
      typedef CGAL::Spatial_sort_traits_adapter_3<Kernel,
                CGAL::Pointer_property_map<Kernel::Point_3>::type > Search_traits_3;

      std::vector<double> distances(total_points.size());
      std::vector<std::size_t> indices;
      indices.reserve(total_points.size());
      std::copy(boost::counting_iterator<std::size_t>(0),
                boost::counting_iterator<std::size_t>(total_points.size()),
                std::back_inserter(indices));
      spatial_sort(indices.begin(),
                   indices.end(),
                  Search_traits_3(CGAL::make_property_map(total_points)));
      std::vector<Kernel::Point_3> sorted_points(total_points.size());
      for(std::size_t i = 0; i < sorted_points.size(); ++i)
      {
        sorted_points[i] = total_points[indices[i]];
      }

      double hausdorff = compute_distances(*poly_B,
                                           sorted_points,
                                           distances);
      //compute the colors
      colors.resize(sorted_points.size()*3);
      for(std::size_t i=0; i<sorted_points.size(); ++i)
      {
        std::size_t k = indices[i];
        double d = distances[i]/hausdorff;
        colors[3*k]=thermal_ramp.r(d);
        colors[3*k+1]=thermal_ramp.g(d);
        colors[3*k+2]=thermal_ramp.b(d);
      }
    }

    //edges
    {
      //Lines
      typedef Kernel::Point_3		Point;
      typedef boost::graph_traits<Face_graph>::edge_descriptor	edge_descriptor;

      BOOST_FOREACH(edge_descriptor he, edges(*poly)){
        const Point& a = get(vpmap,target(he,*poly));
        const Point& b = get(vpmap,source(he,*poly));
        {

          edge_vertices.push_back(a.x());
          edge_vertices.push_back(a.y());
          edge_vertices.push_back(a.z());

          edge_vertices.push_back(b.x());
          edge_vertices.push_back(b.y());
          edge_vertices.push_back(b.z());
        }
      }
    }
    QApplication::restoreOverrideCursor();
  }
  void initializeBuffers(Viewer_interface *viewer)const
  {

    program = getShaderProgram(PROGRAM_WITH_LIGHT, viewer);
    program->bind();
    vaos[Facets]->bind();
    buffers[Vertices].bind();
    buffers[Vertices].allocate(m_vertices.data(),
                               static_cast<GLsizei>(m_vertices.size()*sizeof(float)));
    program->enableAttributeArray("vertex");
    program->setAttributeBuffer("vertex",GL_FLOAT,0,3);
    buffers[Vertices].release();
    buffers[Normals].bind();
    buffers[Normals].allocate(normals.data(),
                              static_cast<GLsizei>(normals.size()*sizeof(float)));
    program->enableAttributeArray("normals");
    program->setAttributeBuffer("normals",GL_FLOAT,0,3);
    buffers[Normals].release();
    buffers[Colors].bind();
    buffers[Colors].allocate(colors.data(),
                             static_cast<GLsizei>(colors.size()*sizeof(float)));
    program->enableAttributeArray("colors");
    program->setAttributeBuffer("colors",GL_FLOAT,0,3);
    buffers[Colors].release();
    vaos[Facets]->release();
    program->release();

    program = getShaderProgram(PROGRAM_WITHOUT_LIGHT, viewer);
    program->bind();
    vaos[Edges]->bind();
    buffers[Edge_vertices].bind();
    buffers[Edge_vertices].allocate(edge_vertices.data(),
                                    static_cast<GLsizei>(edge_vertices.size()*sizeof(float)));
    program->enableAttributeArray("vertex");
    program->setAttributeBuffer("vertex",GL_FLOAT,0,3);
    buffers[Edge_vertices].release();
    vaos[Facets]->release();
    program->release();

    nb_pos = m_vertices.size();
    m_vertices.resize(0);
    //"Swap trick" insures that the memory is indeed freed and not kept available
    std::vector<float>(m_vertices).swap(m_vertices);
    nb_edge_pos = edge_vertices.size();
    edge_vertices.resize(0);
    std::vector<float>(edge_vertices).swap(edge_vertices);
    normals.resize(0);
    std::vector<float>(normals).swap(normals);
    colors.resize(0);
    std::vector<float>(colors).swap(colors);
    are_buffers_filled = true;
  }
};
class DistancePlugin :
    public QObject,
    public Polyhedron_demo_plugin_interface
{
  Q_OBJECT
  Q_INTERFACES(CGAL::Three::Polyhedron_demo_plugin_interface)
  Q_PLUGIN_METADATA(IID "com.geometryfactory.PolyhedronDemo.PluginInterface/1.0")

  typedef Kernel::Point_3 Point_3;
public:
  //decides if the plugin's actions will be displayed or not.
  bool applicable(QAction*) const
  {
    return scene->selectionIndices().size() == 2 &&
        qobject_cast<Scene_face_graph_item*>(scene->item(scene->selectionIndices().first())) &&
        qobject_cast<Scene_face_graph_item*>(scene->item(scene->selectionIndices().last()));
  }
  //the list of the actions of the plugin.
  QList<QAction*> actions() const
  {
    return _actions;
  }
  //this acts like a constructor for the plugin. It gets the references to the mainwindow and the scene, and connects the action.
  void init(QMainWindow* mw, Scene_interface* sc, Messages_interface* mi)
  {
    //gets the reference to the message interface, to display text in the console widget
    this->messageInterface = mi;
    //get the references
    this->scene = sc;
    this->mw = mw;
    //creates the action
    QAction *actionComputeDistance= new QAction(QString("Compute Distance Between Polyhedra"), mw);
    //specifies the subMenu
    actionComputeDistance->setProperty("subMenuName", "Polygon Mesh Processing");
    //links the action
    if(actionComputeDistance) {
      connect(actionComputeDistance, SIGNAL(triggered()),
              this, SLOT(createDistanceItems()));
      _actions << actionComputeDistance;
    }
  }
public Q_SLOTS:
  void createDistanceItems()
  {
    bool ok = false;
    nb_pts_per_face = QInputDialog::getInt(mw, tr("Sampling"),
                                               tr("Number of points per face:"),40, 1,2147483647,1, &ok);
    if (!ok)
      return;

    //check the initial conditions
    Scene_face_graph_item* itemA = qobject_cast<Scene_face_graph_item*>(scene->item(scene->selectionIndices().first()));
    Scene_face_graph_item* itemB = qobject_cast<Scene_face_graph_item*>(scene->item(scene->selectionIndices().last()));
    if(! CGAL::is_triangle_mesh(*itemA->polyhedron()) ||
       !CGAL::is_triangle_mesh(*itemB->polyhedron()) ){
      messageInterface->error(QString("Distance not computed. (Both polyhedra must be triangulated)"));
      return;
    }
    QApplication::setOverrideCursor(Qt::WaitCursor);
    Scene_distance_polyhedron_item* new_itemA = new Scene_distance_polyhedron_item(itemA->polyhedron(),itemB->polyhedron(), itemB->name(), nb_pts_per_face);
    Scene_distance_polyhedron_item* new_itemB = new Scene_distance_polyhedron_item(itemB->polyhedron(),itemA->polyhedron(), itemA->name(), nb_pts_per_face);
    itemA->setVisible(false);
    itemB->setVisible(false);
    new_itemA->setName(QString("%1 to %2").arg(itemA->name()).arg(itemB->name()));
    new_itemB->setName(QString("%1 to %2").arg(itemB->name()).arg(itemA->name()));
    scene->addItem(new_itemA);
    scene->addItem(new_itemB);
    QApplication::restoreOverrideCursor();
  }
private:
  int nb_pts_per_face;
  QList<QAction*> _actions;
  Messages_interface* messageInterface;
  //The reference to the scene
  Scene_interface* scene;
  //The reference to the main window
  QMainWindow* mw;
};
#include "Distance_plugin.moc"


//==================================================================================================
/*!
    @file

    @Copyright 2016 Numscale SAS
    @copyright 2016 J.T.Lapreste

    Distributed under the Boost Software License, Version 1.0.
    (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_ARCH_X86_SSE2_SIMD_FUNCTION_IS_EQZ_HPP_INCLUDED
#define BOOST_SIMD_ARCH_X86_SSE2_SIMD_FUNCTION_IS_EQZ_HPP_INCLUDED
#include <boost/simd/detail/overload.hpp>

#include <boost/simd/meta/as_logical.hpp>
#include <boost/simd/function/bitwise_and.hpp>
#include <boost/simd/function/bitwise_cast.hpp>
#include <boost/simd/function/shuffle.hpp>
#include <boost/dispatch/meta/downgrade.hpp>

namespace boost { namespace simd { namespace ext
{
  namespace bd =  boost::dispatch;
  namespace bs =  boost::simd;
  BOOST_DISPATCH_OVERLOAD ( is_eqz_
                          , (typename A0)
                          , bs::sse2_
                          , bs::pack_<bd::int64_<A0>, bs::sse_>
                         )
  {
    using result =  bs::as_logical_t<A0>;
    BOOST_FORCEINLINE result operator() ( const A0 & a0 ) const BOOST_NOEXCEPT
    {
      using base = bd::downgrade_t<A0>;

      const base tmp1 = boost::simd::bitwise_cast<base>(is_eqz(bs::bitwise_cast<base>(a0)));
      const base tmp2 = shuffle<1,0,3,2>(tmp1);
      return bs::bitwise_cast<result>(bitwise_and(tmp1, tmp2));
    }
  };

} } }

#endif


#include <iostream>
#include <algorithm>  // for find
#include <string>
#include <fstream>
#include <assert.h>

// for tokenizing strings
#include <boost/foreach.hpp>
#include <boost/tokenizer.hpp>

#include <boost/tuple/tuple_io.hpp>
#include <boost/tuple/tuple_comparison.hpp>
#include <math.h>

#define FLT_COST_MULTIPLIER   1e6

using namespace std;

#include "swarm_handler.h"

/********************************************************/
namespace YAML {
template<>
struct convert<QuadVoxel> {
  inline static Node encode(const QuadVoxel& voxel) {
    Node node;
    node.push_back(voxel.get<0>());
    node.push_back(voxel.get<1>());
    node.push_back(voxel.get<2>());
    return node;
  }

  inline static bool decode(const Node& node, QuadVoxel& voxel) {
    if(!node.IsSequence()) {
      return false;
    }

    boost::get<0>(voxel) = node[0].as<int>();
    boost::get<1>(voxel) = node[1].as<int>();
    boost::get<2>(voxel) = node[2].as<int>();
    return true;
  }
};
}

 QuadVoxel operator+ (const QuadVoxel &lhs, const QuadVoxel &rhs)
 {
    QuadVoxel result;
    result.get<X>() = lhs.get<X>() + rhs.get<X>();
    result.get<Y>() = lhs.get<Y>() + rhs.get<Y>();
    result.get<Z>() = lhs.get<Z>() + rhs.get<Z>();
    return result;

 }

/**************************** QuadStateCell **************************/

//function prototypes
 QuadStateCell::QuadStateCell(std::vector<int>::iterator first, std::vector<int>::iterator last)
 {
    m_data.assign(first, last);
    assert(m_data.size() == NUM_DOF+1);
 }

 QuadStateCell::QuadStateCell(int* first, int* last)
 {
    m_data.assign(first, last);
    assert(m_data.size() == NUM_DOF+1);
 }

 void QuadStateCell::print(ostream &output) const
 {
    output << "(" << m_data.at(0);
    //output << m_data.at(0);
        for (size_t dim = 1; dim < m_data.size(); ++dim)
            output << ", " << m_data.at(dim);
        output << ")";
}

bool QuadStateCell::operator== (const QuadStateCell &other) const
{
    assert(m_data.size() == NUM_DOF+1);
    assert(other.m_data.size() == NUM_DOF+1);
    for (size_t dim = 0; dim < NUM_DOF+1; ++dim)
        if (m_data.at(dim) != other.m_data.at(dim))
            return false;
    return true;
}

size_t QuadStateCell::Hash::operator()(const QuadStateCell &val) const
{
    size_t key = 0;
    for (size_t dim = 0; dim < NUM_DOF+1; ++dim) {
        size_t key_dim = (size_t) val.m_data.at(dim);
        key_dim ^= (key_dim << 4);
        key_dim += (key_dim >> 9);
        key ^= key_dim;
    }
    return key;
}

QuadVoxel QuadStateCell::operator+ (const QuadVoxel &other)
{
    QuadVoxel result(m_data.at(X) + other.get<X>(),     \
        m_data.at(Y) + other.get<Y>(),      \
        m_data.at(Z) + other.get<Z>());
    return result;
}

namespace YAML {
template<>
struct convert<QuadStateCell> {
    static bool decode (const YAML::Node& yaml_node, QuadStateCell &state_cell)
    {
        for (size_t dim = 0; dim < NUM_DOF+1; ++dim)
            state_cell.m_data[dim] = yaml_node[dim].as<int>();
        return true;
    }
};
}
/**************************** SwarmStateCell **************************/

//function prototypes
SwarmStateCell::SwarmStateCell(std::vector<int>::iterator first, std::vector<int>::iterator last)
{
    m_data.assign(first, last);
    assert(m_data.size() == NUM_DOF);
}

SwarmStateCell::SwarmStateCell(int* first, int* last)
{
    m_data.assign(first, last);
    assert(m_data.size() == NUM_DOF);
}

void SwarmStateCell::print(ostream &output) const
{
    output << "(" << m_data.at(0);
//output << m_data.at(0);
    for (size_t dim = 1; dim < m_data.size(); ++dim)
        output << ", " << m_data.at(dim);
    output << ")";
}

bool SwarmStateCell::operator== (const SwarmStateCell &other) const
{
    assert(m_data.size() == NUM_DOF);
    assert(other.m_data.size() == NUM_DOF);
    for (size_t dim = 0; dim < NUM_DOF; ++dim)
        if (m_data.at(dim) != other.m_data.at(dim))
            return false;
    return true;
}

size_t SwarmStateCell::Hash::operator()(const SwarmStateCell &val) const
{
    size_t key = 0;
    for (size_t dim = 0; dim < NUM_DOF; ++dim) {
        size_t key_dim = (size_t) val.m_data.at(dim);
        key_dim ^= (key_dim << 4);
        key_dim += (key_dim >> 9);
        key ^= key_dim;
    }
    return key;
}

QuadVoxel SwarmStateCell::operator+ (const QuadVoxel &other)
{
    QuadVoxel result(m_data.at(X) + other.get<X>(),     \
        m_data.at(Y) + other.get<Y>(),      \
        m_data.at(Z) + other.get<Z>());
    return result;
}

namespace YAML {
template<>
struct convert<SwarmStateCell> {
    static bool decode (const YAML::Node& yaml_node, SwarmStateCell &state_cell)
    {
        for (size_t dim = 0; dim < NUM_DOF+1; ++dim)
            state_cell.m_data[dim] = yaml_node[dim].as<int>();
        return true;
    }
};
}

/**************************** SwarmAction **************************/

std::ostream& operator<< (std::ostream &output, const SwarmAction &action)
    {
        output << "#" << action.index << ": " << action.q_f << ", " << action.cost << ", [";
            BOOST_FOREACH(const QuadVoxel &voxel, action.swath) {
                cout << voxel << " ";
            }
            cout << "]";
return output;
}

std::ostream& operator<< (std::ostream &output, const SwarmAction::QuadParam &coeff)
{
    output << "[[" << coeff.x_coeffs.at(0);
        for (size_t dim = 1; dim < coeff.x_coeffs.size(); ++dim)
            output << ", " << coeff.x_coeffs.at(dim);
        output << "], [" << coeff.y_coeffs.at(0);
        for (size_t dim = 1; dim < coeff.y_coeffs.size(); ++dim)
            output << ", " << coeff.y_coeffs.at(dim);
        output << "], [" << coeff.z_coeffs.at(0);
        for (size_t dim = 1; dim < coeff.y_coeffs.size(); ++dim)
            output << ", " << coeff.z_coeffs.at(dim);
        output << "], " << coeff.time << "]";
output << endl;
return output;
}

namespace YAML {
template<>
struct convert<SwarmAction::QuadParam> {
    static bool decode (const YAML::Node& yaml_node, SwarmAction::QuadParam &coeff)
    {
        const YAML::Node& node_x = yaml_node[0];
        for (size_t dim = 0; dim < node_x.size(); ++dim) {
            coeff.x_coeffs.push_back(node_x[dim].as<float>());
        }
        const YAML::Node& node_y = yaml_node[1];
        for (size_t dim = 0; dim < node_y.size(); ++dim) {
            coeff.y_coeffs.push_back(node_y[dim].as<float>());
        }
        const YAML::Node& node_z = yaml_node[2];
        for (size_t dim = 0; dim < node_z.size(); ++dim) {
            coeff.z_coeffs.push_back(node_z[dim].as<float>());
        }
        coeff.time = yaml_node[3].as<float>();
        return yaml_node;
    }
};

template<>
struct convert<SwarmAction> {
    static bool decode (const Node& yaml_node, SwarmAction &action)
    {
        action.cost = (uint) (yaml_node["cost"].as<float>() * FLT_COST_MULTIPLIER); // convert float to integer

        // set q_f
        const YAML::Node &node_qf(yaml_node["q_f"]);
        for (size_t dim = 0; dim < node_qf.size(); ++dim) {
            action.q_f.get(dim) = node_qf[dim].as<int>();
        }

        // set params (coeffs and assignment)
        const YAML::Node &node_param(yaml_node["params"]);
        for (size_t dim = 0; dim < node_param.size()-1; ++dim) {
            action.coeffs.push_back(node_param[dim].as<SwarmAction::QuadParam>());
        }
        const YAML::Node &node_assignment = node_param[node_param.size()-1];
        for (size_t dim = 0; dim < node_assignment.size(); ++dim) {
            action.assignment.push_back(node_assignment[dim].as<int>());
        }

        // set swath
        const YAML::Node &node_swath(yaml_node["swath"]);
        // for each voxel in the swath
        for (size_t i=0; i < node_swath.size(); ++i) {
            QuadVoxel cur_voxel;
            boost::get<0>(cur_voxel) = node_swath[i][0].as<int>();
            boost::get<1>(cur_voxel) = node_swath[i][1].as<int>();
            boost::get<2>(cur_voxel) = node_swath[i][2].as<int>();
            action.swath.push_back(cur_voxel);
        } // end for each voxel in the swath

        return true;
    }
};
}

std::vector<SwarmAction::QuadParam> SwarmAction::getCoeffs(){
    return coeffs;
}

std::vector<int> SwarmAction::getAssignment(){
    return assignment;
}

std::vector<QuadVoxel> SwarmAction::getSwath(){
    return swath;
}

bool SwarmHandler::ReadMotionTemplate(const std::string &template_file, MotionTemplate &motion_template)
{
    YAML::Node doc = YAML::LoadFile(template_file.c_str());
    string buf;
    size_t pos_dim;
    pos_dim = doc["position_dim"].as<size_t>();

    // get extents of non-position vars from the template:
    const YAML::Node &sss(doc["sss"]);
    if (sss.size() != NUM_DOF) {
        printf("ERROR: this template is not compatible with current configuration: its num DOF %zu != %d\n", sss.size(), NUM_DOF);
        return false;
    }

    const YAML::Node &prim_set(doc["prim"]);
    // iterate over initial states, (q_i's):
    for (size_t j = 0; j < prim_set.size(); ++j) {
        // buf now contains the initial state for the prims that follow, in the format: N, N, ...
        boost::char_separator<char> sep(", ");
        boost::tokenizer< boost::char_separator<char> > tokens(prim_set[j][0].as<string>(), sep);

        size_t dim = pos_dim;

        SwarmStateCell q_i;
        BOOST_FOREACH (const string& tok, tokens) {
            assert(dim < NUM_DOF);
            q_i.get(dim) = atoi(tok.c_str());
            ++dim;
        }
        q_i.print(cout);
        // iterate over primitives emanating from this q_i
        size_t prim_idx = 0; // reset primitive index for each q_i
        const YAML::Node &prim_list(prim_set[j][1]);
        for (size_t i=0; i < prim_list.size(); ++i) {
            const YAML::Node &cur_prim(prim_list[i]);
            SwarmAction prim;
            prim.index = prim_idx++; // set index
            prim = cur_prim.as<SwarmAction>();
            int final_state = (int)(2 * prim.q_f.get(3) + prim.q_f.get(4));
            if (motion_template[q_i].size() <= final_state){
                motion_template[q_i].resize(final_state+1);
            }
            motion_template[q_i][final_state].push_back(prim);
        } // end iterate over primitives emanating from this q_i
    } // end iterate over initial states

    return true;
}


#include <iostream>
#include <string>
#include <vector>
#include <queue>
#include <tuple>
#include <array>
#include <map>
#include <boost/algorithm/string.hpp>
#include <boost/unordered_set.hpp>

using namespace std;

typedef vector<char> TableRow;
typedef vector<TableRow> Table;

struct Board {
  Table sData, dData;
  int px, py;

  Board(string b) {
    vector<string> data;
    boost::split(data, b, boost::is_any_of("\n"));

    size_t width = 0;
    for (auto &row: data)
      width = max(width, row.size());

    map<char,char> maps = {{' ',' '}, {'.','.'}, {'@',' '},
                           {'#','#'}, {'$',' '}},
                   mapd = {{' ',' '}, {'.',' '}, {'@','@'},
                           {'#',' '}, {'$','*'}};

    for (size_t r = 0; r < data.size(); r++) {
      TableRow sTemp, dTemp;
      for (size_t c = 0; c < width; c++) {
        char ch = c < data[r].size() ? data[r][c] : ' ';
        sTemp.push_back(maps[ch]);
        dTemp.push_back(mapd[ch]);
        if (ch == '@') {
          px = c;
          py = r;
        }
      }
      sData.push_back(sTemp);
      dData.push_back(dTemp);
    }
  }

  bool move(int x, int y, int dx, int dy, Table &data) {
    if (sData[y+dy][x+dx] == '#' || data[y+dy][x+dx] != ' ')
      return false;

    data[y][x] = ' ';
    data[y+dy][x+dx] = '@';
    return true;
  }

  bool push(int x, int y, int dx, int dy, Table &data) {
    if (sData[y+2*dy][x+2*dx] == '#' || data[y+2*dy][x+2*dx] != ' ')
      return false;

    data[y][x] = ' ';
    data[y+dy][x+dx] = '@';
    data[y+2*dy][x+2*dx] = '*';
    return true;
  }

  bool isSolved(const Table &data) {
    for (size_t r = 0; r < data.size(); r++)
      for (size_t c = 0; c < data[r].size(); c++)
        if ((sData[r][c] == '.') != (data[r][c] == '*'))
          return false;
    return true;
  }

  string solve() {
    boost::unordered_set<Table, boost::hash<Table>> visited;
    visited.insert(dData);

    queue<tuple<Table, string, int, int>> open;
    open.push(make_tuple(dData, "", px, py));

    vector<tuple<int, int, char, char>> dirs = {
        make_tuple( 0, -1, 'u', 'U'),
        make_tuple( 1,  0, 'r', 'R'),
        make_tuple( 0,  1, 'd', 'D'),
        make_tuple(-1,  0, 'l', 'L')
    };

    while (open.size() > 0) {
      Table temp, cur = get<0>(open.front());
      string cSol = get<1>(open.front());
      int x = get<2>(open.front());
      int y = get<3>(open.front());
      open.pop();

      for (int i = 0; i < 4; ++i) {
        temp = cur;
        int dx = get<0>(dirs[i]);
        int dy = get<1>(dirs[i]);

        if (temp[y+dy][x+dx] == '*') {
          if (push(x, y, dx, dy, temp) &&
              visited.find(temp) == visited.end()) {
            if (isSolved(temp))
              return cSol + get<3>(dirs[i]);
            open.push(make_tuple(temp, cSol + get<3>(dirs[i]),
                                 x+dx, y+dy));
            visited.insert(temp);
          }
        } else if (move(x, y, dx, dy, temp) &&
                   visited.find(temp) == visited.end()) {
          if (isSolved(temp))
            return cSol + get<2>(dirs[i]);
          open.push(make_tuple(temp, cSol + get<2>(dirs[i]),
                               x+dx, y+dy));
          visited.insert(temp);
        }
      }
    }

    return "No solution";
  }
};

int main() {
  string level = "#######\n"
                 "#     #\n"
                 "#     #\n"
                 "#. #  #\n"
                 "#. $$ #\n"
                 "#.$$  #\n"
                 "#.#  @#\n"
                 "#######";

  cout << level << endl << endl;
  Board board(level);
  cout << board.solve() << endl;
  return 0;
}