//------------------------------------------------------------------------------
// Copyright (c) 2018 Hsienchi Kuo (Allen Institute, Hanchuan Peng's team)
// All rights reserved.
//------------------------------------------------------------------------------

/*******************************************************************************
*
*  This library provides functionalities for neuron struct operations, such as crop, register, sample, data extraction, etc.
*  NeuronStructUtil class is desinged to take NeuronTree struct as the input and as well output NeuronTree struct for most of the methods.
*  This class intends to operate on the whole neuron struct level, as 'utility' it is called.
*
*  Many NeuronStructUtil class methods are implemented as static functions. The input NeuronTree is always set to be const so that it will not be modified.
*  A typical function call would need at least three input arguments:
*
*		NeuronStructUtil::func(const NeuronTree& inputTree, NeuronTree& outputTree, other input arguments);
*
********************************************************************************/

#include <iostream>
#include <string>
#include <iterator>
#include <set>
#include <cmath>

#include <boost/filesystem.hpp>

#include "basic_4dimage.h"
#include "NeuronStructUtilities.h"
#include "ImgProcessor.h"

using namespace boost;

NeuronTree NeuronStructUtil::swcRegister(NeuronTree& inputTree, const NeuronTree& refTree)
{
	double xShift, yShift, zShift;
	double xScale, yScale, zScale;

	double xmin = 10000, ymin = 10000, zmin = 10000;
	double xmax = 0, ymax = 0, zmax = 0;
	for (QList<NeuronSWC>::iterator it = inputTree.listNeuron.begin(); it != inputTree.listNeuron.end(); ++it)
	{
		if (it->x < xmin) xmin = it->x;
		if (it->x > xmax) xmax = it->x;
		if (it->y < ymin) ymin = it->y;
		if (it->y > ymax) ymax = it->y;
		if (it->z < zmin) zmin = it->z;
		if (it->z > zmax) zmax = it->z;
	}
	double refXmin = 10000, refYmin = 10000, refZmin = 10000;
	double refXmax = 0, refYmax = 0, refZmax = 0;
	for (QList<NeuronSWC>::const_iterator refIt = refTree.listNeuron.begin(); refIt != refTree.listNeuron.end(); ++refIt)
	{
		if (refIt->x < refXmin) refXmin = refIt->x;
		if (refIt->x > refXmax) refXmax = refIt->x;
		if (refIt->y < refYmin) refYmin = refIt->y;
		if (refIt->y > refYmax) refYmax = refIt->y;
		if (refIt->z < refZmin) refZmin = refIt->z;
		if (refIt->z > refZmax) refZmax = refIt->z;
	}

	xScale = (refXmax - refXmin) / (xmax - xmin);
	yScale = (refYmax - refYmin) / (ymax - ymin);
	zScale = (refZmax - refZmin) / (zmax - zmin);
	xShift = refXmin - xmin * xScale;
	yShift = refYmin - ymin * yScale;
	zShift = refZmin - zmin * zScale;

	NeuronTree outputTree;
	for (int i = 0; i < inputTree.listNeuron.size(); ++i)
	{
		NeuronSWC newNode = inputTree.listNeuron.at(i);
		newNode.x = newNode.x * xScale + xShift;
		newNode.y = newNode.y * yScale + yShift;
		newNode.z = newNode.z * zScale + zShift;
		outputTree.listNeuron.push_back(newNode);
	}

	return outputTree;
}

void NeuronStructUtil::swcSlicer(const NeuronTree& inputTree, vector<NeuronTree>& outputTrees, int thickness)
{
	// -- Dissemble SWC files into "slices." Each outputSWC file represents only 1 z slice.
	// thickness * 2 + 1 = the number of consecutive z slices for one SWC node to appear. This is for the purpose producing continous masks.

	QList<NeuronSWC> inputList = inputTree.listNeuron;
	int zMax = 0;
	ptrdiff_t thicknessPtrDiff = ptrdiff_t(thickness); // Determining how many z sections to be included in 1 single slice.
	for (QList<NeuronSWC>::iterator it = inputList.begin(); it != inputList.end(); ++it)
	{
		it->z = round(it->z);
		if (it->z >= zMax) zMax = it->z;
	}

	QList<NeuronTree> slicedTrees; // Determining number of sliced trees in the list.
	for (int i = 0; i < zMax; ++i)
	{
		NeuronTree nt;
		slicedTrees.push_back(nt);
	}

	for (QList<NeuronSWC>::iterator it = inputList.begin(); it != inputList.end(); ++it)
	{
		NeuronSWC currNode = *it;
		ptrdiff_t sliceNo = ptrdiff_t(it->z);
		(slicedTrees.begin() + sliceNo - 1)->listNeuron.push_back(currNode); // SWC starts with 1.
		float currZ = currNode.z;

		// -- Project +/- thickness slices onto the same plane, making sure the tube can be connected accross planes. -- //
		vector<ptrdiff_t> sectionNums;
		for (ptrdiff_t ptri = 1; ptri <= thicknessPtrDiff; ++ptri)
		{
			ptrdiff_t minusDiff = sliceNo - ptri;
			ptrdiff_t plusDiff = sliceNo + ptri;

			if (minusDiff < 0) continue;
			else sectionNums.push_back(minusDiff);

			if (plusDiff > ptrdiff_t(zMax)) continue;
			else sectionNums.push_back(plusDiff);
		}
		for (vector<ptrdiff_t>::iterator ptrIt = sectionNums.begin(); ptrIt != sectionNums.end(); ++ptrIt)
		{
			//cout << "current node z:" << currNode.z << " " << *ptrIt << "| ";
			NeuronSWC newNode = currNode;
			newNode.z = float(*ptrIt);
			(slicedTrees.begin() + *ptrIt - 1)->listNeuron.push_back(newNode);
		}
		//cout << endl;

		sectionNums.clear();
		// ------------------------------------------------------------------------------------------------------------- //
	}

	for (QList<NeuronTree>::iterator it = slicedTrees.begin(); it != slicedTrees.end(); ++it)
		outputTrees.push_back(*it);
}

map<int, QList<NeuronSWC>> NeuronStructUtil::swcSplitByType(const NeuronTree& inputTree)
{
	map<int, QList<NeuronSWC>> outputNodeTypeMap;
	map<int, boost::container::flat_set<int>> nodeIDsetMap;
	for (QList<NeuronSWC>::const_iterator it = inputTree.listNeuron.begin(); it != inputTree.listNeuron.end(); ++it)
	{
		outputNodeTypeMap[it->type].append(*it);
		nodeIDsetMap[it->type].insert(it->n);
	}
	for (map<int, QList<NeuronSWC>>::iterator mapIt = outputNodeTypeMap.begin(); mapIt != outputNodeTypeMap.end(); ++mapIt)
	{
		for (QList<NeuronSWC>::iterator nodeIt = mapIt->second.begin(); nodeIt != mapIt->second.end(); ++nodeIt)
		{
			if (nodeIt->parent == -1) continue;
			else if (nodeIDsetMap.at(nodeIt->type).find(nodeIt->parent) == nodeIDsetMap.at(nodeIt->type).end()) nodeIt->parent = -1;
		}
	}

	return outputNodeTypeMap;
}

NeuronTree NeuronStructUtil::swcSubtraction(const NeuronTree& targetTree, const NeuronTree& refTree, int type)
{
	boost::container::flat_map<string, QList<NeuronSWC>> targetNodeTileMap;
	boost::container::flat_map<string, QList<NeuronSWC>> refNodeTileMap;
	NeuronStructUtil::nodeTileMapGen(targetTree, targetNodeTileMap);
	NeuronStructUtil::nodeTileMapGen(refTree, refNodeTileMap);

	if (type == 0)
	{
		vector<ptrdiff_t> delLocs;
		for (boost::container::flat_map<string, QList<NeuronSWC>>::iterator targetTileIt = targetNodeTileMap.begin(); targetTileIt != targetNodeTileMap.end(); ++targetTileIt)
		{
			if (refNodeTileMap.find(targetTileIt->first) != refNodeTileMap.end())
			{
				for (QList<NeuronSWC>::iterator checkIt1 = targetTileIt->second.begin(); checkIt1 != targetTileIt->second.end(); ++checkIt1)
				{
					for (QList<NeuronSWC>::iterator checkIt2 = refNodeTileMap.at(targetTileIt->first).begin(); checkIt2 != refNodeTileMap.at(targetTileIt->first).end(); ++checkIt2)
					{
						if (checkIt1->x == checkIt2->x && checkIt1->y == checkIt2->y && checkIt1->z == checkIt2->z)
							delLocs.push_back(ptrdiff_t(checkIt1 - targetTileIt->second.begin()));
					}
				}
			}
			else continue;

			sort(delLocs.rbegin(), delLocs.rend());
			for (vector<ptrdiff_t>::iterator delIt = delLocs.begin(); delIt != delLocs.end(); ++delIt) targetTileIt->second.erase(targetTileIt->second.begin() + *delIt);
			delLocs.clear();
		}
	}
	else
	{
		vector<ptrdiff_t> delLocs;
		for (boost::container::flat_map<string, QList<NeuronSWC>>::iterator targetTileIt = targetNodeTileMap.begin(); targetTileIt != targetNodeTileMap.end(); ++targetTileIt)
		{
			if (refNodeTileMap.find(targetTileIt->first) != refNodeTileMap.end())
			{
				for (QList<NeuronSWC>::iterator checkIt1 = targetTileIt->second.begin(); checkIt1 != targetTileIt->second.end(); ++checkIt1)
				{
					for (QList<NeuronSWC>::iterator checkIt2 = refNodeTileMap.at(targetTileIt->first).begin(); checkIt2 != refNodeTileMap.at(targetTileIt->first).end(); ++checkIt2)
					{
						if (checkIt1->x == checkIt2->x && checkIt1->y == checkIt2->y && checkIt1->z == checkIt2->z && checkIt2->type == type)
							delLocs.push_back(ptrdiff_t(checkIt1 - targetTileIt->second.begin()));
					}
				}
			}
			else continue;

			sort(delLocs.rbegin(), delLocs.rend());
			for (vector<ptrdiff_t>::iterator delIt = delLocs.begin(); delIt != delLocs.end(); ++delIt) targetTileIt->second.erase(targetTileIt->second.begin() + *delIt);
			delLocs.clear();
		}
	}

	NeuronTree outputTree;
	for (boost::container::flat_map<string, QList<NeuronSWC>>::iterator mapIt = targetNodeTileMap.begin(); mapIt != targetNodeTileMap.end(); ++mapIt)
		outputTree.listNeuron.append(mapIt->second);

	return outputTree;
}

// ========================================== SWC Profiling Methods =========================================
QList<NeuronSWC> NeuronStructUtil::removeRednNode(const NeuronTree& inputTree)
{
	// -- This method removes dupliated nodes.

	boost::container::flat_map<string, QList<NeuronSWC>> nodeTileMap;
	NeuronStructUtil::nodeTileMapGen(inputTree, nodeTileMap);
	cout << "SWC tile number: " << nodeTileMap.size() << endl;

	QList<NeuronSWC> outputNodeList;
	cout << "processing tile: ";
	int tileCount = 0;
	for (boost::container::flat_map<string, QList<NeuronSWC>>::iterator tileIt = nodeTileMap.begin(); tileIt != nodeTileMap.end(); ++tileIt)
	{
		++tileCount;
		cout << tileCount << " ";
		bool redun = true;
		while (redun)
		{
			for (QList<NeuronSWC>::iterator it1 = tileIt->second.begin(); it1 != tileIt->second.end(); ++it1)
			{
				for (QList<NeuronSWC>::iterator it2 = tileIt->second.begin(); it2 != tileIt->second.end(); ++it2)
				{
					if (it2 == it1) continue;

					if (it2->x == it1->x && it2->y == it1->y && it2->z == it1->z)
					{
						if (it2->parent == -1)
						{
							tileIt->second.erase(it2);
							goto NODE_DELETED;
						}
					}
				}
			}
			redun = false;

		NODE_DELETED:
			continue;
		}

		outputNodeList.append(tileIt->second);
	}
	cout << endl;

	return outputNodeList;
}

NeuronTree NeuronStructUtil::swcZclenUP(const NeuronTree& inputTree, float zThre)
{
	// -- This method removes signal nodes from different depths that share the same x-y coordinates due to 2D connected component procedure for 3D structure.
	//      zThre: depth window allowance.
	//      Nodes that share the same x-y coordinates but belong to different zThre groups will still be considered different nodes.

	map<string, vector<NeuronSWC>> xyLabeledNodeMap;
	for (QList<NeuronSWC>::const_iterator it = inputTree.listNeuron.begin(); it != inputTree.listNeuron.end(); ++it)
	{
		string xLabel = to_string(int(it->x / 2));
		string yLabel = to_string(int(it->y / 2));
		string labelKey = xLabel + "_" + yLabel;
		if (xyLabeledNodeMap.find(labelKey) != xyLabeledNodeMap.end()) xyLabeledNodeMap[labelKey].push_back(*it);
		else
		{
			vector<NeuronSWC> newSet;
			newSet.push_back(*it);
			xyLabeledNodeMap.insert(pair<string, vector<NeuronSWC>>(labelKey, newSet));
		}
	}

	NeuronTree outputTree;
	for (map<string, vector<NeuronSWC>>::iterator it = xyLabeledNodeMap.begin(); it != xyLabeledNodeMap.end(); ++it)
	{
		boost::container::flat_map<float, NeuronSWC> zSortedNodes; // boost::container::flat_map is vector based, so it can do pointer arithmetic.
		if (it->second.size() == 1)
		{
			outputTree.listNeuron.push_back(*(it->second.begin()));
			continue;
		}
		else
		{
			for (vector<NeuronSWC>::iterator nodeIt = it->second.begin(); nodeIt != it->second.end(); ++nodeIt)
				zSortedNodes.insert(pair<float, NeuronSWC>(nodeIt->z, *nodeIt));

			boost::container::flat_map<float, NeuronSWC>::iterator startIt = zSortedNodes.begin();
			for (boost::container::flat_map<float, NeuronSWC>::iterator zIt = zSortedNodes.begin() + 1; zIt != zSortedNodes.end(); ++zIt)
			{
				if (zIt->second.z - (zIt - 1)->second.z > zThre)
				{
					ptrdiff_t steps = zIt - startIt;
					ptrdiff_t selectedLoc = ptrdiff_t(int(steps) / 2);
					outputTree.listNeuron.push_back((startIt + selectedLoc)->second);
					startIt = zIt;
				}
			}
			if (startIt == zSortedNodes.begin())
			{
				ptrdiff_t selectedLoc = ptrdiff_t(int((zSortedNodes.end() - zSortedNodes.begin()) / 2));
				outputTree.listNeuron.push_back((startIt + selectedLoc)->second);
			}
		}
	}

	return outputTree;
}

map<string, float> NeuronStructUtil::selfNodeDist(const QList<NeuronSWC>& inputNodeList)
{
	// -- This method computes the distance of every node to its nearest node in the given swc.
	// -- The output is a map that contains 4 measures: mean, std, var, median.

	boost::container::flat_map<string, vector<NeuronSWC>> labeledNodeMap;
	NeuronStructUtil::nodeTileMapGen(inputNodeList, labeledNodeMap, 30);

	float distSum = 0;
	vector<float> distVec;
	for (boost::container::flat_map<string, vector<NeuronSWC>>::iterator it = labeledNodeMap.begin(); it != labeledNodeMap.end(); ++it)
	{
		if (it->second.size() == 1)
		{
			float dist = 10000;
			for (QList<NeuronSWC>::const_iterator nodeIt = inputNodeList.begin(); nodeIt != inputNodeList.end(); ++nodeIt)
			{
				if (it->second.begin()->x == nodeIt->x && it->second.begin()->y == nodeIt->y && it->second.begin()->z == nodeIt->z)
					continue;

				float tempDist = sqrtf((it->second.begin()->x - nodeIt->x) * (it->second.begin()->x - nodeIt->x) +
					(it->second.begin()->y - nodeIt->y) * (it->second.begin()->y - nodeIt->y) +
					(it->second.begin()->z - nodeIt->z) * (it->second.begin()->z - nodeIt->z) * zRATIO * zRATIO);
				if (tempDist < dist) dist = tempDist;
			}
			distVec.push_back(dist);
			distSum = distSum + dist;
			continue;
		}

		for (vector<NeuronSWC>::iterator nodeIt1 = it->second.begin(); nodeIt1 != it->second.end(); ++nodeIt1)
		{
			float dist = 10000;
			for (vector<NeuronSWC>::iterator nodeIt2 = it->second.begin(); nodeIt2 != it->second.end(); ++nodeIt2)
			{
				if (nodeIt1 == nodeIt2) continue;

				float tempDist = sqrtf((nodeIt1->x - nodeIt2->x) * (nodeIt1->x - nodeIt2->x) +
					(nodeIt1->y - nodeIt2->y) * (nodeIt1->y - nodeIt2->y) +
					(nodeIt1->z - nodeIt2->z) * (nodeIt1->z - nodeIt2->z) * zRATIO * zRATIO);
				if (tempDist < dist) dist = tempDist;
			}
			distVec.push_back(dist);
			distSum = distSum + dist;
		}
	}

	float distMean = distSum / float(distVec.size());
	float distVarSum = 0;
	for (vector<float>::iterator it = distVec.begin(); it != distVec.end(); ++it) distVarSum = distVarSum + (*it - distMean) * (*it - distMean);
	float distVar = distVarSum / float(distVec.size());
	float distStd = sqrtf(distVar);
	sort(distVec.begin(), distVec.end());
	float distMedian = float(distVec.at(floor(distVec.size() / 2)));

	map<string, float> outputMap;
	outputMap.insert(pair<string, float>("mean", distMean));
	outputMap.insert(pair<string, float>("std", distStd));
	outputMap.insert(pair<string, float>("var", distVar));
	outputMap.insert(pair<string, float>("median", distMedian));

	return outputMap;
}

NeuronTree NeuronStructUtil::swcIdentityCompare(const NeuronTree& subjectTree, const NeuronTree& refTree, float distThre, float nodeTileLength)
{
	// -- This method identifies positive signal nodes from noise with given reference NeuronTree.
	// -- radius: tile length
	// -- distThre: distance threshold for nearest node pair

	map<string, vector<NeuronSWC>> gridSWCmap; // Better use vector instead of set here, as set by default sorts the elements.
	// This can cause complication if the element is a data struct.

	NeuronStructUtil::nodeTileMapGen(refTree, gridSWCmap, nodeTileLength);

	NeuronTree outputTree;
	NeuronTree refConfinedFilteredTree;
	for (QList<NeuronSWC>::const_iterator suIt = subjectTree.listNeuron.begin(); suIt != subjectTree.listNeuron.end(); ++suIt)
	{
		string xLabel = to_string(int((suIt->x) / (nodeTileLength)));
		string yLabel = to_string(int((suIt->y) / (nodeTileLength)));
		string zLabel = to_string(int((suIt->z) / (nodeTileLength / zRATIO)));
		string keyLabel = xLabel + "_" + yLabel + "_" + zLabel;

		if (gridSWCmap.find(keyLabel) != gridSWCmap.end())
		{
			bool identified = false;
			for (vector<NeuronSWC>::iterator nodeIt = gridSWCmap[keyLabel].begin(); nodeIt != gridSWCmap[keyLabel].end(); ++nodeIt)
			{
				float dist = sqrt((nodeIt->x - suIt->x) * (nodeIt->x - suIt->x) + (nodeIt->y - suIt->y) * (nodeIt->y - suIt->y) +
					zRATIO * zRATIO * (nodeIt->z - suIt->z) * (nodeIt->z - suIt->z));

				if (dist <= distThre)
				{
					outputTree.listNeuron.push_back(*suIt);
					(outputTree.listNeuron.end() - 1)->type = 2;
					identified = true;
					break;
				}
			}

			if (!identified)
			{
				outputTree.listNeuron.push_back(*suIt);
				(outputTree.listNeuron.end() - 1)->type = 3;
			}
		}
		else refConfinedFilteredTree.listNeuron.push_back(*suIt);
	}

	return outputTree;
}
// ===================================== END of [SWC Profiling Methods] =====================================

// =================================== SWC <-> Connected Components ====================================
vector<connectedComponent> NeuronStructUtil::swc2signal2DBlobs(const NeuronTree& inputTree)
{
	// -- Finds signal blobs "slice by slice" from input NeuronTree. Each slice is independent to one another.
	// -- Therefore, the same real blobs in 3D are consists of certain amount of 2D "blob slices" produced by this method.
	// -- Each 2D blob slice accounts for 1 ImgAnalyzer::connectedComponent.

	vector<NeuronSWC> allNodes;
	for (QList<NeuronSWC>::const_iterator it = inputTree.listNeuron.begin(); it != inputTree.listNeuron.end(); ++it) allNodes.push_back(*it);

	vector<connectedComponent> connComps2D;
	int islandCount = 0;
	cout << "number of SWC nodes processed: ";
	for (vector<NeuronSWC>::iterator nodeIt = allNodes.begin(); nodeIt != allNodes.end(); ++nodeIt)
	{
		if (int(nodeIt - allNodes.begin()) % 10000 == 0) cout << int(nodeIt - allNodes.begin()) << " ";
		for (vector<connectedComponent>::iterator connIt = connComps2D.begin(); connIt != connComps2D.end(); ++connIt)
		{
			if (connIt->coordSets.empty()) continue;
			else if (int(nodeIt->z) == connIt->coordSets.begin()->first)
			{
				for (set<vector<int>>::iterator dotIt = connIt->coordSets[int(nodeIt->z)].begin(); dotIt != connIt->coordSets[int(nodeIt->z)].end(); ++dotIt)
				{
					if (int(nodeIt->x) <= dotIt->at(0) + 1 && int(nodeIt->x) >= dotIt->at(0) - 1 &&
						int(nodeIt->y) <= dotIt->at(1) + 1 && int(nodeIt->y) >= dotIt->at(1) - 1)
					{
						vector<int> newCoord(3);
						newCoord[0] = int(nodeIt->x);
						newCoord[1] = int(nodeIt->y);
						newCoord[2] = int(nodeIt->z);
						connIt->coordSets[newCoord[2]].insert(newCoord);
						connIt->size = connIt->size + 1;

						if (newCoord[0] < connIt->xMin) connIt->xMin = newCoord[0];
						else if (newCoord[0] > connIt->xMax) connIt->xMax = newCoord[0];

						if (newCoord[1] < connIt->yMin) connIt->yMin = newCoord[1];
						else if (newCoord[1] > connIt->yMax) connIt->yMax = newCoord[1];

						goto NODE_INSERTED;
					}
				}
			}
		}

		{
			++islandCount;
			connectedComponent newIsland;
			newIsland.islandNum = islandCount;
			vector<int> newCoord(3);
			newCoord[0] = int(nodeIt->x);
			newCoord[1] = int(nodeIt->y);
			newCoord[2] = int(nodeIt->z);
			set<vector<int>> coordSet;
			coordSet.insert(newCoord);
			newIsland.coordSets.insert(pair<int, set<vector<int>>>(newCoord[2], coordSet));
			newIsland.xMax = newCoord[0];
			newIsland.xMin = newCoord[0];
			newIsland.yMax = newCoord[1];
			newIsland.yMin = newCoord[1];
			newIsland.zMin = newCoord[2];
			newIsland.zMax = newCoord[2];
			newIsland.size = 1;
			connComps2D.push_back(newIsland);
		}

	NODE_INSERTED:
		continue;
	}
	cout << endl << endl;

	vector<float> center(3);
	for (vector<connectedComponent>::iterator it = connComps2D.begin(); it != connComps2D.end(); ++it)
		ImgAnalyzer::ChebyshevCenter_connComp(*it);

	return connComps2D;
}

vector<connectedComponent> NeuronStructUtil::swc2signal3DBlobs(const NeuronTree& inputTree)
{
	// -- This method is a wrapper of NeuronStructUtil::swc2signal2DBlobs and NeuronStructUtil::merge2DConnComponent.
	// -- It produces 3D signal blobs by calling the two swc2signal2DBlobs and merge2DConnComponent sequentially.

	vector<connectedComponent> inputConnCompList = this->swc2signal2DBlobs(inputTree);
	vector<connectedComponent> outputConnCompList = this->merge2DConnComponent(inputConnCompList);

	return outputConnCompList;
}

vector<connectedComponent> NeuronStructUtil::merge2DConnComponent(const vector<connectedComponent>& inputConnCompList)
{
	// -- This method finds 3D signal blobs by grouping 2D signal blobs together, which are produced by NeuronStructUtil::swc2signal2DBlobs.
	// -- The approach is consists of 2 stages:
	//		1. Identifying the same 3D blobs slice by slice.
	//		2. Merging 3D blobs that contain the same 2D blobs.

	cout << "Merging 2D signal blobs.." << endl;
	cout << "-- processing slice ";

	vector<connectedComponent> outputConnCompList;

	int zMax = 0;

	// -- I notice that boost's container templates are able to lift up the performace by ~30%.
	boost::container::flat_map<int, boost::container::flat_set<int>> b2Dtob3Dmap;
	b2Dtob3Dmap.clear();
	boost::container::flat_map<int, boost::container::flat_set<int>> b3Dcomps;
	b3Dcomps.clear();
	// ---------------------------------------------------------------------------------------

	// --------- First slice, container initiation --------------
	int sliceBlobCount = 0;
	for (vector<connectedComponent>::const_iterator it = inputConnCompList.begin(); it != inputConnCompList.end(); ++it)
	{
		if (it->coordSets.begin()->first > zMax) zMax = it->coordSets.begin()->first;

		if (it->coordSets.begin()->first == 0)
		{
			++sliceBlobCount;
			boost::container::flat_set<int> blob3D;
			blob3D.insert(sliceBlobCount);
			b2Dtob3Dmap.insert(pair<int, boost::container::flat_set<int>>(it->islandNum, blob3D));
			boost::container::flat_set<int> comps;
			comps.insert(it->islandNum);
			b3Dcomps[sliceBlobCount] = comps;
		}
	}
	// -----------------------------------------------------------

	// ------------------------------------------- Merge 2D blobs from 2 adjacent slices -------------------------------------------
	vector<connectedComponent> currSliceConnComps;
	vector<connectedComponent> preSliceConnComps;
	size_t increasedSize;
	for (int i = 1; i <= zMax; ++i)
	{
		currSliceConnComps.clear();
		preSliceConnComps.clear();

		increasedSize = 0;
		for (vector<connectedComponent>::const_iterator it = inputConnCompList.begin(); it != inputConnCompList.end(); ++it)
			if (it->coordSets.begin()->first == i) currSliceConnComps.push_back(*it);
		if (currSliceConnComps.empty())
		{
			cout << i << "->0 ";
			continue;
		}

		cout << i << "->";
		for (vector<connectedComponent>::const_iterator it = inputConnCompList.begin(); it != inputConnCompList.end(); ++it)
			if (it->coordSets.begin()->first == i - 1) preSliceConnComps.push_back(*it);
		if (preSliceConnComps.empty())
		{
			for (vector<connectedComponent>::iterator newCompsIt = currSliceConnComps.begin(); newCompsIt != currSliceConnComps.end(); ++newCompsIt)
			{
				++sliceBlobCount;
				boost::container::flat_set<int> blob3D;
				blob3D.insert(sliceBlobCount);
				b2Dtob3Dmap.insert(pair<int, boost::container::flat_set<int>>(newCompsIt->islandNum, blob3D));
				boost::container::flat_set<int> comps;
				comps.insert(newCompsIt->islandNum);
				b3Dcomps[sliceBlobCount] = comps;
				increasedSize = increasedSize + comps.size();
			}
			continue;
		}

		for (vector<connectedComponent>::iterator currIt = currSliceConnComps.begin(); currIt != currSliceConnComps.end(); ++currIt)
		{
			bool merged = false;
			for (vector<connectedComponent>::iterator preIt = preSliceConnComps.begin(); preIt != preSliceConnComps.end(); ++preIt)
			{
				if (currIt->xMin > preIt->xMax + 2 || currIt->xMax < preIt->xMin - 2 ||
					currIt->yMin > preIt->yMax + 2 || currIt->yMax < preIt->yMin - 2) continue;

				for (set<vector<int>>::iterator currDotIt = currIt->coordSets.begin()->second.begin(); currDotIt != currIt->coordSets.begin()->second.end(); ++currDotIt)
				{
					for (set<vector<int>>::iterator preDotIt = preIt->coordSets.begin()->second.begin(); preDotIt != preIt->coordSets.begin()->second.end(); ++preDotIt)
					{
						if (currDotIt->at(0) >= preDotIt->at(0) - 1 && currDotIt->at(0) <= preDotIt->at(0) + 1 &&
							currDotIt->at(1) >= preDotIt->at(1) - 1 && currDotIt->at(1) <= preDotIt->at(1) + 1)
						{
							merged = true;
							boost::container::flat_set<int> asso3Dblob = b2Dtob3Dmap[preIt->islandNum];
							b2Dtob3Dmap.insert(pair<int, boost::container::flat_set<int>>(currIt->islandNum, asso3Dblob));
							for (boost::container::flat_set<int>::iterator blob3DIt = asso3Dblob.begin(); blob3DIt != asso3Dblob.end(); ++blob3DIt)
								b3Dcomps[*blob3DIt].insert(currIt->islandNum);

							goto BLOB_MERGED;
						}
					}
				}

				if (!merged) continue;

			BLOB_MERGED:
				merged = true;
			}

			if (!merged)
			{
				++sliceBlobCount;
				boost::container::flat_set<int> newBlob3D;
				newBlob3D.insert(sliceBlobCount);
				b2Dtob3Dmap.insert(pair<int, boost::container::flat_set<int>>(currIt->islandNum, newBlob3D));
				boost::container::flat_set<int> comps;
				comps.insert(currIt->islandNum);
				b3Dcomps[sliceBlobCount] = comps;
				increasedSize = increasedSize + comps.size();
			}
		}
		cout << increasedSize << ", ";
	}
	cout << endl << endl;
	cout << "Done merging 2D blobs from every 2 slices." << endl << endl;
	// ---------------------------------------- END of [Merge 2D blobs from 2 adjacent slices] -------------------------------------------

	// ------------------------------------------ Merge 3D blobs --------------------------------------------
	cout << "Now merging 3D blobs.." << endl;
	cout << "-- oroginal 3D blobs number: " << b3Dcomps.size() << endl;
	bool mergeFinish = false;
	int currBaseBlob = 1;
	while (!mergeFinish)
	{
		for (boost::container::flat_map<int, boost::container::flat_set<int>>::iterator checkIt1 = b3Dcomps.begin(); checkIt1 != b3Dcomps.end(); ++checkIt1)
		{
			if (checkIt1->first < currBaseBlob) continue;
			for (boost::container::flat_map<int, boost::container::flat_set<int>>::iterator checkIt2 = checkIt1; checkIt2 != b3Dcomps.end(); ++checkIt2)
			{
				if (checkIt2 == checkIt1) continue;
				for (boost::container::flat_set<int>::iterator member1 = checkIt1->second.begin(); member1 != checkIt1->second.end(); ++member1)
				{
					for (boost::container::flat_set<int>::iterator member2 = checkIt2->second.begin(); member2 != checkIt2->second.end(); ++member2)
					{
						if (*member2 == *member1)
						{
							checkIt1->second.insert(checkIt2->second.begin(), checkIt2->second.end());
							b3Dcomps.erase(checkIt2);
							currBaseBlob = checkIt1->first;
							cout << "  merging blob " << checkIt1->first << " and blob " << checkIt2->first << endl;
							goto MERGED;
						}
					}
				}
			}
		}
		mergeFinish = true;

	MERGED:
		continue;
	}
	cout << "-- new 3D blobs number: " << b3Dcomps.size() << endl;
	// --------------------------------------- END of [Merge 3D blobs] --------------------------------------

	map<int, connectedComponent> compsMap;
	for (vector<connectedComponent>::const_iterator inputIt = inputConnCompList.begin(); inputIt != inputConnCompList.end(); ++inputIt)
		compsMap.insert(pair<int, connectedComponent>(inputIt->islandNum, *inputIt));
	int newLabel = 0;
	for (boost::container::flat_map<int, boost::container::flat_set<int>>::iterator it = b3Dcomps.begin(); it != b3Dcomps.end(); ++it)
	{
		++newLabel;
		connectedComponent newComp;
		newComp.islandNum = newLabel;
		newComp.size = 0;
		newComp.xMax = 0; newComp.xMin = 0;
		newComp.yMax = 0; newComp.yMin = 0;
		newComp.zMax = 0; newComp.zMin = 0;
		for (boost::container::flat_set<int>::iterator it2 = it->second.begin(); it2 != it->second.end(); ++it2)
		{
			newComp.coordSets.insert(pair<int, set<vector<int>>>(compsMap[*it2].coordSets.begin()->first, compsMap[*it2].coordSets.begin()->second));
			newComp.xMax = getMax(newComp.xMax, compsMap[*it2].xMax);
			newComp.xMin = getMin(newComp.xMin, compsMap[*it2].xMin);
			newComp.yMax = getMax(newComp.yMax, compsMap[*it2].yMax);
			newComp.yMin = getMin(newComp.yMin, compsMap[*it2].yMin);
			newComp.zMax = getMax(newComp.zMax, compsMap[*it2].zMax);
			newComp.zMin = getMin(newComp.zMin, compsMap[*it2].zMin);
			newComp.size = newComp.size + compsMap[*it2].size;
		}

		outputConnCompList.push_back(newComp);
	}

	return outputConnCompList;
}
// =============================== END of [SWC <-> Connected Components] ===============================

// ==================================== Neuron Struct Clustering Methods ========================================
vector<connectedComponent> NeuronStructUtil::swc2clusters_distance(const NeuronTree& inputTree, float dist)
{
	QList<NeuronSWC> inputList = inputTree.listNeuron;
	vector<connectedComponent> outputConnCompList;
	outputConnCompList.clear();
	connectedComponent connComp1;
	vector<int> node1Coord(3);
	node1Coord.at(0) = int(inputList.begin()->x);
	node1Coord.at(1) = int(inputList.begin()->y);
	node1Coord.at(2) = int(inputList.begin()->z);
	set<vector<int>> connComp1Set;
	connComp1Set.insert(node1Coord);
	connComp1.coordSets.insert(pair<int, set<vector<int>>>(node1Coord.at(2), connComp1Set));
	connComp1.size = 1;
	outputConnCompList.push_back(connComp1);
	inputList.erase(inputList.begin());

	while (inputList.size() > 0)
	{
		for (QList<NeuronSWC>::iterator nodeIt = inputList.begin(); nodeIt != inputList.end(); ++nodeIt)
		{
			for (vector<connectedComponent>::iterator cluIt = outputConnCompList.begin(); cluIt != outputConnCompList.end(); ++cluIt)
			{
				for (map<int, set<vector<int>>>::iterator zIt = cluIt->coordSets.begin(); zIt != cluIt->coordSets.end(); ++zIt)
				{
					for (set<vector<int>>::iterator cluNodeIt = zIt->second.begin(); cluNodeIt != zIt->second.end(); ++cluNodeIt)
					{
						if (sqrt((nodeIt->x - float(cluNodeIt->at(0))) * (nodeIt->x - float(cluNodeIt->at(0))) +
							     (nodeIt->y - float(cluNodeIt->at(1))) * (nodeIt->y - float(cluNodeIt->at(1))) +
							     (nodeIt->z - float(cluNodeIt->at(2))) * (nodeIt->z - float(cluNodeIt->at(2))) * zRATIO * zRATIO) <= dist)
						{
							if (cluIt->coordSets.find(nodeIt->z) != cluIt->coordSets.end())
							{
								vector<int> newPoint(3);
								newPoint.at(0) = int(nodeIt->x);
								newPoint.at(1) = int(nodeIt->y);
								newPoint.at(2) = int(nodeIt->z);
								cluIt->coordSets[newPoint.at(2)].insert(newPoint);
								++cluIt->size;
							}
							else
							{
								vector<int> newPoint(3);
								newPoint.at(0) = int(nodeIt->x);
								newPoint.at(1) = int(nodeIt->y);
								newPoint.at(2) = int(nodeIt->z);
								set<vector<int>> newSet;
								newSet.insert(newPoint);
								cluIt->coordSets.insert(pair<int, set<vector<int>>>(newPoint.at(2), newSet));
								++cluIt->size;
							}

							inputList.erase(nodeIt);
							goto NODE_PROCESSED;
						}
					}
				}
			}

			connectedComponent newComp;
			vector<int> newCompPoint(3);
			newCompPoint.at(0) = int(nodeIt->x);
			newCompPoint.at(1) = int(nodeIt->y);
			newCompPoint.at(2) = int(nodeIt->z);
			set<vector<int>> newCompSet;
			newCompSet.insert(newCompPoint);
			newComp.coordSets.insert(pair<int, set<vector<int>>>(newCompPoint.at(2), newCompSet));
			newComp.size = 1;
			outputConnCompList.push_back(newComp);
			inputList.erase(nodeIt);
			break;
		}

	NODE_PROCESSED:
		continue;
	}

	vector<connectedComponent>::iterator mergeIt;
	vector<connectedComponent>::iterator currCompIt;
	while (1)
	{
		for (vector<connectedComponent>::iterator compBaseIt = outputConnCompList.begin(); compBaseIt != outputConnCompList.end(); ++compBaseIt)
		{
			for (vector<connectedComponent>::iterator checkCompIt = compBaseIt + 1; checkCompIt != outputConnCompList.end(); ++checkCompIt)
			{
				for (map<int, set<vector<int>>>::iterator checkIt = checkCompIt->coordSets.begin(); checkIt != checkCompIt->coordSets.end(); ++checkIt)
				{
					for (set<vector<int>>::iterator checkNodeIt = checkIt->second.begin(); checkNodeIt != checkIt->second.end(); ++checkNodeIt)
					{
						for (map<int, set<vector<int>>>::iterator currIt = compBaseIt->coordSets.begin(); currIt != compBaseIt->coordSets.end(); ++currIt)
						{
							for (set<vector<int>>::iterator currNodeIt = currIt->second.begin(); currNodeIt != currIt->second.end(); ++currNodeIt)
							{
								if (sqrt((float(currNodeIt->at(0)) - float(checkNodeIt->at(0))) * (float(currNodeIt->at(0)) - float(checkNodeIt->at(0))) +
										 (float(currNodeIt->at(1)) - float(checkNodeIt->at(1))) * (float(currNodeIt->at(1)) - float(checkNodeIt->at(1))) +
										 (float(currNodeIt->at(2)) - float(checkNodeIt->at(2))) * (float(currNodeIt->at(2)) - float(checkNodeIt->at(2))) * zRATIO * zRATIO) <= dist)
								{
									mergeIt = checkCompIt;
									currCompIt = compBaseIt;
									goto CLUSTER_MERGE;
								}
							}
						}
					}
				}
			}
		}
		break;

	CLUSTER_MERGE:
		{
			for (map<int, set<vector<int>>>::iterator zIt = mergeIt->coordSets.begin(); zIt != mergeIt->coordSets.end(); ++zIt)
			{
				for (set<vector<int>>::iterator nodeIt = zIt->second.begin(); nodeIt != zIt->second.end(); ++nodeIt)
				{
					if (currCompIt->coordSets.find(nodeIt->at(0)) != currCompIt->coordSets.end())
					{
						vector<int> newPoint(3);
						newPoint.at(0) = nodeIt->at(0);
						newPoint.at(1) = nodeIt->at(1);
						newPoint.at(2) = nodeIt->at(2);
						currCompIt->coordSets[newPoint.at(2)].insert(newPoint);
					}
					else
					{
						vector<int> newPoint(3);
						newPoint.at(0) = nodeIt->at(0);
						newPoint.at(1) = nodeIt->at(1);
						newPoint.at(2) = nodeIt->at(2);
						set<vector<int>> newSet;
						newSet.insert(newPoint);
						currCompIt->coordSets.insert(pair<int, set<vector<int>>>(newPoint.at(2), newSet));
					}
				}
			}

			outputConnCompList.erase(mergeIt);
			continue;
		}
	}

	return outputConnCompList;
}
// ================================ END of [Neuron Struct Clustering Methods] ===================================

/* =================================== Volumetric SWC sampling methods =================================== */
void NeuronStructUtil::sigNode_Gen(const NeuronTree& inputTree, NeuronTree& outputTree, float ratio, float distance)
{
	// -- Randomly generate signal patches within given distance range
	//      ratio:    the ratio of targeted number of upsampling nodes to the number of maunal nodes in the inputTree
	//      distance: the radius allowed with SWC node centered

	cout << "target signal patch number: " << int(inputTree.listNeuron.size() * ratio) << endl;
	int nodeCount = 0;
	for (QList<NeuronSWC>::const_iterator it = inputTree.listNeuron.begin(); it != inputTree.listNeuron.end(); ++it)
	{
		outputTree.listNeuron.push_back(*it);
		(outputTree.listNeuron.end() - 1)->parent = -1;
		(outputTree.listNeuron.end() - 1)->type = 2;
		int foldCount = 2;
		while (foldCount <= ratio)
		{
			int randNumX = rand() % int(distance * 2) + int(it->x - distance);
			int randNumY = rand() % int(distance * 2) + int(it->y - distance);
			int randNumZ = rand() % int(distance * 2) + int(it->z - distance);

			++nodeCount;
			if (nodeCount % 10000 == 0) cout << nodeCount << " signal nodes generated." << endl;

			NeuronSWC newNode;
			newNode.x = randNumX;
			newNode.y = randNumY;
			newNode.z = randNumZ;
			newNode.type = 2;
			newNode.radius = 1;
			newNode.parent = -1;
			outputTree.listNeuron.push_back(newNode);

			++foldCount;
		}
	}
}

void NeuronStructUtil::bkgNode_Gen(const NeuronTree& inputTree, NeuronTree& outputTree, int dims[], float ratio, float distance)
{
	// -- Randomly generate background patches away from the forbidden distance
	//      dims:     image stack dimension
	//      ratio:    the ratio of targeted number of background nodes to the number of manual nodes in the inputTree
	//      distance: the forbidden distance from each SWC node

	QList<NeuronSWC> neuronList = inputTree.listNeuron;
	int targetBkgNodeNum = int(neuronList.size() * ratio);
	cout << targetBkgNodeNum << " targeted bkg nodes to ge generated." << endl;
	int bkgNodeCount = 0;
	while (bkgNodeCount <= targetBkgNodeNum)
	{
		int randNumX = rand() % (dims[0] - 20) + 10;
		int randNumY = rand() % (dims[1] - 20) + 10;
		int randNumZ = rand() % dims[2];

		bool flag = false;
		for (QList<NeuronSWC>::iterator it = neuronList.begin(); it != neuronList.end(); ++it)
		{
			float distSqr;
			float diffx = float(randNumX) - it->x;
			float diffy = float(randNumY) - it->y;
			float diffz = float(randNumZ) - it->z;
			distSqr = diffx * diffx + diffy * diffy + diffz * diffz;

			if (distSqr <= distance * distance)
			{
				flag = true;
				break;
			}
		}

		if (flag == false)
		{
			++bkgNodeCount;
			if (bkgNodeCount % 10000 == 0) cout << bkgNodeCount << " bkg nodes generated." << endl;

			NeuronSWC newBkgNode;
			newBkgNode.x = randNumX;
			newBkgNode.y = randNumY;
			newBkgNode.z = randNumZ;
			newBkgNode.type = 3;
			newBkgNode.radius = 1;
			newBkgNode.parent = -1;
			outputTree.listNeuron.push_back(newBkgNode);
		}
	}
}

void NeuronStructUtil::bkgNode_Gen_somaArea(const NeuronTree& intputTree, NeuronTree& outputTree, int xLength, int yLength, int zLength, float ratio, float distance)
{
	// -- Randomly generate background patches away from the forbidden distance. This method aims to reinforce the background recognition near soma area.
	//      xLength, yLength, zLength: decide the range to apply with soma centered
	//      ratio:    the ratio of targeted number of soma background nodes to the number of manual nodes in the inputTree
	//      distance: the forbidden distance from each SWC node

	NeuronSWC somaNode;
	for (QList<NeuronSWC>::const_iterator it = intputTree.listNeuron.begin(); it != intputTree.listNeuron.end(); ++it)
	{
		if (it->parent == -1)
		{
			somaNode = *it;
			break;
		}
	}
	float xlb = somaNode.x - float(xLength);
	float xhb = somaNode.x + float(xLength);
	float ylb = somaNode.y - float(yLength);
	float yhb = somaNode.y + float(yLength);
	float zlb = somaNode.z - float(zLength);
	float zhb = somaNode.z + float(zLength);

	list<NeuronSWC> confinedNodes;
	for (QList<NeuronSWC>::const_iterator it = intputTree.listNeuron.begin(); it != intputTree.listNeuron.end(); ++it)
		if (xlb <= it->x && xhb >= it->x && ylb <= it->y && yhb >= it->y && zlb <= it->z && zhb >= it->z) confinedNodes.push_back(*it);

	int targetBkgNodeNum = int(float(xLength) * float(yLength) * float(zLength) * ratio);
	cout << targetBkgNodeNum << " targeted bkg nodes to ge generated." << endl;
	int bkgNodeCount = 0;
	while (bkgNodeCount <= targetBkgNodeNum)
	{
		int randNumX = rand() % int(xhb - xlb + 1) + int(xlb);
		int randNumY = rand() % int(yhb - ylb + 1) + int(ylb);
		int randNumZ = rand() % int(zhb - zlb + 1) + int(zlb);

		bool flag = false;
		for (list<NeuronSWC>::iterator it = confinedNodes.begin(); it != confinedNodes.end(); ++it)
		{
			float distSqr;
			float diffx = float(randNumX) - it->x;
			float diffy = float(randNumY) - it->y;
			float diffz = float(randNumZ) - it->z;
			distSqr = diffx * diffx + diffy * diffy + diffz * diffz;

			if (distSqr <= distance * distance)
			{
				flag = true;
				break;
			}
		}

		if (flag == false)
		{
			++bkgNodeCount;
			if (bkgNodeCount % 10000 == 0) cout << bkgNodeCount << " bkg nodes generated within the soma area." << endl;

			NeuronSWC newBkgNode;
			newBkgNode.x = randNumX;
			newBkgNode.y = randNumY;
			newBkgNode.z = randNumZ;
			newBkgNode.type = 3;
			newBkgNode.radius = 1;
			newBkgNode.parent = -1;
			outputTree.listNeuron.push_back(newBkgNode);
		}
	}
}
/* =================================== Volumetric SWC sampling methods =================================== */


#include <iostream>
#include <Eigen/Core>

#include "celerite/solver/direct.h"
#include "celerite/solver/cholesky.h"

#define DO_TEST(NAME, VAR1, VAR2)                            \
{                                                            \
  double base, comp, delta;                                  \
  base = VAR1;                                               \
  comp = VAR2;                                               \
  delta = std::abs(base - comp);                             \
  if (delta > 1e-10) {                                       \
    std::cerr << "Test failed: '" << #NAME << "' - error: " << delta << " " << base << " " << comp << std::endl; \
    return 1;                                                \
  } else                                                     \
    std::cerr << "Test passed: '" << #NAME << "' - error: " << delta << std::endl; \
}

int main (int argc, char* argv[])
{
  srand(42);

  size_t N = 1024;
  if (argc >= 2) N = atoi(argv[1]);
  size_t niter = 10;
  if (argc >= 3) niter = atoi(argv[2]);

  // Set up the coefficients.
  size_t p_real = 2, p_complex = 2;
  double jitter = 0.01;
  Eigen::VectorXd alpha_real(p_real),
                  alpha_complex_real(p_complex),
                  alpha_complex_imag(p_complex),
                  beta_real(p_real),
                  beta_complex_real(p_complex),
                  beta_complex_imag(p_complex);

  alpha_real << 1.3, 1.5;
  beta_real  << 0.5, 0.2;
  //alpha_complex_real << 1.0;
  //alpha_complex_imag << 0.1;
  //beta_complex_real  << 1.0;
  //beta_complex_imag  << 1.0;
  alpha_complex_real << 1.0, 2.0;
  alpha_complex_imag << 0.1, 0.05;
  beta_complex_real  << 1.0, 0.8;
  beta_complex_imag  << 1.0, 0.1;

  // Generate some fake data.
  Eigen::VectorXd x = Eigen::VectorXd::Random(N),
                  yerr2 = Eigen::VectorXd::Random(N),
                  y;

  // Set the scale of the uncertainties.
  yerr2.array() *= 0.1;
  yerr2.array() += 0.3;

  // The times need to be sorted.
  std::sort(x.data(), x.data() + x.size());

  // Compute the y values.
  y = sin(x.array());

  Eigen::MatrixXd U(3, N), V(3, N);
  Eigen::VectorXd A(N);
  for (int n = 0; n < N; ++n) {
    A(n) = 1e-6;
    for (int j = 0; j < 3; ++j) {
      U(j, n) = pow(x(n), j);
      V(j, n) = pow(x(n), j) / (1.0 + j);
      A(n) += U(j, n) * V(j, n);
    }
  }

  celerite::solver::DirectSolver<double> direct;
  celerite::solver::CholeskySolver<double> cholesky;

  cholesky.compute(jitter, alpha_real, beta_real, x, yerr2);
  direct.compute(jitter, alpha_real, beta_real, x, yerr2);
  DO_TEST(cholesky_real_log_det, cholesky.log_determinant(), direct.log_determinant())
  DO_TEST(cholesky_real_dot_solve, direct.dot_solve(y), cholesky.dot_solve(y))

  direct.compute(jitter, alpha_complex_real, alpha_complex_imag, beta_complex_real, beta_complex_imag, x, yerr2);
  cholesky.compute(jitter, alpha_complex_real, alpha_complex_imag, beta_complex_real, beta_complex_imag, x, yerr2);
  DO_TEST(cholesky_complex_log_det, direct.log_determinant(), cholesky.log_determinant())
  DO_TEST(cholesky_complex_dot_solve, cholesky.dot_solve(y), direct.dot_solve(y))

  direct.compute(jitter, alpha_real, beta_real, alpha_complex_real, alpha_complex_imag, beta_complex_real, beta_complex_imag, x, yerr2);
  cholesky.compute(jitter, alpha_real, beta_real, alpha_complex_real, alpha_complex_imag, beta_complex_real, beta_complex_imag, x, yerr2);
  DO_TEST(cholesky_mixed_log_det, direct.log_determinant(), cholesky.log_determinant())
  DO_TEST(cholesky_mixed_dot_solve, cholesky.dot_solve(y), direct.dot_solve(y))

  direct.compute(jitter, alpha_real, beta_real, alpha_complex_real, alpha_complex_imag, beta_complex_real, beta_complex_imag, A, U, V, x, yerr2);
  cholesky.compute(jitter, alpha_real, beta_real, alpha_complex_real, alpha_complex_imag, beta_complex_real, beta_complex_imag, A, U, V, x, yerr2);
  DO_TEST(cholesky_general_log_det, direct.log_determinant(), cholesky.log_determinant())
  DO_TEST(cholesky_general_dot_solve, cholesky.dot_solve(y), direct.dot_solve(y))

  return 0;
}


/*
 * Licensed to the Apache Software Foundation (ASF) under one or more
 * contributor license agreements.  See the NOTICE file distributed with
 * this work for additional information regarding copyright ownership.
 * The ASF licenses this file to You under the Apache License, Version 2.0
 * (the "License"); you may not use this file except in compliance with
 * the License.  You may obtain a copy of the License at
 *
 *    http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#include "../plib/JointProb.h"
#include "../plib/MutualInfo.h"
#include "../plib/utils.h"

#include <vector>
#include <algorithm>
#include <limits>
#include <boost/bind.hpp>
#include <stdio.h>
#include <string.h>

using namespace std;

//Returns the index of the higher value in the classRelevances Vector different from classIndex
uint getMaxRelevance(vector<double> classRelevances, uint classIndex) {
	uint i = 0;
	uint newFeature = -1;
	double relevance = 0;
	for (i = 0; i < classRelevances.size(); ++i) {
		if (classRelevances[i] > relevance && i != classIndex) {
			relevance = classRelevances[i];
			newFeature = i;
		}
	}
	return newFeature;
}

options parseOptions(int argc, char*argv[]) {
	options opts;
	opts.classIndex = 0;
	opts.selectedFeatures = 10;
	opts.file = "../data.mrmr";

	if (argc > 1) {
		for (int i = 0; i < argc; ++i) {
			if (strcmp(argv[i], "-f") == 0) {
				opts.file = argv[i + 1];
			}
			if (strcmp(argv[i], "-a") == 0) {
				opts.selectedFeatures = atoi(argv[i + 1]) - 1;
			}
			if (strcmp(argv[i], "-c") == 0) {
				opts.classIndex = atoi(argv[i + 1]) - 1;
			}
			if (strcmp(argv[i], "-h") == 0) {
				printf(
						"fast-mrmr:\nOptions:\n -f <inputfile>\t\tMRMR file generated using mrmrReader (default: data.mrmr).\n-c <classindex>\t\tIndicates the class index in the dataset (default: 0).\n-a <nfeatures>\t Indicates the number of features to select (default: 10).\n-h Prints this message");
				exit(0);
			}
		}
	}
	return opts;
}

int main(int argc, char* argv[]) {
	options opts;
	uint i = 0;
	uint j = 0;
	uint newFeatureIndex = 0;
	uint lastFeatureIndex = 0;
	double mrmr = 0;
	double acum = 0;
	vector<double> relevances;
	vector<double> redundances;
	vector<int> selectedFeatures;

	Timer tm;
	opts = parseOptions(argc, argv);
	RawData rawData = RawData(opts.file);
	tm.start();
	ProbTable prob = ProbTable(rawData);
	MutualInfo mutualInfo = MutualInfo(rawData, prob);

	//Get relevances between all features and class.
	for (i = 0; i < rawData.getFeaturesSize(); ++i) {
		relevances.push_back(mutualInfo.get(opts.classIndex, i));
		redundances.push_back(0);
	}

	// Max relevance feature is added because no redundancy is possible.
	newFeatureIndex = getMaxRelevance(relevances, opts.classIndex);
	selectedFeatures.push_back(newFeatureIndex);
	lastFeatureIndex = newFeatureIndex;

	cout << newFeatureIndex << ",";
	//MRMR
	while (selectedFeatures.size() < rawData.getFeaturesSize() - 1 //-1 because class is discarded
	and selectedFeatures.size() < opts.selectedFeatures) {
		acum = -std::numeric_limits<double>::infinity();
		for (j = 0; j < rawData.getFeaturesSize(); ++j) {
			//If feature not in selected selectedFeatures
			if (find(selectedFeatures.begin(), selectedFeatures.end(), j)
					== selectedFeatures.end() && j != opts.classIndex) {
				redundances[j] += mutualInfo.get(lastFeatureIndex, j);
				mrmr = relevances[j]
						- (redundances[j] / selectedFeatures.size());
				if (mrmr > acum) {
					acum = mrmr;
					newFeatureIndex = j;
				}
			}
		}
		//Last feature doesn't prints comma.
		if ( (selectedFeatures.size() == (opts.selectedFeatures - 1)) or (selectedFeatures.size() == (rawData.getFeaturesSize() -2)) ){
			cout << newFeatureIndex;
		}else{
			cout << newFeatureIndex << ",";
		}
		selectedFeatures.push_back(newFeatureIndex);
		lastFeatureIndex = newFeatureIndex;
	}

	rawData.destroy();
	prob.destroy();
	printf("\n");
	return (0);
}


#include "HAstar.h"

#include <cassert>
#include <boost/heap/binomial_heap.hpp>
#include "utils/Helper.h"

namespace HybridAStar{
namespace Geometry{
  struct CompareNodes {
    /// Sorting 3D nodes by increasing C value - the total estimated cost
    bool operator()(const std::shared_ptr<SE2State> lhs, const std::shared_ptr<SE2State> rhs) const {
      return lhs->getC() > rhs->getC();
    }
    /// Sorting 2D nodes by increasing C value - the total estimated cost
    bool operator()(const std::shared_ptr<GridState> lhs, const std::shared_ptr<GridState> rhs) const {
      return lhs->getC() > rhs->getC();
    }
  };

  HAstar::HAstar(shared_ptr<SE2State> start,SE2State &goal,
                shared_ptr<Map<SE2State>> pmap,shared_ptr<CollisionDetection> &configSpace):
                Planner(start,goal),start_(start),goal_(goal),
                configSpace_(configSpace),
                rsPlanner_(std::make_unique<hRScurve>(*start_,goal_)),
                // rrtxPlanner_(std::make_unique<hRRTx>(*start_,goal_,
                //                                       pmap->info_.width*pmap->info_.resolution, // real width [meters]
                //                                       pmap->info_.height*pmap->info_.resolution,  // real height [meters]
                //                                       configSpace)),
                aStarPlanner_(std::make_unique<hAStar>()),
                pMap_(pmap){
    #ifdef _HEURISTIC_ASTAR_H
    auto s = std::make_shared<GridState>(pmap->info_.planResolution,0);
    GridState g(pmap->info_.planResolution,0);
    s->setX(start->getX()/pmap->info_.planResolution);
    s->setY(start->getY()/pmap->info_.planResolution);
    g.setX(goal.getX()/pmap->info_.planResolution);
    g.setY(goal.getY()/pmap->info_.planResolution);

    aStarPlanner_.reset(new hAStar(s,g,configSpace));
    // set map
    // TODO: make this two different class copyable
    aStarPlanner_->returnMap().reset(new Map<GridState>());
    aStarPlanner_->returnMap()->info_.width = pmap->info_.width;
    aStarPlanner_->returnMap()->info_.height = pmap->info_.height;
    aStarPlanner_->returnMap()->info_.resolution = pmap->info_.resolution;
    aStarPlanner_->returnMap()->info_.planResolution = pmap->info_.planResolution;
    aStarPlanner_->returnMap()->info_.data = pmap->info_.data;
    aStarPlanner_->setSS();
    #endif
  }

  void HAstar::updateHeuristic(SE2State &start,SE2State &goal) {
    rsPlanner_->setStartGoal(start,goal);
    auto rsCost = rsPlanner_->getDistance();

    #ifdef _HEURISTIC_RRTX_H
    rrtxPlanner_->setStartGoal(start,goal);
    auto rrtxCost = rrtxPlanner_->getDistance();

    start.setH(std::max(rrtxCost,rsCost));
    #elif defined _HEURISTIC_ASTAR_H
    auto s = std::make_shared<GridState>(pMap_->info_.planResolution,0);
    GridState g(pMap_->info_.planResolution, 0);
    float srx = start.getX()/pMap_->info_.planResolution;
    float sry = start.getY()/pMap_->info_.planResolution;
    float grx = goal.getX()/pMap_->info_.planResolution;
    float gry = goal.getY()/pMap_->info_.planResolution;
    float sx = srx - (int)srx;
    float sy = sry - (int)sry;
    float gx = grx - (int)grx;
    float gy = gry - (int)gry;
    s->setX(srx); s->setY(sry);
    g.setX(grx); g.setY(gry);
    aStarPlanner_->setStartGoal(s,g);
    auto astarCost = aStarPlanner_->getDistance();

    double offSet = sqrt(pow(sx-gx,2) + pow(sy-gy,2));
    astarCost -= offSet;
    astarCost *= pMap_->info_.planResolution;

    start.setH(std::max(astarCost,rsCost));
    #endif
  }

#ifdef DUBINS_H
  std::shared_ptr<SE2State> HAstar::dubinsShot(
    std::shared_ptr<SE2State> start, const SE2State& goal) {
    // start
    double q0[] = { start->getX(), start->getY(), start->getT() };
    // goal
    double q1[] = { goal.getX(), goal.getY(), goal.getT() };
    // initialize the path
    DubinsPath path;
    // calculate the path
    dubins_init(q0, q1, carPlant_->rad_, &path);

    float x = carPlant_->dubinsStepSize_;
    float length = dubins_path_length(&path);
    int i = 0;

    // TODO: this construction should not related to the default constructor
    // TODO: the angle resolution should be synthesized to a class
    auto dubinsNodes = std::vector<std::shared_ptr<SE2State>>(
      (int)(length / carPlant_->dubinsStepSize_) + 2);
      // std::make_shared<SE2State>(pMap_->info_.planResolution, 0.087266)

    while (x <  length) {
      // TODO: this angle resolution should be synthesized to a class
      auto temp = std::make_shared<SE2State>(pMap_->info_.planResolution, 0.087266);
      double q[3];
      dubins_path_sample(&path, x, q);
      temp->setX(q[0]);
      temp->setY(q[1]);
      temp->setT(Utils::normalizeHeadingRad(q[2]));
      dubinsNodes[i] = temp;

      // collision check
      if (configSpace_->isTraversable(dubinsNodes[i].get())) {

        // set the predecessor to the previous step
        if (i > 0) {
          dubinsNodes[i]->setPred(dubinsNodes[i-1]);
        } else {
          dubinsNodes[i]->setPred(start);
        }

        if ((dubinsNodes[i]) == dubinsNodes[i]->getPred()) {
          std::cout << "looping shot";
        }

        x += carPlant_->dubinsStepSize_;
        i++;
      } else {
        // collided, discarding the path
        std::shared_ptr<SE2State> temp;
        return temp;
      }
    }

    // TODO: change to a more delicated loop, which also include is Traversable judging
    auto temp = std::make_shared<SE2State>(pMap_->info_.planResolution, 0.087266);
    temp->setX(q1[0]);
    temp->setY(q1[1]);
    temp->setT(Utils::normalizeHeadingRad(q1[2]));
    temp->setPred(dubinsNodes[i-1]);
    dubinsNodes[i] = temp;

    return dubinsNodes[i];
  }
#endif

#ifdef _HYBRIDASTAR_REEDSSHEPPPATH_H
  std::shared_ptr<SE2State> HAstar::ReedsShepp(std::shared_ptr<SE2State> start, const SE2State &goal) {
    std::vector<double> bounds{0,(pMap_->info_.width*pMap_->info_.resolution),
                                0,(pMap_->info_.width*pMap_->info_.resolution)};
    auto startNode = std::make_shared<Node3d>(start->getX(),start->getY(),start->getT(),pMap_->info_.planResolution,0.08726646,bounds);
    auto goalNode = std::make_shared<Node3d>(goal.getX(),goal.getY(),goal.getT(),pMap_->info_.planResolution,0.08726646,bounds);

    auto a = start->getT()*180/M_PI;
    // TODO:change the last 0.5 to a carplant related value
    // should carplant class be an abstract base class and other class inherit from this class?
    auto reed_shepp_generator = std::make_shared<RSPath4Fork>(1/carPlant_->rad_,0.2);
    ReedSheppPath optimal_path;

    // TODO: what if the algorithm really goes into this?
    // more robustness should be achieved
    if (!reed_shepp_generator->ShortestRSP(startNode, goalNode, optimal_path)) {
      std::cout << "[NOT FATEL]RS path generation failed!" << std::endl;
      std::shared_ptr<SE2State> temp;
      return temp;
    }

    const int samplSize = optimal_path.x.size() - 1;  // remove the start point, which is a repeated point

    // TODO : change to a smart pointer
    // includes start and goal node
    auto RSNodes = std::vector<std::shared_ptr<SE2State>>(samplSize);

    int i;
    for (i = 0; i < samplSize; ++i) {
      // TODO: this angle resolution should be synthesized to a class
      auto temp = std::make_shared<SE2State>(pMap_->info_.planResolution, 0.087266);
      temp->setX(optimal_path.x[i+1]);
      temp->setY(optimal_path.y[i+1]);
      temp->setT(Utils::normalizeHeadingRad(optimal_path.phi[i+1]));
      RSNodes[i] = temp;

      // collision check
      if (configSpace_->isTraversable(RSNodes[i].get())) {
        optimal_path.gear[i+1] ? RSNodes[i]->setPrim(0) : RSNodes[i]->setPrim(3);
        // set the predecessor to the previous step
        if (i > 0) {
          RSNodes[i]->setPred(RSNodes[i - 1]);
        } else {
          RSNodes[i]->setPred(start);
        }

        if (RSNodes[i] == RSNodes[i]->getPred()) {
          std::cout << "looping shot";
        }

      } else {
        // collided, discarding the path
        std::shared_ptr<SE2State> temp;
        return temp;
      }
    }
    return RSNodes[i-1];
  }
#endif

  SE2State &HAstar::getStart() {
    return *start_;
  }

  SE2State &HAstar::getGoal() {
    return goal_;
  }

  void HAstar::setStart(std::shared_ptr<SE2State> start) {
    #ifdef _HEURISTIC_RRTX_H
    assert(rrtxPlanner_->isTreeconstructed());
    start_ = start;
    rsPlanner_->setStart(*start);
    rrtxPlanner_->setGoal(*start); // the goal and start in rrtx is reversed
    #elif defined _HEURISTIC_ASTAR_H
    start_ = start;
    rsPlanner_->setStart(*start);
    auto s = std::make_shared<GridState>(pMap_->info_.planResolution,0);
    s->setX(start->getX()/pMap_->info_.planResolution);
    s->setY(start->getY()/pMap_->info_.planResolution);
    aStarPlanner_->setStart(s);
    #endif
  }

  // solve for the whole path
  std::shared_ptr<SE2State> HAstar::solve(){
    int pWidth = pMap_->info_.width*(pMap_->info_.resolution)/(pMap_->info_.planResolution);
    int pHeight = pMap_->info_.height*(pMap_->info_.resolution)/(pMap_->info_.planResolution);
    // PREDECESSOR AND SUCCESSOR INDEX
    int iPred, iSucc;
    float newG;
    // Number of possible directions, 3 for forward driving and an additional 3 for reversing
    int dir = carPlant_->isReversable_ ? 6 : 3;
    // Number of iterations the algorithm has run for stopping based on Constants::iterations
    int iterations = 0;

    // VISUALIZATION DELAY
    ros::Duration d(0.003);

    // OPEN LIST AS BOOST IMPLEMENTATION
    typedef boost::heap::binomial_heap<std::shared_ptr<SE2State>,
            boost::heap::compare<CompareNodes>
            > priorityQueue;
    priorityQueue O;

    // update h value
    updateHeuristic(*start_, goal_);
    // mark start_ as open
    start_->open();
    // push on priority queue open list
    O.push(start_);
    iPred = start_->setIdx(pWidth, pHeight);
    pMap_->statespace[iPred] = *start_;

    // NODE POINTER
    std::shared_ptr<SE2State> nPred;
    std::shared_ptr<SE2State> nSucc;

    // float max = 0.f;

    // continue until O empty
    while (!O.empty()) {
      // pop node with lowest cost from priority queue
      nPred = O.top();
      // set index
      iPred = nPred->setIdx(pWidth, pHeight);
      iterations++;

      // _____________________________
      // LAZY DELETION of rewired node
      // if there exists a pointer this node has already been expanded
      if (pMap_->statespace[iPred].isClosed()) {
        // pop node from the open list and start_ with a fresh node
        O.pop();
        continue;
      }
      // _________________
      // EXPANSION OF NODE
      else if (pMap_->statespace[iPred].isOpen()) {
        // add node to closed list
        pMap_->statespace[iPred].close();
        // remove node from open list
        O.pop();

        // _________
        // GOAL TEST
        if (*nPred == goal_ || iterations > defaultIter) {
          // DEBUG
          return nPred;
        }

        // ____________________
        // CONTINUE WITH SEARCH
        else {
          // _______________________
          // SEARCH WITH DUBINS SHOT
          // TODO:when changed to RS curve, the primitives < 3 needs to be removed
          // the isInRange function needs to be more scientific
          if (nPred->isInRange(goal_) && nPred->getPrim() < 3) {
            #if defined DUBINS_H
            nSucc = dubinsShot(nPred, goal_);
            #elif defined _HYBRIDASTAR_REEDSSHEPPPATH_H
            nSucc = ReedsShepp(nPred, goal_);
            #endif
            if (nSucc != nullptr && *nSucc == goal_) {
              std::cout << "iterations:" << iterations << std::endl;

              return nSucc;
            }
          }

          // ______________________________
          // SEARCH WITH FORWARD SIMULATION
          for (int i = 0; i < dir; i++) {
            // create possible successor
            nSucc.reset(nPred->createSuccessor(i,nPred));
            // set index of the successor
            iSucc = nSucc->setIdx(pWidth, pHeight);

            // ensure successor is on grid and traversable / resolution:[meters/cell]
            if (nSucc->isOnGrid(static_cast<int>(pWidth), static_cast<int>(pHeight)) && configSpace_->isTraversable(nSucc.get())) {

              // ensure successor is not on closed list or it has the same index as the predecessor
              if (!pMap_->statespace[iSucc].isClosed() || iPred == iSucc) {

                // calculate new G value
                nSucc->updateG();
                newG = nSucc->getG();

                // if successor not on open list or found a shorter way to the cell
                if (!pMap_->statespace[iSucc].isOpen() || newG < pMap_->statespace[iSucc].getG() || iPred == iSucc) {

                  // calculate H value
                  updateHeuristic(*nSucc, goal_);

                  // if the successor is in the same cell but the C value is larger
                  if (iPred == iSucc && nSucc->getC() > nPred->getC() + carPlant_->tieBreaker_) {
                    continue;
                  }
                  // if successor is in the same cell and the C value is lower, set predecessor to predecessor of predecessor
                  else if (iPred == iSucc && nSucc->getC() <= nPred->getC() + carPlant_->tieBreaker_) {
                    nSucc->setPred(nPred->getPred());
                  }

                  if (nSucc->getPred() == nSucc) {
                    std::cout << "looping";
                  }

                  // put successor on open list
                  nSucc->open();
                  pMap_->statespace[iSucc] = *nSucc;
                  O.push(nSucc);
                }
              }
            }
          }
        }
      }
    }
    std::cout << "iterations:" << iterations << std::endl;
    std::shared_ptr<SE2State> temp;
    if (O.empty()) {
      return temp;
    }

    return temp;
  }
} // namespace Geometry
} // namespace HybridAStar


/*!
 * @file
 * Defines the @ref Group typeclass.
 *
 *
 * @copyright Louis Dionne 2014
 * Distributed under the Boost Software License, Version 1.0.
 *         (See accompanying file LICENSE.md or copy at
 *             http://www.boost.org/LICENSE_1_0.txt)
 */

#ifndef BOOST_MPL11_GROUP_HPP
#define BOOST_MPL11_GROUP_HPP

#include <boost/mpl11/fwd/group.hpp>

#include <boost/mpl11/fwd/bool.hpp>
#include <boost/mpl11/fwd/core.hpp>
#include <boost/mpl11/fwd/monoid.hpp>

namespace boost { namespace mpl11 {
    template <>
    struct instantiate<Group> {
        template <typename Left, typename Right = Left>
        struct with;

        template <typename Datatype>
        struct with<Datatype> : true_ {
            template <typename x, typename y>
            using minus_impl = plus<box<x>, negate<box<y>>>;

            template <typename x>
            using negate_impl = minus<zero<Datatype>, box<x>>;
        };
    };
}} // end namespace boost::mpl11

#include <boost/mpl11/bool.hpp>
#include <boost/mpl11/core.hpp>

namespace boost { namespace mpl11 {
    template <typename Left, typename Right, typename>
    struct Group : false_ { };

    template <typename x, typename y>
    struct minus :
        Group<
            typename datatype<typename x::type>::type,
            typename datatype<typename y::type>::type
        >::template minus_impl<typename x::type, typename y::type>
    { };

    template <typename x>
    struct negate :
        Group<typename datatype<typename x::type>::type>::
        template negate_impl<typename x::type>
    { };
}} // end namespace boost::mpl11

#endif // !BOOST_MPL11_GROUP_HPP


//
// Created by jhz on 19-8-22.
//

#include "utils/dataprocess_utils.h"
// 字符串读取和分割.
#include <fstream>
#include <boost/algorithm/string/classification.hpp>
#include <boost/algorithm/string/split.hpp>

#include <Eigen/Core>
#include <Eigen/Dense>

#include <fstream>

#include "Thirdparty/g2o/g2o/types/types_six_dof_expmap.h"

using namespace std;
using namespace Eigen;
typedef Matrix<double, 7, 1> Vector7d;
typedef Matrix<double, 6, 1> Vector6d;

Eigen::MatrixXd readDataFromFile(const char* fileName, bool dropFirstline){
    ifstream fin(fileName);
    string line;

    if(dropFirstline)
        getline(fin, line); // 扔掉第一行数据

    MatrixXd mat;
    int line_num = 0;
    while( getline(fin, line) )
    {
        vector<string> s;
        boost::split( s, line, boost::is_any_of( " \t," ), boost::token_compress_on );

        VectorXd lineVector(s.size());
        for (int i=0;i<int(s.size());i++)
            lineVector(i) = stod(s[i]);

        if(line_num == 0)
            mat.conservativeResize(1, s.size());
        else
            // vector to matrix.
            mat.conservativeResize(mat.rows()+1, mat.cols());

        mat.row(mat.rows()-1) = lineVector;

        line_num++;
    }
    fin.close();

    return mat;
}

// for multi objects
void selectKeyFrame(Eigen::MatrixXd &poses_mat_selected, std::vector<Eigen::MatrixXd> &detect_mat_selected,
                    Eigen::MatrixXd &poses_mat_key, std::vector<Eigen::MatrixXd> &detect_mat_key,
                    double deltaTheta, double deltaTrans){
    MatrixXd poses_key(0, poses_mat_selected.cols());
    std::vector<MatrixXd> detect_key;

    int rows = poses_mat_selected.rows();
    g2o::SE3Quat lastPose;
    for(int i=0;i<rows;i++)
    {
        Vector7d v = poses_mat_selected.row(i).tail(7);
        g2o::SE3Quat Twc(v);

        g2o::SE3Quat delta = lastPose.inverse() * Twc;
        Eigen::AngleAxisd rotation_vector(delta.rotation());
        if( delta.translation().norm() > deltaTrans && rotation_vector.angle() > deltaTheta ){
            // add one pose
            poses_key.conservativeResize(poses_key.rows()+1, poses_key.cols());
            poses_key.row(poses_key.rows()-1) =  poses_mat_selected.row(i);

            detect_key.push_back(detect_mat_selected[i]);

            lastPose = Twc;
        }

    }

    poses_mat_key = poses_key;
    detect_mat_key = detect_key;

}

void selectKeyFrame(Eigen::MatrixXd &poses_mat_selected, Eigen::MatrixXd &detect_mat_selected,
                    Eigen::MatrixXd &poses_mat_key, Eigen::MatrixXd &detect_mat_key,
                    double deltaTheta, double deltaTrans){
    MatrixXd poses_key(0, poses_mat_selected.cols());
    MatrixXd detect_key(0, detect_mat_selected.cols());

    int rows = poses_mat_selected.rows();
    g2o::SE3Quat lastPose;
    for(int i=0;i<rows;i++)
    {
        Vector7d v = poses_mat_selected.row(i).tail(7);
        g2o::SE3Quat Twc(v);

        g2o::SE3Quat delta = lastPose.inverse() * Twc;
        Eigen::AngleAxisd rotation_vector(delta.rotation());
        if( delta.translation().norm() > deltaTrans && rotation_vector.angle() > deltaTheta ){
            // add one pose
            poses_key.conservativeResize(poses_key.rows()+1, poses_key.cols());
            poses_key.row(poses_key.rows()-1) =  poses_mat_selected.row(i);

            detect_key.conservativeResize(detect_key.rows()+1, detect_key.cols());
            detect_key.row(detect_key.rows()-1) =  detect_mat_selected.row(i);

            lastPose = Twc;
        }

    }

    poses_mat_key = poses_key;
    detect_mat_key = detect_key;

}

void analyzeDetectionsAndStoreAsFrameAndID(std::vector<Eigen::MatrixXd> &detect_mat_key,
                                           std::vector<Eigen::MatrixXd> &bbox_mats)
{
    // 处理重复的检测. 暂时都按第一个.
    int num = detect_mat_key.size();

    std::set<int> label_sets;

    bbox_mats.resize(num);
    for(int i=0;i<num;i++)
    {
        MatrixXd detMat = detect_mat_key[i];
        bbox_mats[i].resize(0, detMat.cols());

        int totalDet = detMat.rows();
        for(int p=0;p<totalDet;p++)
        {
            VectorXd detVec = detMat.row(p);
            int objID = detVec(5);
            if( label_sets.find(objID) == label_sets.end() )   // 仅当没有时插入.
            {
                label_sets.insert(objID);
                bbox_mats[i].conservativeResize(bbox_mats[i].rows()+1, bbox_mats[i].cols());
                bbox_mats[i].row(bbox_mats[i].rows()-1) = detVec;
            }
        }
        label_sets.clear(); // 新的以帧 重新考虑检测.

    }

}

bool saveMatToFile(Eigen::MatrixXd &matIn, const char* fileName){
    ofstream fout;
    fout.open(fileName);

    int rows = matIn.rows();
    for(int i=0;i<rows;i++)
    {
        VectorXd v = matIn.row(i);
        int nums = v.rows();
        for( int m=0;m<nums;m++){
            fout << setprecision(12) << v(m);

            if( m== nums-1 )
                break;
            fout << " ";
        }

        fout << std::endl;
    }
    fout.close();

}

// 以下为InteriorNet数据集专场.
namespace InteriorNet{

bool loadPosesAndSingleObjectAnnotation(Eigen::MatrixXd &poses, Eigen::MatrixXd &detections){

    char fileName_cam[200] = "/home/jhz/liaoziwei/workspace/object-slam/ellipsoid-slam/core/test/debug-data/interiornet_poses.txt";
    char fileName_detect[200] = "/home/jhz/liaoziwei/workspace/object-slam/ellipsoid-slam/core/test/debug-data/interiornet_detections.txt";

    MatrixXd poses_mat = readDataFromFile(fileName_cam);
    MatrixXd detect_mat = readDataFromFile(fileName_detect);

    poses = poses_mat;
    detections = detect_mat;

    return true;
}

bool cameraPoseProcess(Eigen::MatrixXd &poses){
    // wxyz - > xyzw
    switchQuaternionOrder(poses);

    // 相机坐标系矫正
    calibrateCameraAxis(poses);

    // 将 Tcw转为 Twc
    // inverseEveryPose(poses);

    return true;
}

bool inverseEveryPose(Eigen::MatrixXd &poses){
    int rows = int(poses.rows());

    for(int i=0;i<rows;i++)
    {
        Vector7d v = poses.row(i).tail(7);
        g2o::SE3Quat Twc(v);
        g2o::SE3Quat Twc_trans = Twc.inverse();

        Vector7d v_done = Twc_trans.toVector();
        poses.row(i).tail(7) = v_done;
    }

    return true;
}

bool calibrateCameraAxis(Eigen::MatrixXd &poses){
    // 让其绕自身x轴旋转一个pi

    int rows = int(poses.rows());

    // 生成一个绕x轴转pi的变换.
    Vector6d v_trans;
    v_trans << 0,0,0,M_PI,0,0;
    g2o::SE3Quat tTrans;
    tTrans.fromXYZPRYVector(v_trans);

    for(int i=0;i<rows;i++)
    {
        Vector7d v = poses.row(i).tail(7);
        g2o::SE3Quat Twc(v);

        g2o::SE3Quat Twc_trans = Twc * tTrans;

        Vector7d v_done = Twc_trans.toVector();
        poses.row(i).tail(7) = v_done;

    }

    return true;

}

// 将 w x y z 的方式转为 x y z w 的方式.
bool switchQuaternionOrder(MatrixXd &poses)
{
    // timestamp x y z qw qx qy qz
    MatrixXd qRows = poses.block(0, 4, poses.rows(), 4);

    MatrixXd qRowsOrdered(qRows.rows(), qRows.cols());
    qRowsOrdered.col(0) = qRows.col(1);
    qRowsOrdered.col(1) = qRows.col(2);
    qRowsOrdered.col(2) = qRows.col(3);
    qRowsOrdered.col(3) = qRows.col(0);  // qw

    poses.block(0, 4, poses.rows(), 4) = qRowsOrdered;

    return true;

}

cv::Mat processDepth(cv::Mat& depth, double fx, double cx, double cy){

    cv::Mat output(depth.rows, depth.cols, CV_16UC1);

    for( int y = 0; y< output.rows; y++)
        for( int x = 0; x< output.cols; x++)
        {
            ushort d = depth.at<ushort>(y,x);
            double realz = calculateRealZ(fx, double(d), (x - cx), (y - cy));
            output.at<ushort>(y, x)  = ushort(realz);

        }

    return output;

}

double calculateRealZ(double f, double d, double xi, double yi){
    return f*sqrt(d*d/(xi*xi+f*f+yi*yi));
}

}


//==============================================================================
//          Copyright 2014    Jean-Thierry Lapreste
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_SWAR_FUNCTIONS_CUMMIN_HPP_INCLUDED
#define BOOST_SIMD_SWAR_FUNCTIONS_CUMMIN_HPP_INCLUDED
#include <boost/simd/include/functor.hpp>
#include <boost/dispatch/include/functor.hpp>
#include <boost/simd/include/functions/min.hpp>
#include <boost/simd/constant/constants/inf.hpp>

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

      Represents the cummin function in generic contexts.

      @par Models:
      Hierarchy
    **/
    struct cummin_ : ext::cumulative_<cummin_, tag::min_, tag::Inf>
    {
      /// @brief Parent hierarchy
      typedef ext::cumulative_<cummin_, tag::min_, tag::Inf> parent;
      template<class... Args>
      static BOOST_FORCEINLINE BOOST_AUTO_DECLTYPE dispatch(Args&&... args)
      BOOST_AUTO_DECLTYPE_BODY( dispatching_cummin_( ext::adl_helper(), static_cast<Args&&>(args)... ) )
    };
  }
  namespace ext
  {
    template<class Site>
    BOOST_FORCEINLINE generic_dispatcher<tag::cummin_, Site> dispatching_cummin_(adl_helper, boost::dispatch::meta::unknown_<Site>, ...)
    {
      return generic_dispatcher<tag::cummin_, Site>();
    }
    template<class... Args>
    struct impl_cummin_;
  }
  /*!
    @brief compute the cumulate minimum of the vector elements

    @par Semantic:

    For every parameter of type T0

    @code
    T0 r = cummin(a0);
    @endcode

    is similar to:

    @code
    T r =x;
    for(int i=0;i < T::static_size; ++i)
      r[i] += min(r[i-1], r[i]);
    @endcode

    @param a0

    @return a value of the same type as the second parameter
  **/
  BOOST_DISPATCH_FUNCTION_IMPLEMENTATION(tag::cummin_, cummin, 1)
  BOOST_DISPATCH_FUNCTION_IMPLEMENTATION(tag::cummin_, cummin, 2)
} }

#endif


//  (C) Copyright Matt Borland 2022.
//  Use, modification and distribution are subject to the
//  Boost Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

#include <cmath>
#include <cfloat>
#include <cstdint>
#include <limits>
#include <type_traits>
#include <boost/math/ccmath/islessequal.hpp>
#include <boost/math/ccmath/isnan.hpp>
#include <boost/math/ccmath/isinf.hpp>

#ifdef BOOST_HAS_FLOAT128
#include <boost/multiprecision/float128.hpp>
#endif

template <typename T>
constexpr void test()
{
    static_assert(!boost::math::ccmath::islessequal(std::numeric_limits<T>::quiet_NaN(), T(1)));
    static_assert(!boost::math::ccmath::islessequal(T(1), std::numeric_limits<T>::quiet_NaN()));
    static_assert(!boost::math::ccmath::islessequal(std::numeric_limits<T>::quiet_NaN(), std::numeric_limits<T>::quiet_NaN()));

    static_assert(!boost::math::ccmath::islessequal(T(2), T(1)));
    static_assert(boost::math::ccmath::islessequal(T(2), T(2)));
    static_assert(boost::math::ccmath::islessequal(T(1), T(2)));
}

#if !defined(BOOST_MATH_NO_CONSTEXPR_DETECTION) && !defined(BOOST_MATH_USING_BUILTIN_CONSTANT_P)
int main()
{
    test<float>();
    test<double>();

    #ifndef BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS
    test<long double>();
    #endif

    #ifdef BOOST_HAS_FLOAT128
    test<boost::multiprecision::float128>();
    #endif

    return 0;
}
#else
int main()
{
    return 0;
}
#endif


//////////////////////////////////////////////////////////////////////////////////////////////
/// \file PositionEvaluator.hpp
///
/// \author Kai van Es, ASRL
//////////////////////////////////////////////////////////////////////////////////////////////

#ifndef STEAM_POSITION_EVALUATOR_HPP
#define STEAM_POSITION_EVALUATOR_HPP

#include <Eigen/Core>

#include <steam/evaluator/blockauto/transform/TransformEvaluator.hpp>

namespace steam {
namespace se3 {

//////////////////////////////////////////////////////////////////////////////////////////////
/// \brief Evaluator for the position component of a transformation matrix
///
/// *Note that we fix MAX_STATE_DIM to 6. Typically the performance benefits of fixed size
///  matrices begin to die if larger than 6x6. Size 6 allows for transformation matrices
///  and 6D velocities. If you have a state larger than this, consider writing an
///  error evaluator that extends from ErrorEvaluatorX.
//////////////////////////////////////////////////////////////////////////////////////////////
class PositionEvaluator : public BlockAutomaticEvaluator<Eigen::Matrix<double, 3, 1>, 3, 6> {
public:

  /// Convenience typedefs
  typedef boost::shared_ptr<PositionEvaluator> Ptr;
  typedef boost::shared_ptr<const PositionEvaluator> ConstPtr;

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Constructor
  //////////////////////////////////////////////////////////////////////////////////////////////
  PositionEvaluator(const TransformEvaluator::ConstPtr &transform);

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Pseudo constructor - return a shared pointer to a new instance
  //////////////////////////////////////////////////////////////////////////////////////////////
  static Ptr MakeShared(const TransformEvaluator::ConstPtr &transform);

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Returns whether or not an evaluator contains unlocked state variables
  //////////////////////////////////////////////////////////////////////////////////////////////
  virtual bool isActive() const;

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Adds references (shared pointers) to active state variables to the map output
  //////////////////////////////////////////////////////////////////////////////////////////////
  virtual void getActiveStateVariables(
      std::map<unsigned int, steam::StateVariableBase::Ptr> *outStates) const;

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Evaluate the resultant 6x1 vector belonging to the se(3) algebra
  //////////////////////////////////////////////////////////////////////////////////////////////
  virtual Eigen::Matrix<double, 3, 1> evaluate() const;

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Evaluate the resultant 6x1 vector belonging to the se(3) algebra and
  ///        sub-tree of evaluations
  ///
  /// ** Note that the returned pointer belongs to the memory pool EvalTreeNode<TYPE>::pool,
  ///    and should be given back to the pool, rather than being deleted.
  //////////////////////////////////////////////////////////////////////////////////////////////
  virtual EvalTreeNode<Eigen::Matrix<double, 3, 1> > *evaluateTree() const;

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Evaluate the Jacobian tree
  //////////////////////////////////////////////////////////////////////////////////////////////
  virtual void appendBlockAutomaticJacobians(const Eigen::MatrixXd &lhs,
                                             EvalTreeNode<Eigen::Matrix<double, 3, 1> > *evaluationTree,
                                             std::vector<Jacobian<> > *outJacobians) const;

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Fixed-size evaluations of the Jacobian tree
  //////////////////////////////////////////////////////////////////////////////////////////////
  virtual void appendBlockAutomaticJacobians(const Eigen::Matrix<double, 1, 3> &lhs,
                                             EvalTreeNode<Eigen::Matrix<double, 3, 1> > *evaluationTree,
                                             std::vector<Jacobian<1, 6> > *outJacobians) const;

  virtual void appendBlockAutomaticJacobians(const Eigen::Matrix<double, 2, 3> &lhs,
                                             EvalTreeNode<Eigen::Matrix<double, 3, 1> > *evaluationTree,
                                             std::vector<Jacobian<2, 6> > *outJacobians) const;

  virtual void appendBlockAutomaticJacobians(const Eigen::Matrix<double, 3, 3> &lhs,
                                             EvalTreeNode<Eigen::Matrix<double, 3, 1> > *evaluationTree,
                                             std::vector<Jacobian<3, 6> > *outJacobians) const;

  virtual void appendBlockAutomaticJacobians(const Eigen::Matrix<double, 4, 3> &lhs,
                                             EvalTreeNode<Eigen::Matrix<double, 3, 1> > *evaluationTree,
                                             std::vector<Jacobian<4, 6> > *outJacobians) const;

  virtual void appendBlockAutomaticJacobians(const Eigen::Matrix<double, 6, 3> &lhs,
                                             EvalTreeNode<Eigen::Matrix<double, 3, 1> > *evaluationTree,
                                             std::vector<Jacobian<6, 6> > *outJacobians) const;

private:

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Implementation for Block Automatic Differentiation
  //////////////////////////////////////////////////////////////////////////////////////////////
  template<int LHS_DIM, int INNER_DIM, int MAX_STATE_SIZE>
  void appendJacobiansImpl(const Eigen::Matrix<double, LHS_DIM, INNER_DIM> &lhs,
                           EvalTreeNode<Eigen::Matrix<double, 3, 1> > *evaluationTree,
                           std::vector<Jacobian<LHS_DIM, MAX_STATE_SIZE> > *outJacobians) const;

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Transform evaluator
  //////////////////////////////////////////////////////////////////////////////////////////////
  TransformEvaluator::ConstPtr transform_;

};

} // se3
} // steam

#endif //STEAM_POSITION_EVALUATOR_HPP


/****************************************************************************
 * Copyright (c) 2017-2021 by the ArborX authors                            *
 * All rights reserved.                                                     *
 *                                                                          *
 * This file is part of the ArborX library. ArborX is                       *
 * distributed under a BSD 3-clause license. For the licensing terms see    *
 * the LICENSE file in the top-level directory.                             *
 *                                                                          *
 * SPDX-License-Identifier: BSD-3-Clause                                    *
 ****************************************************************************/
#include <ArborX_Box.hpp>
#include <ArborX_Ray.hpp>

#include <boost/test/unit_test.hpp>

BOOST_AUTO_TEST_CASE(intersects_box)
{
  using ArborX::Box;
  using ArborX::Point;
  using ArborX::Experimental::Ray;

  Box unit_box{{0, 0, 0}, {1, 1, 1}};

  // origin is within the box
  BOOST_TEST(intersects(Ray{{.5, .5, .5}, {1, 0, 0}}, unit_box));
  BOOST_TEST(intersects(Ray{{.5, .5, .5}, {0, 1, 0}}, unit_box));
  BOOST_TEST(intersects(Ray{{.5, .5, .5}, {0, 0, 1}}, unit_box));
  BOOST_TEST(intersects(Ray{{.5, .5, .5}, {1, 1, 0}}, unit_box));
  BOOST_TEST(intersects(Ray{{.5, .5, .5}, {1, 1, 1}}, unit_box));

  // origin is outside the box
  // hit the center of the face
  BOOST_TEST(intersects(Ray{{-1, .5, .5}, {1, 0, 0}}, unit_box));
  BOOST_TEST(!intersects(Ray{{-1, .5, .5}, {-1, 0, 0}}, unit_box));
  BOOST_TEST(!intersects(Ray{{-1, .5, .5}, {0, 1, 1}}, unit_box));
  BOOST_TEST(intersects(Ray{{2, .5, .5}, {-1, 0, 0}}, unit_box));
  BOOST_TEST(!intersects(Ray{{2, .5, .5}, {1, 0, 0}}, unit_box));
  BOOST_TEST(!intersects(Ray{{2, .5, .5}, {0, 1, 1}}, unit_box));
  BOOST_TEST(intersects(Ray{{-1, 1.5, .5}, {1, -1, 0}}, unit_box));
  BOOST_TEST(!intersects(Ray{{-1, 1.5, .5}, {1, 0, 0}}, unit_box));
  BOOST_TEST(intersects(Ray{{-1, 1.5, 1.5}, {1, -1, -1}}, unit_box));
  BOOST_TEST(!intersects(Ray{{-1, 1.5, 1.5}, {1, 0, 0}}, unit_box));
  BOOST_TEST(!intersects(Ray{{-1, 1.5, 1.5}, {1, -1, 0}}, unit_box));
  BOOST_TEST(!intersects(Ray{{-1, 1.5, 1.5}, {1, 0, -1}}, unit_box));
  BOOST_TEST(intersects(Ray{{2, -.5, .5}, {-1, 1, 0}}, unit_box));
  BOOST_TEST(!intersects(Ray{{2, -.5, .5}, {-1, 0, 0}}, unit_box));
  BOOST_TEST(intersects(Ray{{2, -.5, 1.5}, {-1, 1, -1}}, unit_box));
  BOOST_TEST(!intersects(Ray{{2, -.5, 1.5}, {-1, 0, 0}}, unit_box));
  BOOST_TEST(!intersects(Ray{{2, -.5, 1.5}, {-1, 1, 0}}, unit_box));
  BOOST_TEST(!intersects(Ray{{2, -.5, 1.5}, {-1, 0, -1}}, unit_box));

  // hit the center of an edge
  BOOST_TEST(intersects(Ray{{-1, 0, .5}, {1, 0, 0}}, unit_box));
  BOOST_TEST(!intersects(Ray{{-1, 0, .5}, {-1, 0, 0}}, unit_box));
  BOOST_TEST(!intersects(Ray{{-1, 0, .5}, {0, 1, 1}}, unit_box));
  BOOST_TEST(intersects(Ray{{2, 0, .5}, {-1, 0, 0}}, unit_box));
  BOOST_TEST(!intersects(Ray{{2, 0, .5}, {1, 0, 0}}, unit_box));
  BOOST_TEST(!intersects(Ray{{2, 0, .5}, {0, 1, 1}}, unit_box));
  BOOST_TEST(intersects(Ray{{-1, -1, .5}, {1, 1, 0}}, unit_box));
  BOOST_TEST(intersects(Ray{{-1, 1, .5}, {1, -1, 0}}, unit_box));
  BOOST_TEST(intersects(Ray{{-1, -2, .5}, {1, 2, 0}}, unit_box));
  BOOST_TEST(intersects(Ray{{2, 2, .5}, {-1, -1, 0}}, unit_box));
  BOOST_TEST(intersects(Ray{{2, -1, .5}, {-1, 1, 0}}, unit_box));
  BOOST_TEST(intersects(Ray{{2, 1, .5}, {-1, -1, 0}}, unit_box));

  // hit a corner
  BOOST_TEST(intersects(Ray{{-0.5, 1.5, 1.5}, {1, -1, -1}}, unit_box));
  BOOST_TEST(!intersects(Ray{{-1, 1, 1}, {-1, 0, 0}}, unit_box));
  BOOST_TEST(!intersects(Ray{{-1, 1, 1}, {0, 1, 1}}, unit_box));
  BOOST_TEST(!intersects(Ray{{2, 1, 1}, {1, 0, 0}}, unit_box));
  BOOST_TEST(!intersects(Ray{{2, 1, 1}, {0, 1, 1}}, unit_box));
  BOOST_TEST(intersects(Ray{{-1, -1, -1}, {1, 1, 1}}, unit_box));
  BOOST_TEST(intersects(Ray{{2, 2, 2}, {-1, -1, -1}}, unit_box));

  BOOST_TEST(!intersects(Ray{{1, 2, 3}, {4, 5, 6}}, unit_box));
  BOOST_TEST(intersects(Ray{{1, 2, 3}, {-1, -2, -3}}, unit_box));

  // origin is on the box (no 0*inf).
  BOOST_TEST(intersects(Ray{{0, 0, 0}, {1, 1, 1}}, unit_box));
  BOOST_TEST(intersects(Ray{{0, 0, 0}, {-1, -1, -1}}, unit_box));
  BOOST_TEST(intersects(Ray{{1, 1, 1}, {-1, -1, -1}}, unit_box));
  BOOST_TEST(intersects(Ray{{1, 1, 1}, {1, 1, 1}}, unit_box));

  BOOST_TEST(intersects(Ray{{1, .5, .5}, {-1, 0, 0}}, unit_box));
  BOOST_TEST(intersects(Ray{{1, .5, .5}, {1, 0, 0}}, unit_box));
  BOOST_TEST(intersects(Ray{{.5, 1, .5}, {0, -1, 0}}, unit_box));
  BOOST_TEST(intersects(Ray{{.5, 1, .5}, {0, 1, 0}}, unit_box));
  BOOST_TEST(intersects(Ray{{.5, .5, 1}, {0, 0, -1}}, unit_box));
  BOOST_TEST(intersects(Ray{{.5, .5, 1}, {0, 0, 1}}, unit_box));

  BOOST_TEST(intersects(Ray{{0, 0, .5}, {1, 1, 0}}, unit_box));
  BOOST_TEST(intersects(Ray{{0, 0, .5}, {-1, -1, 0}}, unit_box));
  BOOST_TEST(intersects(Ray{{1, 1, .5}, {-1, -1, 0}}, unit_box));
  BOOST_TEST(intersects(Ray{{1, 1, .5}, {1, 1, 0}}, unit_box));

  BOOST_TEST(intersects(Ray{{0, 0.5, 0}, {-1, -2, 0}}, unit_box));
  BOOST_TEST(intersects(Ray{{0.5, 0, 0.5}, {2, 0, -1}}, unit_box));

  // origin is on the box (with 0*inf).
  BOOST_TEST(intersects(Ray{{0, 0, 0}, {0, 1, -1}}, unit_box));
  BOOST_TEST(intersects(Ray{{0, 0, 0}, {-1, 0, -1}}, unit_box));
  BOOST_TEST(intersects(Ray{{0, 0, 0}, {-1, 1, 0}}, unit_box));
  BOOST_TEST(intersects(Ray{{0, 0, 0}, {0, 0, 1}}, unit_box));
  BOOST_TEST(intersects(Ray{{0, 0, 0}, {-1, 0, 0}}, unit_box));
  BOOST_TEST(intersects(Ray{{0, 0, 0}, {0, -1, 0}}, unit_box));

  BOOST_TEST(intersects(Ray{{1, 1, 1}, {0, -1, -1}}, unit_box));
  BOOST_TEST(intersects(Ray{{1, 1, 1}, {1, 0, 1}}, unit_box));
  BOOST_TEST(intersects(Ray{{1, 1, 1}, {-1, -1, 0}}, unit_box));
  BOOST_TEST(intersects(Ray{{1, 1, 1}, {1, 0, 0}}, unit_box));
  BOOST_TEST(intersects(Ray{{1, 1, 1}, {0, 1, 0}}, unit_box));
  BOOST_TEST(intersects(Ray{{1, 1, 1}, {0, 0, -1}}, unit_box));

  BOOST_TEST(intersects(Ray{{0, 1, 1}, {0, 0, 1}}, unit_box));
  BOOST_TEST(intersects(Ray{{1, 0, 1}, {1, 0, 0}}, unit_box));
  BOOST_TEST(intersects(Ray{{1, 1, 0}, {0, 1, 0}}, unit_box));

  BOOST_TEST(intersects(Ray{{0, 0, 1}, {-1, 0, -1}}, unit_box));
  BOOST_TEST(intersects(Ray{{0, 0, 1}, {0, 0, 1}}, unit_box));
  BOOST_TEST(intersects(Ray{{1, 0, 0}, {0, 1, 0}}, unit_box));
  BOOST_TEST(intersects(Ray{{1, 0, 0}, {-1, 0, 0}}, unit_box));
  BOOST_TEST(intersects(Ray{{0, 1, 0}, {0, 0, 1}}, unit_box));
  BOOST_TEST(intersects(Ray{{0, 1, 0}, {0, 0, -1}}, unit_box));

  // more cases with 0*inf:
  BOOST_TEST(intersects(Ray{{0, 1.5, 1.5}, {0, -1, -1}}, unit_box));
  BOOST_TEST(!intersects(Ray{{0, 1.5, 1.5}, {0, 1, 1}}, unit_box));
  BOOST_TEST(intersects(Ray{{1.5, 0, 1.5}, {-1, 0, -1}}, unit_box));
  BOOST_TEST(!intersects(Ray{{1.5, 0, 1.5}, {1, 0, 1}}, unit_box));
  BOOST_TEST(intersects(Ray{{1.5, 1.5, 0}, {-1, -1, 0}}, unit_box));
  BOOST_TEST(!intersects(Ray{{1.5, 1.5, 0}, {1, 1, 0}}, unit_box));

  BOOST_TEST(intersects(Ray{{1, 1.5, 1.5}, {0, -1, -1}}, unit_box));
  BOOST_TEST(!intersects(Ray{{1, 1.5, 1.5}, {0, 1, 1}}, unit_box));
  BOOST_TEST(intersects(Ray{{1.5, 1, 1.5}, {-1, 0, -1}}, unit_box));
  BOOST_TEST(!intersects(Ray{{1.5, 1, 1.5}, {1, 0, 1}}, unit_box));
  BOOST_TEST(intersects(Ray{{1.5, 1.5, 1}, {-1, -1, 0}}, unit_box));
  BOOST_TEST(!intersects(Ray{{1.5, 1.5, 1}, {1, 1, 0}}, unit_box));

  BOOST_TEST(intersects(Ray{{1.5, 0, 0}, {-1, 0, 2}}, unit_box));
  BOOST_TEST(intersects(Ray{{1.5, 0, 0}, {-1, 2, 0}}, unit_box));
  BOOST_TEST(!intersects(Ray{{1.5, 0, 0}, {-1, 0, 2.1}}, unit_box));
  BOOST_TEST(!intersects(Ray{{1.5, 0, 0}, {-1, 2.1, 0}}, unit_box));
  BOOST_TEST(intersects(Ray{{1.5, 1, 1}, {-1, 0, -2}}, unit_box));
  BOOST_TEST(intersects(Ray{{1.5, 1, 1}, {-1, -2, 0}}, unit_box));
  BOOST_TEST(!intersects(Ray{{1.5, 1, 1}, {-1, 0, -2.1}}, unit_box));
  BOOST_TEST(!intersects(Ray{{1.5, 1, 1}, {-1, -2.1, 0}}, unit_box));

  BOOST_TEST(intersects(Ray{{0, 1.5, 0}, {0, -1, 2}}, unit_box));
  BOOST_TEST(intersects(Ray{{0, 1.5, 0}, {2, -1, 0}}, unit_box));
  BOOST_TEST(!intersects(Ray{{0, 1.5, 0}, {0, -1, 2.1}}, unit_box));
  BOOST_TEST(!intersects(Ray{{0, 1.5, 0}, {2.1, -1, 0}}, unit_box));
  BOOST_TEST(intersects(Ray{{1, 1.5, 1}, {0, -1, -2}}, unit_box));
  BOOST_TEST(intersects(Ray{{1, 1.5, 1}, {-2, -1, 0}}, unit_box));
  BOOST_TEST(!intersects(Ray{{1, 1.5, 1}, {0, -1, -2.1}}, unit_box));
  BOOST_TEST(!intersects(Ray{{1, 1.5, 1}, {-2.1, -1, 0}}, unit_box));
}

// NOTE until boost 1.70 need to cast both operands when comparing floating
// points
BOOST_AUTO_TEST_CASE(overlap_distance_sphere,
                     *boost::unit_test::tolerance(1e-6f))
{
  using ArborX::Sphere;
  using ArborX::Experimental::Ray;
  constexpr Sphere unit_sphere{{0, 0, 0}, 1};

  auto const sqrtf_3 = std::sqrt(3.f);
  auto const half_sqrtf_2 = std::sqrt(2.f) / 2;
  auto const half_sqrtf_3 = std::sqrt(3.f) / 2;

  // hit center of the sphere
  BOOST_TEST(overlapDistance(Ray{{-2, 0, 0}, {1, 0, 0}}, unit_sphere) == 2.f);
  BOOST_TEST(overlapDistance(Ray{{0, 3, 0}, {0, -1, 0}}, unit_sphere) == 2.f);
  BOOST_TEST(overlapDistance(Ray{{0, 0, -4}, {0, 0, 1}}, unit_sphere) == 2.f);
  BOOST_TEST(overlapDistance(Ray{{-2, -2, -2}, {1, 1, 1}}, unit_sphere) == 2.f);
  // miss it
  BOOST_TEST(overlapDistance(Ray{{-2, -2, -2}, {1, 0, 0}}, unit_sphere) == 0.f);
  BOOST_TEST(overlapDistance(Ray{{1, 2, 3}, {4, 5, 6}}, unit_sphere) == 0.f);
  // half-radius
  BOOST_TEST(overlapDistance(Ray{{-2, 0, 0.5}, {1, 0, 0}}, unit_sphere) ==
             sqrtf_3);
  BOOST_TEST(overlapDistance(Ray{{0, -0.5, 2}, {0, 0, -1}}, unit_sphere) ==
             sqrtf_3);
  // touch surface but no intersection
  BOOST_TEST(overlapDistance(Ray{{1, -2, 0}, {0, 1, 0}}, unit_sphere) == 0.f);
  BOOST_TEST(overlapDistance(Ray{{2, 2, -1}, {-1, -1, 0}}, unit_sphere) == 0.f);

  // behind the origin
  BOOST_TEST(overlapDistance(Ray{{-2, 0, 0}, {-1, 0, 0}}, unit_sphere) == 0.f);
  BOOST_TEST(overlapDistance(Ray{{0, 2, 2}, {0, 1, 1}}, unit_sphere) == 0.f);
  BOOST_TEST(overlapDistance(Ray{{0, 2, 2}, {0, 1, 1}}, unit_sphere) == 0.f);

  // origin inside
  BOOST_TEST(overlapDistance(Ray{{0.5, 0, 0}, {0, 1, 1}}, unit_sphere) ==
             half_sqrtf_3);
  BOOST_TEST(overlapDistance(Ray{{0, -half_sqrtf_3, 0}, {0, 0, 1}},
                             unit_sphere) == 0.5f);
  BOOST_TEST(overlapDistance(Ray{{0, 0, half_sqrtf_2}, {1, -1, 0}},
                             unit_sphere) == half_sqrtf_2);
  BOOST_TEST(overlapDistance(Ray{{0, 0.6, 0}, {0, 1, 0}}, unit_sphere) == 0.4f);
  BOOST_TEST(overlapDistance(Ray{{0, 0, 0}, {1, 2, 3}}, unit_sphere) == 1.f);

  // origin on surface
  //   tangent
  BOOST_TEST(overlapDistance(Ray{{0, 0, 1}, {1, -1, 0}}, unit_sphere) == 0.f);
  BOOST_TEST(overlapDistance(Ray{{0, 1, 0}, {2, 0, 3}}, unit_sphere) == 0.f);
  BOOST_TEST(
      overlapDistance(Ray{{half_sqrtf_2, half_sqrtf_2, 0}, {1, -1, 0}},
                      unit_sphere) == 0.f,
      boost::test_tools::tolerance(5e-4f)); // inside at machine precision...
  BOOST_TEST(overlapDistance(Ray{{-half_sqrtf_3, 0.5, 0}, {1, 2, 0}},
                             unit_sphere) == 0.f);
  BOOST_TEST(overlapDistance(Ray{{0.5, half_sqrtf_3, 0}, {2, -1, 0}},
                             unit_sphere) == 0.f);
  //   directed at sphere
  BOOST_TEST(overlapDistance(Ray{{1, 0, 0}, {-2, 0, 0}}, unit_sphere) == 2.f);
  BOOST_TEST(overlapDistance(Ray{{1, 0, 0}, {-2, 0, 0}}, unit_sphere) == 2.f);
  BOOST_TEST(overlapDistance(Ray{{0, -half_sqrtf_2, half_sqrtf_2}, {0, 1, -1}},
                             unit_sphere) == 2.f);
  BOOST_TEST(overlapDistance(Ray{{half_sqrtf_3, 0.5, 0}, {-1, 0, 0}},
                             unit_sphere) == sqrtf_3);
  //   directed away
  BOOST_TEST(overlapDistance(Ray{{0, 0, 1}, {1, 1, 1}}, unit_sphere) == 0.f);
  BOOST_TEST(overlapDistance(Ray{{half_sqrtf_3, 0.5, 0}, {1, 0, 0}},
                             unit_sphere) == 0.f);
}

#define STATIC_ASSERT(cond) static_assert(cond, "");
BOOST_AUTO_TEST_CASE(make_euclidean_vector)
{
  using ArborX::Experimental::makeVector;
  using ArborX::Experimental::Vector;
  STATIC_ASSERT((makeVector({0, 0, 0}, {1, 2, 3}) == Vector{1, 2, 3}));
  STATIC_ASSERT((makeVector({1, 2, 3}, {4, 5, 6}) == Vector{3, 3, 3}));
}

BOOST_AUTO_TEST_CASE(dot_product)
{
  using ArborX::Experimental::dotProduct;
  STATIC_ASSERT(dotProduct({1, 0, 0}, {1, 0, 0}) == 1);
  STATIC_ASSERT(dotProduct({1, 0, 0}, {0, 1, 0}) == 0);
  STATIC_ASSERT(dotProduct({1, 0, 0}, {0, 0, 1}) == 0);
  STATIC_ASSERT(dotProduct({1, 1, 1}, {1, 1, 1}) == 3);
}

BOOST_AUTO_TEST_CASE(cross_product)
{
  using ArborX::Experimental::crossProduct;
  using ArborX::Experimental::Vector;
  STATIC_ASSERT((crossProduct({1, 0, 0}, {1, 0, 0}) == Vector{0, 0, 0}));
  STATIC_ASSERT((crossProduct({1, 0, 0}, {0, 1, 0}) == Vector{0, 0, 1}));
  STATIC_ASSERT((crossProduct({1, 0, 0}, {0, 0, 1}) == Vector{0, -1, 0}));
  STATIC_ASSERT((crossProduct({0, 1, 0}, {1, 0, 0}) == Vector{0, 0, -1}));
  STATIC_ASSERT((crossProduct({0, 1, 0}, {0, 1, 0}) == Vector{0, 0, 0}));
  STATIC_ASSERT((crossProduct({0, 1, 0}, {0, 0, 1}) == Vector{1, 0, 0}));
  STATIC_ASSERT((crossProduct({1, 1, 1}, {1, 1, 1}) == Vector{0, 0, 0}));
}
#undef STATIC_ASSERT


#include "asset_cache.h"
#include <Eigen/Eigen>
#include <catch2/catch.hpp>
#include <entt/entity/registry.hpp>
#include <lmlib/math_constants.h>
#include <lmng/archetype.h>
#include <lmng/hierarchy.h>
#include <lmng/logging.h>
#include <lmng/name.h>
#include <lmng/transform.h>
#include <lmng/yaml_save_load.h>

lmng::archetype_data create_test_archetype()
{
    lmng::archetype_data archetype_data;

    YAML::Node components_yaml;

    YAML::Node transform_yaml;

    transform_yaml["Position"] = "1.0 2.0 3.0";
    transform_yaml["Rotation"] = "0.0 0.0 0.0 1.0";

    components_yaml["Transform"] = transform_yaml;

    YAML::Node children_yaml;

    YAML::Node child_yaml;

    YAML::Node child_components_yaml;
    YAML::Node child_children_yaml;

    child_yaml["components"] = child_components_yaml;
    child_yaml["children"] = child_children_yaml;

    children_yaml["Child"] = child_yaml;

    archetype_data.yaml["components"] = components_yaml;
    archetype_data.yaml["children"] = children_yaml;
    return archetype_data;
}

TEST_CASE("Map serialisation with archetypes")
{
    test_asset_cache asset_cache;

    asset_cache.emplace<lmng::archetype_data>(
      "archetype", create_test_archetype());

    entt::registry registry;
    lmng::hierarchy_system hierarchy_system{registry};

    auto parent = registry.create();
    auto child = registry.create();

    registry.emplace<lmng::parent>(child, parent);
    registry.emplace<lmng::name>(parent, "Parent");
    registry.emplace<lmng::transform>(
      parent, Eigen::Vector3f{1.f, 2.f, 3.f}, Eigen::Quaternionf::Identity());
    registry.emplace<lmng::name>(child, "Child");
    registry.emplace<lmng::archetype>(parent, "archetype");

    YAML::Node expected_yaml, expected_parent_yaml, expected_components_yaml,
      expected_transform_yaml;

    expected_transform_yaml["Rotation"] = "0 0 0 1";
    expected_transform_yaml["Position"] = "1 2 3";

    expected_components_yaml["Transform"] = expected_transform_yaml;

    expected_parent_yaml["archetype"] = "archetype";
    expected_parent_yaml["components"] = expected_components_yaml;
    expected_parent_yaml["children"] = YAML::Node{};

    expected_yaml["Parent"] = expected_parent_yaml;

    YAML::Node actual_yaml;
    lmng::save_registry_as_yaml(registry, asset_cache, actual_yaml);

    std::ostringstream expected_str{}, actual_str{};
    expected_str << expected_yaml;
    actual_str << actual_yaml["entities"];

    REQUIRE(actual_str.str() == expected_str.str());
}

TEST_CASE("Map deserialisation with archetypes")
{
    test_asset_cache asset_cache;

    asset_cache.emplace<lmng::archetype_data>(
      "archetype", create_test_archetype());

    YAML::Node map_yaml;

    YAML::Node parent_yaml;
    parent_yaml["archetype"] = "archetype";

    YAML::Node parent_transform_yaml;

    parent_transform_yaml["Position"] = "4.0 5.0 6.0";

    YAML::Node parent_components_yaml;
    parent_components_yaml["Transform"] = parent_transform_yaml;

    parent_yaml["components"] = parent_components_yaml;
    parent_yaml["children"] = YAML::Node{};

    map_yaml["entities"]["Parent"] = parent_yaml;
    map_yaml["schema_version"] = "1";

    entt::registry registry;
    lmng::hierarchy_system hierarchy_system{registry};
    lmng::connect_component_logging(registry);

    lmng::populate_registry_from_yaml(map_yaml, registry, asset_cache);

    auto parent = lmng::find_entity(registry, "Parent");

    REQUIRE((parent != entt::null) == true);

    auto child = lmng::find_entity(registry, "Child");

    REQUIRE((child != entt::null) == true);

    auto parent_transform = registry.get<lmng::transform>(parent);

    REQUIRE(parent_transform.position == Eigen::Vector3f{4.f, 5.f, 6.f});
}