#ifndef _OPENCV_DMP_HPP_
#define _OPENCV_DMP_HPP_

#include <vector>
#include <opencv2/core/core.hpp>
#include <boost/filesystem.hpp>

#include <limits>
#include <map>
#include <set>
#include <list>
#include <vector>
#include <stdexcept>

#include <opencv2/imgproc/imgproc.hpp>

//for DEBUG
//#include <opencv2/highgui/highgui.hpp>

///
/// Generate a opening DMP profile
/// \param inputImage The image that will be filtered for light objects
/// \param ses The set of strictly increasing geodesic structuring elements
///
template <typename OperatingPixelType>
std::vector<cv::Mat> opencvOpenDmp(const cv::Mat& inputImage, const std::vector<int>& ses);

///
/// Dilation by Reconstruction using the Downhill Filter algorithm (Robinson,Whelan)
///
/// \param mask The image/surface that bounds the reconstruction of the marker
/// \param mark The image/surface that initializes the reconstruction
/// \returns The dilation reconstruction
template <typename OperatingPixelType>
cv::Mat dilationByReconstructionDownhill(const cv::Mat& mask, const cv::Mat& mark);

///
/// Generate a closing DMP profile
/// \param inputImage The image that will be filtered for dark objects
/// \param ses The set of strictly increasing geodesic structuring elements
///
template <typename OperatingPixelType>
std::vector<cv::Mat> opencvCloseDmp(const cv::Mat& inputImage, const std::vector<int>& ses);

///
/// Erosion by Reconstruction using an Uphill Filter algorithm (insp. by Robinson,Whelan)
///
/// \param mask The image/surface that bounds the reconstruction of the marker
/// \param mark The image/surface that initializes the reconstruction
/// \returns The erosion reconstruction
template <typename OperatingPixelType>
cv::Mat erosionByReconstructionUphill(const cv::Mat& mask, const cv::Mat& mark);

/// Dump the pyramid levels
/// \param dmp the pyramid cells to dump
/// \param sideTag the Open or Close identifier for the levels
/// \param outputPath the output directory
/// \param filePath the filename being processed
void dumpDmpLevels(const std::vector<cv::Mat>& dmp, const std::vector<int>& ses, const std::string& sideTag,
			const boost::filesystem::path& outputPath,
			const boost::filesystem::path& filePath);

// ============================
// ============================
// == BEGIN Template Functions
// ============================
// ============================

template <typename OperatingPixelType>
std::vector<cv::Mat> opencvOpenDmp(const cv::Mat& inputImage, const std::vector<int>& ses)
{
	//cgi::log::MetaLogStream& log(cgi::log::MetaLogStream::instance());
	// ++ Open, then dilate by reconstruction

	//if (false == std::numeric_limits<T>::is_integer)
	//	CGI_THROW(std::logic_error,"Invalid template type");

	// Build Structuring Elements
	std::vector<cv::Mat> structuringElements;
	for (std::vector<int>::const_iterator level = ses.begin();level!=ses.end();++level)
	{
		const int lev = *level;
		// Circular structuring element
		const int edgeSize = lev+1+lev; // turn radius into edge size
		structuringElements.push_back(
					cv::getStructuringElement(cv::MORPH_ELLIPSE,cv::Size(edgeSize,edgeSize))
					);
	}

	// Build Morphological Operation Stack (open)
	std::vector<cv::Mat> mos;
	for (unsigned int level = 0;level<structuringElements.size();++level)
	{
		cv::Mat filteredImage(inputImage.size().height, inputImage.size().width, cv::DataType<OperatingPixelType>::type);
		cv::morphologyEx(inputImage, filteredImage, cv::MORPH_OPEN, structuringElements.at(level));
		mos.push_back(filteredImage);
	}

	const int n_lvls = mos.size();
//	log << cgi::log::Priority::INFO << "opencvOpenDmp" << "MOS Produced, size:" << n_lvls << cgi::log::flush;

	// Set the Tile band count to the SEs array size
	std::vector<cv::Mat> profile;
	profile.reserve(n_lvls);

	// ////////////////////////////////////////////////////////////////////////
	// USE: dilationByReconstructionDownhill(const cv::Mat& mask,const cv::Mat& mark)

	cv::Mat tempRec;			// Temporary Open Record
	cv::Mat lastRec;			// previous level Open Record

	// Open = (erode,dilate)
	//typename MorphologyOperator<DataType>::type morphology(_ses[0]);
	//typename ReconstructionOperator<DataType>::type reconstructor;

	//dilationByReconstructionDownhill( inputImage, mos.at(0) ).copyTo(tempRec);
	tempRec = dilationByReconstructionDownhill<OperatingPixelType>( inputImage, mos.at(0) ).clone();

	// get the absolute difference of the two images
	cv::Mat diffImage(inputImage.size().height, inputImage.size().width, cv::DataType<OperatingPixelType>::type);
	absdiff(inputImage, tempRec, diffImage);

	profile.push_back(diffImage.clone());
	tempRec.copyTo(lastRec);

	// Subsequent Levels
	for (int i = 1;i < n_lvls;++i)
	{
		//  reconstruction(mask     ,  mark)
		//dilationByReconstructionDownhill( inputImage, mos.at(i) ).copyTo(tempRec);
		tempRec = dilationByReconstructionDownhill<OperatingPixelType>( inputImage, mos.at(i) ).clone();

		// BEGIN DEBUG
		/*
		std::stringstream ss;
		ss << "reconstruction_from_opening_" << i << ".png";
		if(!cv::imwrite(ss.str(), tempRec))
		  {
		    throw std::runtime_error("Failed to write");
		  }
		*/
		// END DEBUG

		// get the absolute difference of the two images
		absdiff(lastRec, tempRec, diffImage);
		profile.push_back(diffImage.clone());

		tempRec.copyTo(lastRec);

	}

	return profile;
}

template <typename OperatingPixelType>
cv::Mat dilationByReconstructionDownhill(const cv::Mat& mask, const cv::Mat& mark)
{
	//typedef unsigned short OperatingPixelType;
	//cgi::log::MetaLogStream& log(cgi::log::MetaLogStream::instance());

	if (false == std::numeric_limits<OperatingPixelType>::is_integer)
		//CGI_THROW(std::logic_error,"Invalid template / operating-pixel type");
		throw new std::logic_error("Invalid template / operating-pixel type");

	const cv::Size size = mask.size();

	//if (size.width != mark.size().width || size.height != mark.size().height)
	if (size != mark.size())
	{
		//log << cgi::log::Priority::ERROR << "dilationByReconstructionUphill"
		//<< "Size mismatch: ("
		//<< size.width << "," << size.height << ") != ("
		//<< mark.size().width << "," << mark.size().height << ")" << cgi::log::flush;
	}

	// --------------------------------
	// Place each p : I(p) > 0 into List[I(p)]
	// --------------------------------
	// Copy the marker into the reconstruction
	//	we can then grow it in place
	// value i has a list of pixels at position (y,x)
	cv::Mat reconstruction(mark.size().height, mark.size().width, cv::DataType<OperatingPixelType>::type);
	mark.copyTo(reconstruction);

	// for template, use typename
	//	typedef typename std::list<std::pair<int,int> > LocationListType;
	//	typedef typename std::map< OperatingPixelType, LocationListType > MapOfLocationListType;
	typedef typename std::list<std::pair<int,int> > LocationListType;
	typedef typename std::map< OperatingPixelType, LocationListType > MapOfLocationListType;

	// This is essentially an indexed image version of the input `mark' raster
	MapOfLocationListType valueLocationListMap;

	// Build an in-memory indexed image
	// but while we are at it, verify that it is properly bounded by `mask' raster
	for (int i = 0; i < size.height; ++i)
		for (int j = 0; j < size.width; ++j)
		{
			const OperatingPixelType pixelValue =  reconstruction.at<OperatingPixelType>(i, j);
			if (pixelValue > mask.at<OperatingPixelType>(i, j))
			{
				//std::cout << "Storing ("<< i << "," << j <<") into valueLocationListMap["<< static_cast<int>(pixelValue) << "]" << std::endl;
				std::stringstream msg;
				msg << "(pixelValue > mask[0](i, j)), for raster location = ("<<i<<","<<j<<")";
				//CGI_THROW (std::logic_error, msg.str()); // the marker must always be LTE the mask, by definition
				throw new std::logic_error( msg.str());
			}

			valueLocationListMap[pixelValue].push_back( std::make_pair<int,int>(i,j) );
		}

	// No valid pixels or values above floor?  Return the input as output
	if (valueLocationListMap.size() == 0)
		return reconstruction;

	// --------------------------------
	// The farthest downhill we will go
	//	is to the current min(mark[0])
	//	therefore, we do not need to
	//	process the current min, we
	//	already copied them to output
	const OperatingPixelType minValue =  valueLocationListMap.begin()->first;
	valueLocationListMap[minValue].clear();

	std::set<std::pair<int,int> > finalizedPixels;

	// --------------------------------
	// Do a backward iteration (downhill)
	// through the
	//	valueLocationListMap[values]
	// --------------------------------
	// use typename for template version
	//for (typename MapOfLocationListType::reverse_iterator valueList = valueLocationListMap.rbegin();
	//			valueList != valueLocationListMap.rend(); ++valueList)
	for (typename MapOfLocationListType::reverse_iterator valueList = valueLocationListMap.rbegin();
					valueList != valueLocationListMap.rend(); ++valueList)
	{
		// The gray value
		const OperatingPixelType currentValue = valueList->first;
		// The list of (y,x) tuples
		LocationListType locations = valueList->second;

		// for each location indexed in the value list
		while (!locations.empty())
		{

			// Debug thing
//			if (locations.size() > (size.width * size.height))
//			{
//				CGI_THROW(std::logic_error, "GrayscaleDilationByReconstructionDownhill has gone out of control, this should never happen!");
//			}

			// pull/pop the first position from the list, mark it as finalized
			std::pair<int,int> frontPosition = locations.front();
			finalizedPixels.insert(frontPosition);
			locations.pop_front();

			const int y = frontPosition.first;
			const int x = frontPosition.second;

			const int pre_x = x - 1;
			const int post_x = x + 1;
			const int pre_y = y - 1;
			const int post_y = y + 1;

			// For each neighbor
			// - a -
			// b p d
			// - c -
			// a = (pre_y,x), b = (y,pre_x), c = (post_y,x), d = (y,post_x)

			// Neighbor Pixel 'a'
			const std::pair<int,int> a = std::make_pair<int,int>(pre_y,x);
			const OperatingPixelType mask_a = mask.at<OperatingPixelType>(pre_y,x);
			// if neighbor index is within bounds and not finalized
			if ((finalizedPixels.find(a) == finalizedPixels.end()) && (pre_y >= 0) && (mask_a > 0))
			{
				OperatingPixelType neighborValue = reconstruction.at<OperatingPixelType>(pre_y,x);
				OperatingPixelType constraintValue = std::min<OperatingPixelType>(currentValue,mask_a);

//				std::cout << "Neighbor(a) = ("<< static_cast<int>(neighborValue) << "), constraint=(" << static_cast<int>(constraintValue) <<")" << std::endl;

				if (neighborValue < constraintValue)
				{
					reconstruction.at<OperatingPixelType>(pre_y,x) = constraintValue;
					if ( constraintValue == currentValue )
					{
//						std::cout << "Adding ("<< pre_y << "," << x <<") to current list" << std::endl;

						locations.push_back( a );
					}
					else
					{
//						std::cout << "Moving ("<< pre_y << "," << x <<") from "
//						<< static_cast<int>(neighborValue) << " to " << static_cast<int>(constraintValue) << std::endl;

						valueLocationListMap[neighborValue].remove( a );
						valueLocationListMap[constraintValue].push_back( a );
					}
				}
			}

			// Neighbor Pixel 'b'
			const std::pair<int,int> b = std::make_pair<int,int>(y,pre_x);
			const OperatingPixelType mask_b = mask.at<OperatingPixelType>(y,pre_x);
			if ((finalizedPixels.find(b) == finalizedPixels.end()) && (pre_x >= 0) && (mask_b > 0))
			{
				OperatingPixelType neighborValue = reconstruction.at<OperatingPixelType>(y,pre_x);
				OperatingPixelType constraintValue = std::min<OperatingPixelType>(currentValue,mask_b);

//				std::cout << "Neighbor(b) = ("<< static_cast<int>(neighborValue) << "), constraint=(" << static_cast<int>(constraintValue) <<")" << std::endl;

				if (neighborValue < constraintValue)
				{
					reconstruction.at<OperatingPixelType>(y,pre_x) = constraintValue;
					if ( constraintValue == currentValue )
					{
//						std::cout << "Adding ("<< y << "," << pre_x <<") to current list" << std::endl;

						locations.push_back( b );
					}
					else
					{
//						std::cout << "Moving ("<< y << "," << pre_x <<") from "
//						<< static_cast<int>(neighborValue) << " to " << static_cast<int>(constraintValue) << std::endl;

						valueLocationListMap[neighborValue].remove( b );
						valueLocationListMap[constraintValue].push_back( b );
					}
				}
			}

			// Neighbor Pixel 'c'
			const std::pair<int,int> c = std::make_pair<int,int>(post_y,x);
			const OperatingPixelType mask_c = mask.at<OperatingPixelType>(post_y,x);
			if ((finalizedPixels.find(c) == finalizedPixels.end()) && (post_y < size.height) && (mask_c > 0))
			{
				OperatingPixelType neighborValue = reconstruction.at<OperatingPixelType>(post_y,x);
				OperatingPixelType constraintValue = std::min<OperatingPixelType>(currentValue,mask_c);

//				std::cout << "Neighbor(c) = ("<< static_cast<int>(neighborValue) << "), constraint=(" << static_cast<int>(constraintValue) <<")" << std::endl;

				if (neighborValue < constraintValue)
				{
					reconstruction.at<OperatingPixelType>(post_y,x) = constraintValue;
					if ( constraintValue == currentValue )
					{
//						std::cout << "Adding ("<< post_y << "," << x <<") to current list" << std::endl;

						locations.push_back( c );
					}
					else
					{
//						std::cout << "Moving ("<< post_y << "," << x <<") from "
//						<< static_cast<int>(neighborValue) << " to " << static_cast<int>(constraintValue) << std::endl;

						valueLocationListMap[neighborValue].remove( c );
						valueLocationListMap[constraintValue].push_back( c );
					}
				}
			}

			// Neighbor Pixel 'd'
			const std::pair<int,int> d = std::make_pair<int,int>(y,post_x);
			const OperatingPixelType mask_d = mask.at<OperatingPixelType>(y,post_x);
			if ((finalizedPixels.find(d) == finalizedPixels.end()) && (post_x < size.width) && (mask_d > 0))
			{
				OperatingPixelType neighborValue = reconstruction.at<OperatingPixelType>(y,post_x);
				OperatingPixelType constraintValue = std::min<OperatingPixelType>(currentValue,mask_d);

//				std::cout << "Neighbor(d) = ("<< static_cast<int>(neighborValue) << "), constraint=(" << static_cast<int>(constraintValue) <<")" << std::endl;

				if (neighborValue < constraintValue)
				{
					reconstruction.at<OperatingPixelType>(y,post_x) = constraintValue;

					if ( constraintValue == currentValue )
					{
//						std::cout << "Adding ("<< y << "," << post_x <<") to current list" << std::endl;

						locations.push_back( d );
					}
					else
					{
//						std::cout << "Moving ("<< y << "," << post_x <<") from "
//						<< static_cast<int>(neighborValue) << " to " << static_cast<int>(constraintValue) << std::endl;

						valueLocationListMap[neighborValue].remove( d );
						valueLocationListMap[constraintValue].push_back( d );
					}
				}
			}

		} // end, a value's position list is emptied.

	} // end for each value in the marker

	return reconstruction;
};

template <typename OperatingPixelType>
std::vector<cv::Mat> opencvCloseDmp(const cv::Mat& inputImage, const std::vector<int>& ses)
{
	//cgi::log::MetaLogStream& log(cgi::log::MetaLogStream::instance());
	// ++ Close, then erode by reconstruction

	//if (false == std::numeric_limits<T>::is_integer)
	//	CGI_THROW(std::logic_error,"Invalid template type");

	// Build Structuring Elements
	std::vector<cv::Mat> structuringElements;
	for (std::vector<int>::const_iterator level = ses.begin();level!=ses.end();++level)
	{
		const int lev = *level;
		// Circular structuring element
		const int edgeSize = lev+1+lev; // turn radius into edge size
		structuringElements.push_back(
					cv::getStructuringElement(cv::MORPH_ELLIPSE,cv::Size(edgeSize,edgeSize))
					);
	}

	// Build Morphological Operation Stack (close)
	std::vector<cv::Mat> mos;
	for (unsigned int level = 0;level<structuringElements.size();++level)
	{
		cv::Mat filteredImage(inputImage.size().height, inputImage.size().width, cv::DataType<OperatingPixelType>::type);
		cv::morphologyEx(inputImage, filteredImage, cv::MORPH_CLOSE, structuringElements.at(level));
		mos.push_back(filteredImage);
	}

	const int n_lvls = mos.size();
//	log << cgi::log::Priority::INFO << "opencvCloseDmp" << "MOS Produced, size:" << n_lvls << cgi::log::flush;

	// Set the Tile band count to the SEs array size
	std::vector<cv::Mat> profile;
	profile.reserve(n_lvls);

	// ////////////////////////////////////////////////////////////////////////
	// USE: erosionByReconstructionUphill(const cv::Mat& mask,const cv::Mat& mark)

	cv::Mat tempRec;			// Temporary Close Record
	cv::Mat lastRec;			// previous level Close Record

	// Close = (dilate,erode)
	//typename MorphologyOperator<DataType>::type morphology(_ses[0]);
	//typename ReconstructionOperator<DataType>::type reconstructor;

	//erosionByReconstructionUphill<OperatingPixelType>( inputImage, mos.at(0) ).copyTo(tempRec);
	tempRec = erosionByReconstructionUphill<OperatingPixelType>( inputImage, mos.at(0) ).clone();

	// get the absolute difference of the two images
	cv::Mat diffImage(inputImage.size().height, inputImage.size().width, cv::DataType<OperatingPixelType>::type);
	absdiff(inputImage, tempRec, diffImage);

	profile.push_back(diffImage.clone());
	tempRec.copyTo(lastRec);

	// Subsequent Levels
	for (int i = 1;i < n_lvls;++i)
	{
		//  reconstruction(mask     ,  mark)
	        // NOTE: Potentially bad line
	        //erosionByReconstructionUphill<OperatingPixelType>( inputImage, mos.at(i) ).copyTo(tempRec);
		tempRec = erosionByReconstructionUphill<OperatingPixelType>( inputImage, mos.at(i) ).clone();

		// BEGIN DEBUG
		/*
		std::stringstream ss;
		ss << "reconstruction_from_closing_" << i << ".png";
		if(!cv::imwrite(ss.str(), tempRec))
		  {
		    throw std::runtime_error("Failed to write");
		  }
		*/
		// END DEBUG

		// get the absolute difference of the two images
		absdiff(lastRec, tempRec, diffImage);
		profile.push_back(diffImage.clone());

		tempRec.copyTo(lastRec);

	}

	return profile;

};

template <typename OperatingPixelType>
cv::Mat erosionByReconstructionUphill(const cv::Mat& mask, const cv::Mat& mark)
{
//	cgi::log::MetaLogStream& log(cgi::log::MetaLogStream::instance());

	if (false == std::numeric_limits<OperatingPixelType>::is_integer)
		//CGI_THROW(std::logic_error,"Invalid template / operating-pixel type");
		throw new std::logic_error("Invalid template / operating-pixel type");

	const cv::Size size = mask.size();

	//if (size.width != mark.size().width || size.height != mark.size().height)
	if (size != mark.size())
	{
		//log << cgi::log::Priority::ERROR << "erosionByReconstructionDownhill"
		//<< "Size mismatch: ("
		//<< size.width << "," << size.height << ") != ("
		//<< mark.size().width << "," << mark.size().height << ")" << cgi::log::flush;
	}

	// --------------------------------
	// Place each p : I(p) > 0 into List[I(p)]
	// --------------------------------
	// Copy the marker into the reconstruction
	//	we can then grow it in place
	// value i has a list of pixels at position (y,x)
	cv::Mat reconstruction(mark.size().height, mark.size().width, cv::DataType<OperatingPixelType>::type);
	mark.copyTo(reconstruction);

	// for template, use typename
	//	typedef typename std::list<std::pair<int,int> > LocationListType;
	//	typedef typename std::map< OperatingPixelType, LocationListType > MapOfLocationListType;
	typedef std::list<std::pair<int,int> > LocationListType;
	typedef std::map< OperatingPixelType, LocationListType > MapOfLocationListType;

	// This is essentially an indexed image version of the input `mark' raster
	MapOfLocationListType valueLocationListMap;

	// Build an in-memory indexed image
	// but while we are at it, verify that it is properly bounded by `mask' raster
	for (int i = 0; i < size.height; ++i)
		for (int j = 0; j < size.width; ++j)
		{
			const OperatingPixelType pixelValue =  reconstruction.at<OperatingPixelType>(i, j);
			if (pixelValue < mask.at<OperatingPixelType>(i, j))
			{
				//std::cout << "Storing ("<< i << "," << j <<") into valueLocationListMap["<< static_cast<int>(pixelValue) << "]" << std::endl;
				std::stringstream msg;
				msg << "(pixelValue < mask[0](i, j)), for raster location = ("<<i<<","<<j<<")";
				//CGI_THROW (std::logic_error, msg.str()); // the marker must always be GTE the mask, by definition
				throw new std::logic_error(msg.str());
			}

			valueLocationListMap[pixelValue].push_back( std::make_pair<int,int>(i,j) );
		}

	// No valid pixels or values below floor?  Return the input as output
	if (valueLocationListMap.size() == 0)
		return reconstruction;

	// --------------------------------
	// The farthest uphill we will go
	//	is to the current max(mark[0])
	//	therefore, we do not need to
	//	process the current max, we
	//	already copied them to output
	const OperatingPixelType maxValue =  valueLocationListMap.rbegin()->first;
	valueLocationListMap[maxValue].clear();

	std::set<std::pair<int,int> > finalizedPixels;

	// --------------------------------
	// Do a forward iteration (uphill)
	// through the
	//	valueLocationListMap[values]
	// --------------------------------
	// use typename for template version
	//for (typename MapOfLocationListType::iterator valueList = valueLocationListMap.begin();
	//			valueList != valueLocationListMap.end(); ++valueList)
	for (typename MapOfLocationListType::iterator valueList = valueLocationListMap.begin();
					valueList != valueLocationListMap.end(); ++valueList)
	{
		// The gray value
		const OperatingPixelType currentValue = valueList->first;
		// The list of (y,x) tuples
		LocationListType locations = valueList->second;

		// for each location indexed in the value list
		while (!locations.empty())
		{

			// Debug thing
//			if (locations.size() > (size.width * size.height))
//			{
//				CGI_THROW(std::logic_error, "GrayscaleDilationByReconstructionDownhill has gone out of control, this should never happen!");
//			}

			// pull/pop the first position from the list, mark it as finalized
			std::pair<int,int> frontPosition = locations.front();
			finalizedPixels.insert(frontPosition);
			locations.pop_front();

			const int y = frontPosition.first;
			const int x = frontPosition.second;

			const int pre_x = x - 1;
			const int post_x = x + 1;
			const int pre_y = y - 1;
			const int post_y = y + 1;

			// For each neighbor
			// - a -
			// b p d
			// - c -
			// a = (pre_y,x), b = (y,pre_x), c = (post_y,x), d = (y,post_x)

			// Neighbor Pixel 'a'
			const std::pair<int,int> a = std::make_pair<int,int>(pre_y,x);
			const OperatingPixelType mask_a = mask.at<OperatingPixelType>(pre_y,x);
			// if neighbor index is within bounds and not finalized
			if ((finalizedPixels.find(a) == finalizedPixels.end()) && (pre_y >= 0) && (mask_a > 0))
			{
				OperatingPixelType neighborValue = reconstruction.at<OperatingPixelType>(pre_y,x);
				OperatingPixelType constraintValue = std::max<OperatingPixelType>(currentValue,mask_a);

//				std::cout << "Neighbor(a) = ("<< static_cast<int>(neighborValue) << "), constraint=(" << static_cast<int>(constraintValue) <<")" << std::endl;

				if (neighborValue > constraintValue)
				{
					reconstruction.at<OperatingPixelType>(pre_y,x) = constraintValue;
					if ( constraintValue == currentValue )
					{
//						std::cout << "Adding ("<< pre_y << "," << x <<") to current list" << std::endl;

						locations.push_back( a );
					}
					else
					{
//						std::cout << "Moving ("<< pre_y << "," << x <<") from "
//						<< static_cast<int>(neighborValue) << " to " << static_cast<int>(constraintValue) << std::endl;

						valueLocationListMap[neighborValue].remove( a );
						valueLocationListMap[constraintValue].push_back( a );
					}
				}
			}

			// Neighbor Pixel 'b'
			const std::pair<int,int> b = std::make_pair<int,int>(y,pre_x);
			const OperatingPixelType mask_b = mask.at<OperatingPixelType>(y,pre_x);
			if ((finalizedPixels.find(b) == finalizedPixels.end()) && (pre_x >= 0) && (mask_b > 0))
			{
				OperatingPixelType neighborValue = reconstruction.at<OperatingPixelType>(y,pre_x);
				OperatingPixelType constraintValue = std::max<OperatingPixelType>(currentValue,mask_b);

//				std::cout << "Neighbor(b) = ("<< static_cast<int>(neighborValue) << "), constraint=(" << static_cast<int>(constraintValue) <<")" << std::endl;

				if (neighborValue > constraintValue)
				{
					reconstruction.at<OperatingPixelType>(y,pre_x) = constraintValue;
					if ( constraintValue == currentValue )
					{
//						std::cout << "Adding ("<< y << "," << pre_x <<") to current list" << std::endl;

						locations.push_back( b );
					}
					else
					{
//						std::cout << "Moving ("<< y << "," << pre_x <<") from "
//						<< static_cast<int>(neighborValue) << " to " << static_cast<int>(constraintValue) << std::endl;

						valueLocationListMap[neighborValue].remove( b );
						valueLocationListMap[constraintValue].push_back( b );
					}
				}
			}

			// Neighbor Pixel 'c'
			const std::pair<int,int> c = std::make_pair<int,int>(post_y,x);
			const OperatingPixelType mask_c = mask.at<OperatingPixelType>(post_y,x);
			if ((finalizedPixels.find(c) == finalizedPixels.end()) && (post_y < size.height) && (mask_c > 0))
			{
				OperatingPixelType neighborValue = reconstruction.at<OperatingPixelType>(post_y,x);
				OperatingPixelType constraintValue = std::max<OperatingPixelType>(currentValue,mask_c);

//				std::cout << "Neighbor(c) = ("<< static_cast<int>(neighborValue) << "), constraint=(" << static_cast<int>(constraintValue) <<")" << std::endl;

				if (neighborValue > constraintValue)
				{
					reconstruction.at<OperatingPixelType>(post_y,x) = constraintValue;
					if ( constraintValue == currentValue )
					{
//						std::cout << "Adding ("<< post_y << "," << x <<") to current list" << std::endl;

						locations.push_back( c );
					}
					else
					{
//						std::cout << "Moving ("<< post_y << "," << x <<") from "
//						<< static_cast<int>(neighborValue) << " to " << static_cast<int>(constraintValue) << std::endl;

						valueLocationListMap[neighborValue].remove( c );
						valueLocationListMap[constraintValue].push_back( c );
					}
				}
			}

			// Neighbor Pixel 'd'
			const std::pair<int,int> d = std::make_pair<int,int>(y,post_x);
			const OperatingPixelType mask_d = mask.at<OperatingPixelType>(y,post_x);
			if ((finalizedPixels.find(d) == finalizedPixels.end()) && (post_x < size.width) && (mask_d > 0))
			{
				OperatingPixelType neighborValue = reconstruction.at<OperatingPixelType>(y,post_x);
				OperatingPixelType constraintValue = std::max<OperatingPixelType>(currentValue,mask_d);

//				std::cout << "Neighbor(d) = ("<< static_cast<int>(neighborValue) << "), constraint=(" << static_cast<int>(constraintValue) <<")" << std::endl;

				if (neighborValue > constraintValue)
				{
					reconstruction.at<OperatingPixelType>(y,post_x) = constraintValue;

					if ( constraintValue == currentValue )
					{
//						std::cout << "Adding ("<< y << "," << post_x <<") to current list" << std::endl;

						locations.push_back( d );
					}
					else
					{
//						std::cout << "Moving ("<< y << "," << post_x <<") from "
//						<< static_cast<int>(neighborValue) << " to " << static_cast<int>(constraintValue) << std::endl;

						valueLocationListMap[neighborValue].remove( d );
						valueLocationListMap[constraintValue].push_back( d );
					}
				}
			}

		} // end, a value's position list is emptied.

	} // end for each value in the marker

	return reconstruction;

};

#endif //_OPENCV_DMP_HPP_


#pragma once

#include <igl/AABB.h>
#include <igl/in_element.h>
#include <igl/barycentric_coordinates.h>

#include <Eigen/Dense>

namespace polyfem
{
	class InterpolatedFunction2d
	{
	public:
		InterpolatedFunction2d() { }
		InterpolatedFunction2d(const Eigen::MatrixXd &fun, const Eigen::MatrixXd &pts, const Eigen::MatrixXi &tris);
		void init(const Eigen::MatrixXd &fun, const Eigen::MatrixXd &pts, const Eigen::MatrixXi &tris);

		Eigen::MatrixXd interpolate(const Eigen::MatrixXd &pts) const;

	private:
		igl::AABB<Eigen::MatrixXd, 2> tree_;
		Eigen::MatrixXd fun_;
		Eigen::MatrixXd pts_;
		Eigen::MatrixXi tris_;
	};
}


#include "gtest/gtest.h"
#include <limits>
#include <boost/archive/binary_iarchive.hpp>
#include <boost/archive/binary_oarchive.hpp>
#include "bakuage/lof.h"
#include "bakuage/memory.h"
#include "bakuage/vector_math.h"

namespace {
    typedef bakuage::AlignedPodVector<double> Point;

    struct DistFunc {
        double operator () (const Point &a, const Point &b) {
            return bakuage::VectorNormDiffL2(a.data(), b.data(), a.size());
        }

    private:
        friend class boost::serialization::access;
        template<class Archive>
        void serialize(Archive & ar, const unsigned int version) {}
    };
}

TEST(Lof, SameAsScikit) {
    const int dim = 2;
    const std::vector<double> points_data({
        0, 1,
        2, 3,
        4, 5,
        16, 17,
    });
    const std::vector<double> query_data({
        -2, -1,
        1, 0,
        3, 2,
        20, 10,
    });
    const std::vector<double> expected({
        1.16666667,
        0.90942658,
        0.875,
        2.65161739,
    });

    DistFunc func;
    bakuage::Lof<Point, double, DistFunc> lof(func);
    std::vector<Point> points;
    std::vector<Point> querys;
    for (int i = 0; i < points_data.size() / dim; i++) {
        Point v(dim);
        for (int j = 0; j < dim; j++) {
            v[j] = points_data[dim * i + j];
        }
        points.emplace_back(v);
    }
    for (int i = 0; i < query_data.size() / dim; i++) {
        Point v(dim);
        for (int j = 0; j < dim; j++) {
            v[j] = query_data[dim * i + j];
        }
        querys.emplace_back(v);
    }
    lof.Prepare(points.begin(), points.end(), 2);

    for (int i = 0; i < expected.size(); i++) {
        const auto x = lof.CalculateLof(querys[i]);
        EXPECT_NEAR(expected[i], x, 1e-7);
    }
}

TEST(Lof, Serialization) {
    const int dim = 2;
    const std::vector<double> points_data({
        0, 1,
        2, 3,
        4, 5,
        16, 17,
    });
    const std::vector<double> query_data({
        -2, -1,
        1, 0,
        3, 2,
        20, 10,
    });
    const std::vector<double> expected({
        1.16666667,
        0.90942658,
        0.875,
        2.65161739,
    });

    std::stringstream ss;
    {
        std::vector<Point> points;
        for (int i = 0; i < points_data.size() / dim; i++) {
            Point v(dim);
            for (int j = 0; j < dim; j++) {
                v[j] = points_data[dim * i + j];
            }
            points.emplace_back(v);
        }
        DistFunc func;
        bakuage::Lof<Point, double, DistFunc> lof(func);
        lof.Prepare(points.begin(), points.end(), 2);
        boost::archive::binary_oarchive oa(ss);
        oa << lof;
    }

    DistFunc func;
    bakuage::Lof<Point, double, DistFunc> lof2(func);
    {
        boost::archive::binary_iarchive ia(ss);
        ia >> lof2;
    }

    std::vector<Point> querys;
    for (int i = 0; i < query_data.size() / dim; i++) {
        Point v(dim);
        for (int j = 0; j < dim; j++) {
            v[j] = query_data[dim * i + j];
        }
        querys.emplace_back(v);
    }

    for (int i = 0; i < expected.size(); i++) {
        const auto x = lof2.CalculateLof(querys[i]);
        EXPECT_NEAR(expected[i], x, 1e-7);
    }
}


// Copyright (c) 2014-2017 The Zozocoin Core developers

#include "governance.h"

#include "test/test_zozocoin.h"

#include <boost/test/unit_test.hpp>

BOOST_FIXTURE_TEST_SUITE(ratecheck_tests, BasicTestingSetup)

BOOST_AUTO_TEST_CASE(ratecheck_test)
{
    CRateCheckBuffer buffer;

    BOOST_CHECK(buffer.GetCount() == 0);
    BOOST_CHECK(buffer.GetMinTimestamp() == numeric_limits<int64_t>::max());
    BOOST_CHECK(buffer.GetMaxTimestamp() == 0);
    BOOST_CHECK(buffer.GetRate() == 0.0);

    buffer.AddTimestamp(1);

    std::cout << "buffer.GetMinTimestamp() = " << buffer.GetMinTimestamp() << std::endl;

    BOOST_CHECK(buffer.GetCount() == 1);
    BOOST_CHECK(buffer.GetMinTimestamp() == 1);
    BOOST_CHECK(buffer.GetMaxTimestamp() == 1);
    BOOST_CHECK(buffer.GetRate() == 0.0);

    buffer.AddTimestamp(2);
    BOOST_CHECK(buffer.GetCount() == 2);
    BOOST_CHECK(buffer.GetMinTimestamp() == 1);
    BOOST_CHECK(buffer.GetMaxTimestamp() == 2);
    BOOST_CHECK(fabs(buffer.GetRate() - 2.0) < 1.0e-9);

    buffer.AddTimestamp(3);
    BOOST_CHECK(buffer.GetCount() == 3);
    BOOST_CHECK(buffer.GetMinTimestamp() == 1);
    BOOST_CHECK(buffer.GetMaxTimestamp() == 3);

    int64_t nMin = buffer.GetMinTimestamp();
    int64_t nMax = buffer.GetMaxTimestamp();
    double dRate = buffer.GetRate();

    std::cout << "buffer.GetCount() = " << buffer.GetCount() << std::endl;
    std::cout << "nMin = " << nMin << std::endl;
    std::cout << "nMax = " << nMax << std::endl;
    std::cout << "buffer.GetRate() = " << dRate << std::endl;

    BOOST_CHECK(fabs(buffer.GetRate() - (3.0/2.0)) < 1.0e-9);

    buffer.AddTimestamp(4);
    BOOST_CHECK(buffer.GetCount() == 4);
    BOOST_CHECK(buffer.GetMinTimestamp() == 1);
    BOOST_CHECK(buffer.GetMaxTimestamp() == 4);
    BOOST_CHECK(fabs(buffer.GetRate() - (4.0/3.0)) < 1.0e-9);

    buffer.AddTimestamp(5);
    BOOST_CHECK(buffer.GetCount() == 5);
    BOOST_CHECK(buffer.GetMinTimestamp() == 1);
    BOOST_CHECK(buffer.GetMaxTimestamp() == 5);
    BOOST_CHECK(fabs(buffer.GetRate() - (5.0/4.0)) < 1.0e-9);

    buffer.AddTimestamp(6);
    BOOST_CHECK(buffer.GetCount() == 5);
    BOOST_CHECK(buffer.GetMinTimestamp() == 2);
    BOOST_CHECK(buffer.GetMaxTimestamp() == 6);
    BOOST_CHECK(fabs(buffer.GetRate() - (5.0/4.0)) < 1.0e-9);

    CRateCheckBuffer buffer2;

    std::cout << "Before loop tests" << std::endl;
    for(int64_t i = 1; i < 11; ++i)  {
        std::cout << "In loop: i = " << i << std::endl;
        buffer2.AddTimestamp(i);
        BOOST_CHECK(buffer2.GetCount() == (i <= 5 ? i : 5));
        BOOST_CHECK(buffer2.GetMinTimestamp() == max(int64_t(1), i - 4));
        BOOST_CHECK(buffer2.GetMaxTimestamp() == i);
    }
}

BOOST_AUTO_TEST_SUITE_END()


#ifndef CANNON_PHYSICS_SYSTEMS_SYSTEM_H
#define CANNON_PHYSICS_SYSTEMS_SYSTEM_H

#include <Eigen/Dense>

#include <ompl/config.h>
#include <ompl/control/ODESolver.h>

namespace oc = ompl::control;
namespace ob = ompl::base;

using namespace Eigen;

namespace cannon {
  namespace physics {
    namespace systems {

      class System {
        public:
          virtual void operator()(const VectorXd &s, VectorXd &dsdt,
                                  const double t) = 0;

          virtual void ompl_ode_adaptor(const oc::ODESolver::StateType &q,
                                        const oc::Control *control,
                                        oc::ODESolver::StateType &qdot) = 0;

          virtual std::tuple<MatrixXd, MatrixXd, VectorXd>
          get_linearization(const VectorXd &x) = 0;

          virtual void
          get_continuous_time_linearization(const oc::ODESolver::StateType &q,
                                            Ref<MatrixXd> A,
                                            Ref<MatrixXd> B) = 0;
      };

    } // namespace system
  } // namespace physics
} // namespace cannon

#endif /* ifndef CANNON_PHYSICS_SYSTEMS_SYSTEM_H */


//=======================================================================
// Copyright 2007 Aaron Windsor
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//=======================================================================
#include <iostream>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/properties.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/property_map/property_map.hpp>
#include <boost/ref.hpp>
#include <vector>

#include <boost/graph/biconnected_components.hpp>
#include <boost/graph/make_biconnected_planar.hpp>
#include <boost/graph/boyer_myrvold_planar_test.hpp>

using namespace boost;

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

    typedef adjacency_list< vecS, vecS, undirectedS,
        property< vertex_index_t, int >, property< edge_index_t, int > >
        graph;

    graph g(11);
    add_edge(0, 1, g);
    add_edge(2, 3, g);
    add_edge(3, 0, g);
    add_edge(3, 4, g);
    add_edge(4, 5, g);
    add_edge(5, 3, g);
    add_edge(5, 6, g);
    add_edge(6, 7, g);
    add_edge(7, 8, g);
    add_edge(8, 5, g);
    add_edge(8, 9, g);
    add_edge(0, 10, g);

    // Initialize the interior edge index
    property_map< graph, edge_index_t >::type e_index = get(edge_index, g);
    graph_traits< graph >::edges_size_type edge_count = 0;
    graph_traits< graph >::edge_iterator ei, ei_end;
    for (boost::tie(ei, ei_end) = edges(g); ei != ei_end; ++ei)
        put(e_index, *ei, edge_count++);

    // Test for planarity; compute the planar embedding as a side-effect
    typedef std::vector< graph_traits< graph >::edge_descriptor > vec_t;
    std::vector< vec_t > embedding(num_vertices(g));
    if (boyer_myrvold_planarity_test(boyer_myrvold_params::graph = g,
            boyer_myrvold_params::embedding = &embedding[0]))
        std::cout << "Input graph is planar" << std::endl;
    else
        std::cout << "Input graph is not planar" << std::endl;

    typedef std::vector< graph_traits< graph >::edges_size_type >
        component_storage_t;
    typedef iterator_property_map< component_storage_t::iterator,
        property_map< graph, edge_index_t >::type >
        component_map_t;

    component_storage_t component_storage(num_edges(g));
    component_map_t component(component_storage.begin(), get(edge_index, g));

    std::cout << "Before calling make_biconnected_planar, the graph has "
              << biconnected_components(g, component)
              << " biconnected components" << std::endl;

    make_biconnected_planar(g, &embedding[0]);

    // Re-initialize the edge index, since we just added a few edges
    edge_count = 0;
    for (boost::tie(ei, ei_end) = edges(g); ei != ei_end; ++ei)
        put(e_index, *ei, edge_count++);

    // Re-size the storage for the biconnected components, since we
    // just added a few edges

    component_storage.resize(num_edges(g));
    component = component_map_t(component_storage.begin(), get(edge_index, g));

    std::cout << "After calling make_biconnected_planar, the graph has "
              << biconnected_components(g, component)
              << " biconnected components" << std::endl;

    if (boyer_myrvold_planarity_test(g))
        std::cout << "Also, the graph is still planar." << std::endl;
    else
        std::cout << "But the graph is not still planar." << std::endl;

    return 0;
}


/*
   Copyright 2018 Simon Vogl <svogl@voxel.at>
                  Angel Merino-Sastre <amerino@voxel.at>

   Licensed 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 <string>
#include <iostream>
#include <fstream>
#include <limits.h>

#include <pcl/range_image/range_image_planar.h>

#include <pcl/io/pcd_io.h>
#include <pcl/sample_consensus/ransac.h>
#include <pcl/sample_consensus/sac_model.h>
#include <pcl/sample_consensus/sac_model_line.h>
#include <pcl/sample_consensus/sac_model_plane.h>

#include <pcl/sample_consensus/sac.h>

#include <boost/date_time/posix_time/posix_time.hpp>

#include <boost/log/trivial.hpp>
#include <boost/foreach.hpp>
#include <boost/property_tree/json_parser.hpp>

#include "toffy/3d/sampleConsensus.hpp"

using namespace toffy;
using namespace toffy::filters::f3d;
using namespace cv;
using namespace std;

#ifdef CM_DEBUG
const bool dbg=true;
#else
const bool dbg=false;
#endif

bool SampleConsensus::filter(const Frame &in, Frame& out)
{
    using namespace boost::posix_time;

    ptime start = boost::posix_time::microsec_clock::local_time();
    ptime tick = boost::posix_time::microsec_clock::local_time();
    boost::posix_time::time_duration diff = tick - start;

    pcl::PCLPointCloud2Ptr planar;
    try {
        planar = boost::any_cast<pcl::PCLPointCloud2Ptr>(in.getData(this->in));
    } catch(const boost::bad_any_cast &) {
	BOOST_LOG_TRIVIAL(warning) <<
	    "Could not cast input "  <<
	    ", filter  " << id() <<" not applied.";
	return false;
    }
    /*
    pcl::SampleConsensusModelPtr scModel;
    if (model == "plane" ) {
	scModel = model_plane(new pcl::SampleConsensusModelPlane<pcl::PointWithRange>(planar));
    } else if (model == "line" ) {
	pcl::SampleConsensusModelLine<pcl::PointWithRange>::Ptr
	    model_line(new pcl::SampleConsensusModelLine<pcl::PointWithRange>(planar));
    } else {
	return false;
    }
    */

    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud;
    pcl::fromPCLPointCloud2(*planar,*cloud);

    pcl::SampleConsensusModelPlane<pcl::PointXYZ>::Ptr
        model_plane(new pcl::SampleConsensusModelPlane<pcl::PointXYZ>(cloud));

    pcl::RandomSampleConsensus<pcl::PointXYZ> ransac(model_plane, threshold );

    ransac.setMaxIterations(maxIters);

    tick = boost::posix_time::microsec_clock::local_time();
    diff = tick - start;
    cout << id() << "::filter\t" << diff.total_milliseconds() << "\tpre sac" << endl;

    bool success = ransac.computeModel(1);
    if (!success) {
	cout << " could not ransac - skip!" << endl;
	return true;
    }

    tick = boost::posix_time::microsec_clock::local_time();
    diff = tick - start;
    cout << id() << "::filter\t" << diff.total_milliseconds() << "\tpost sac" << endl;

    std::vector<int> inliers;
    ransac.getInliers(inliers);

    pcl::PointCloud<pcl::PointXYZ>::Ptr pi(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PointCloud<pcl::PointXYZ>::Ptr po(new pcl::PointCloud<pcl::PointXYZ>);

    if (inliers.size() ) {
	size_t idx=0;
	size_t pIdx=0;
	while ( pIdx < cloud->size() ) {
	    if ( pIdx == (unsigned int)inliers[idx]) {
		pi->push_back( cloud->at(pIdx));
		if (idx<inliers.size()) // or push a dummy value to inliers
		    idx++;
	    } else {
		po->push_back( cloud->at(pIdx));
	    }
	    pIdx++;
	}

	out.addData( this->inliers, pi);
	out.addData( this->outliers, po);
    }

    Eigen::VectorXf coeffs;
    ransac.getModelCoefficients(coeffs);

    cout << "PLAN " << coeffs.transpose() << endl;
    /*
    float roll, pitch, yaw;
    Eigen::Affine3f A;
    pcl::getEulerAngles ( coeffs, roll, pitch, yaw);

    cout << "PLAN " << coeffs.transpose() << " " << roll << " " << pitch << " " << yaw <<  endl;
    */

    tick = boost::posix_time::microsec_clock::local_time();
    diff = tick - start;
    cout << id() << "::filter\t" << diff.total_milliseconds() << "\tfin" << endl;

    return true;
}

/******************************************************************************************************
 */

int SampleConsensus::loadConfig(const boost::property_tree::ptree& pt)
{
    BOOST_LOG_TRIVIAL(debug) << __FUNCTION__ <<  " " << id();

    updateConfig(pt.begin()->second );
    return 1;
}

boost::property_tree::ptree SampleConsensus::getConfig() const
{
    boost::property_tree::ptree pt;

    pt.put("inputs.cloud", in);

    pt.put("outputs.inliers", inliers);
    pt.put("outputs.outliers", outliers);

    pt.put("threshold", threshold);
    pt.put("maxIterations", maxIters);

    return pt;
}

void SampleConsensus::updateConfig(const boost::property_tree::ptree &pt)
{
    using namespace boost::property_tree;

    BOOST_LOG_TRIVIAL(debug) << __FUNCTION__ <<  " " << id();

    in = pt.get("inputs.cloud", in);

    inliers = pt.get("outputs.inliers", inliers);
    outliers = pt.get("outputs.outliers", outliers);

    threshold = pt.get("threshold", threshold);
    maxIters = pt.get("maxIterations", maxIters);
}


// Persistent Homology code by Hubert Wagner based on an implementation by Chao Chen
#include <cmath>
#include <vector>
#include <map>
#include <set>
#include <list>
#include <cassert>
#include <algorithm>
#include <iostream>
#include <sstream>
#include <fstream>
#include <iomanip>
#include <deque>
#include <cstring>
#include <ctime>
#include <blitz/array.h>
#include <blitz/tinyvec-et.h>

using namespace std;

const int BIG_INT = 0x7FFFFFFF;	//be careful, could be too small compared to # of cubes
typedef int CellNrType; // it could be long for really large inputs...

#include "PersistenceIO.h"
#include "Debugging.h"
#include "GeneralFiltration.h"

#include "InputRunner.h"

#include "PersistentPair.h"
#include "DataReaders.h"

#include "PersistenceCalculator.h"
#include "PersistenceCalcRunner.h"

// python thingy
#include <pybind11/pybind11.h>
#include <iostream>

namespace py = pybind11;

#include <pybind11/stl.h>

// std::vector<std::vector<double> > kw_func4(std::vector<std::vector<double> > &entries) {
//     std::vector<std::vector<double> > try_vec(2);
//     std::cout << "kw_func4: " << std::endl;
//     for (std::vector<double> i : entries){
//         for (double j : i){
//             std::cout << j << " ";
//             try_vec[0].push_back(j);
//             try_vec[1].push_back(j*j);
//         }
//         std::cout << endl;
//     }
//     return try_vec;
// }
//

std::vector<std::vector<double> > cubePers(std::vector< double > &entries, std::vector<int> dims, double pers_thd ) {
	string lfile = "log.txt";
	string efile = "error.txt";
	DebuggerClass::init( true, lfile, efile );

        int dim = dims.size();
	InputFileInfo input_file_info(dim);

	int max_pers_pts = BIG_INT; // the maximal number of persistence pairs recorded

	time_t startTime1, startTime2, endTime;
	time(&startTime1);

        std::vector<std::vector< double > > ret;

	switch(input_file_info.dimension)
	{
	case 1:
		ret = InputRunner<1>::run(input_file_info, dims, entries, pers_thd);
		break;
	case 2:
		ret = InputRunner<2>::run(input_file_info, dims, entries, pers_thd);
		break;
	case 3:
		ret = InputRunner<3>::run(input_file_info, dims, entries, pers_thd);
		break;
	case 4:
		ret = InputRunner<4>::run(input_file_info, dims, entries, pers_thd);
		break;
	case 5:
		ret = InputRunner<5>::run(input_file_info, dims, entries, pers_thd);
		break;
	case 6:
		ret = InputRunner<6>::run(input_file_info, dims, entries, pers_thd);
		break;
	case 7:
		ret = InputRunner<7>::run(input_file_info, dims, entries, pers_thd);
		break;
	case 8:
		ret = InputRunner<8>::run(input_file_info, dims, entries, pers_thd);
		break;
        default:
                assert(false);
	}

	time(&endTime);
	double ellapsed1 = difftime (endTime,startTime1);

	cout << "All calculations computed in " << setprecision(2) << ellapsed1/60.0 << endl;
	DebuggerClass::finish();

	return ret;
}

PYBIND11_PLUGIN(PersistencePython) {
        py::module m("PersistencePython", "python binding for persistence computation (cubical complex)");

//    m.def("kw_func4", &kw_func4, py::arg("myList") = list);
    m.def("cubePers", &cubePers);
    return m.ptr();
}


#include <dynamic_reconfigure/server.h>
#include <kr_mav_controllers/SO3Config.h>
#include <kr_mav_controllers/SO3Control.h>
#include <kr_mav_msgs/Corrections.h>
#include <kr_mav_msgs/PositionCommand.h>
#include <kr_mav_msgs/TRPYCommand.h>
#include <nav_msgs/Odometry.h>
#include <nodelet/nodelet.h>
#include <ros/ros.h>
#include <std_msgs/Bool.h>

#include <Eigen/Geometry>

#define CLAMP(x, min, max) ((x) < (min)) ? (min) : ((x) > (max)) ? (max) : (x)

class SO3TRPYControlNodelet : public nodelet::Nodelet
{
 public:
  SO3TRPYControlNodelet()
      : odom_set_(false),
        position_cmd_updated_(false),
        position_cmd_init_(false),
        des_yaw_(0),
        des_yaw_dot_(0),
        yaw_int_(0),
        enable_motors_(false),
        use_external_yaw_(false),
        g_(9.81)
  {
    controller_.resetIntegrals();
  }

  void onInit();

  EIGEN_MAKE_ALIGNED_OPERATOR_NEW;  // Need this since we have SO3Control which needs aligned pointer

 private:
  void publishCommand();
  void position_cmd_callback(const kr_mav_msgs::PositionCommand::ConstPtr &cmd);
  void odom_callback(const nav_msgs::Odometry::ConstPtr &odom);
  void enable_motors_callback(const std_msgs::Bool::ConstPtr &msg);
  void corrections_callback(const kr_mav_msgs::Corrections::ConstPtr &msg);
  void cfg_callback(kr_mav_controllers::SO3Config &config, uint32_t level);

  SO3Control controller_;
  ros::Publisher trpy_command_pub_;
  ros::Subscriber odom_sub_, position_cmd_sub_, enable_motors_sub_, corrections_sub_;

  bool odom_set_, position_cmd_updated_, position_cmd_init_;
  std::string frame_id_;

  Eigen::Vector3f des_pos_, des_vel_, des_acc_, des_jrk_, config_kx_, config_kv_, config_ki_, config_kib_, kx_, kv_;
  float des_yaw_, des_yaw_dot_, yaw_int_;
  bool enable_motors_, use_external_yaw_;
  float kR_[3], kOm_[3], corrections_[3];
  float ki_yaw_;
  float mass_;
  const float g_;
  Eigen::Quaternionf current_orientation_;

  boost::recursive_mutex config_mutex_;
  typedef dynamic_reconfigure::Server<kr_mav_controllers::SO3Config> ReconfigureServer;
  boost::shared_ptr<ReconfigureServer> reconfigure_server_;
};

void SO3TRPYControlNodelet::publishCommand()
{
  if(!odom_set_)
  {
    NODELET_INFO_THROTTLE(1, "Odom not set, not publishing command!");
    return;
  }

  float ki_yaw = 0;
  Eigen::Vector3f ki = Eigen::Vector3f::Zero();
  Eigen::Vector3f kib = Eigen::Vector3f::Zero();
  if(enable_motors_)
  {
    ki_yaw = ki_yaw_;
    ki = config_ki_;
    kib = config_kib_;
  }
  controller_.calculateControl(des_pos_, des_vel_, des_acc_, des_jrk_, des_yaw_, des_yaw_dot_, kx_, kv_, ki, kib);

  const Eigen::Vector3f &force = controller_.getComputedForce();
  const Eigen::Quaternionf &q_des = controller_.getComputedOrientation();
  const Eigen::Vector3f &ang_vel = controller_.getComputedAngularVelocity();

  const Eigen::Matrix3f R_des(q_des);
  const Eigen::Matrix3f R_cur(current_orientation_);

  const float yaw_cur = std::atan2(R_cur(1, 0), R_cur(0, 0));
  const float yaw_des = std::atan2(R_des(1, 0), R_des(0, 0));
  const float pitch_des = -std::asin(R_des(2, 0));
  const float roll_des = std::atan2(R_des(2, 1), R_des(2, 2));

  const float Psi = 0.5f * (3.0f - (R_des(0, 0) * R_cur(0, 0) + R_des(1, 0) * R_cur(1, 0) + R_des(2, 0) * R_cur(2, 0) +
                                    R_des(0, 1) * R_cur(0, 1) + R_des(1, 1) * R_cur(1, 1) + R_des(2, 1) * R_cur(2, 1) +
                                    R_des(0, 2) * R_cur(0, 2) + R_des(1, 2) * R_cur(1, 2) + R_des(2, 2) * R_cur(2, 2)));

  float thrust = 0.0f;
  if(Psi < 1.0f)  // Position control stability guaranteed only when Psi < 1
    thrust = force(0) * R_cur(0, 2) + force(1) * R_cur(1, 2) + force(2) * R_cur(2, 2);

  float e_yaw = yaw_des - yaw_cur;

  const float PI = static_cast<float>(M_PI);
  if(e_yaw > PI)
    e_yaw -= 2 * PI;
  else if(e_yaw < -PI)
    e_yaw += 2 * PI;

  // Yaw integral
  yaw_int_ += ki_yaw * e_yaw;
  if(yaw_int_ > PI)
    yaw_int_ = PI;
  else if(yaw_int_ < -PI)
    yaw_int_ = -PI;

  float yaw_cmd = yaw_des + yaw_int_;
  if(yaw_cmd > PI)
    yaw_cmd -= 2 * PI;
  else if(yaw_cmd < -PI)
    yaw_cmd += 2 * PI;

  kr_mav_msgs::TRPYCommand::Ptr trpy_command(new kr_mav_msgs::TRPYCommand);
  trpy_command->header.stamp = ros::Time::now();
  trpy_command->header.frame_id = frame_id_;
  if(enable_motors_)
  {
    trpy_command->thrust = CLAMP(thrust, 0.01f * 9.81f, 10.0f * 9.81f);
    trpy_command->roll = roll_des;
    trpy_command->pitch = pitch_des;
    trpy_command->yaw = yaw_cmd;
    trpy_command->angular_velocity.x = ang_vel(0);
    trpy_command->angular_velocity.y = ang_vel(1);
    trpy_command->angular_velocity.z = ang_vel(2);
    for(int i = 0; i < 3; i++)
    {
      trpy_command->kR[i] = kR_[i];
      trpy_command->kOm[i] = kOm_[i];
    }
  }
  trpy_command->aux.current_yaw = yaw_cur;
  trpy_command->aux.kf_correction = corrections_[0];
  trpy_command->aux.angle_corrections[0] = corrections_[1];
  trpy_command->aux.angle_corrections[1] = corrections_[2];
  trpy_command->aux.enable_motors = enable_motors_;
  trpy_command->aux.use_external_yaw = use_external_yaw_;
  trpy_command_pub_.publish(trpy_command);
}

void SO3TRPYControlNodelet::position_cmd_callback(const kr_mav_msgs::PositionCommand::ConstPtr &cmd)
{
  des_pos_ = Eigen::Vector3f(cmd->position.x, cmd->position.y, cmd->position.z);
  des_vel_ = Eigen::Vector3f(cmd->velocity.x, cmd->velocity.y, cmd->velocity.z);
  des_acc_ = Eigen::Vector3f(cmd->acceleration.x, cmd->acceleration.y, cmd->acceleration.z);
  des_jrk_ = Eigen::Vector3f(cmd->jerk.x, cmd->jerk.y, cmd->jerk.z);

  // Check use_msg_gains_flag to decide whether to use gains from the msg or config
  kx_[0] = (cmd->use_msg_gains_flags & cmd->USE_MSG_GAINS_POSITION_X) ? cmd->kx[0] : config_kx_[0];
  kx_[1] = (cmd->use_msg_gains_flags & cmd->USE_MSG_GAINS_POSITION_Y) ? cmd->kx[1] : config_kx_[1];
  kx_[2] = (cmd->use_msg_gains_flags & cmd->USE_MSG_GAINS_POSITION_Z) ? cmd->kx[2] : config_kx_[2];
  kv_[0] = (cmd->use_msg_gains_flags & cmd->USE_MSG_GAINS_VELOCITY_X) ? cmd->kv[0] : config_kv_[0];
  kv_[1] = (cmd->use_msg_gains_flags & cmd->USE_MSG_GAINS_VELOCITY_Y) ? cmd->kv[1] : config_kv_[1];
  kv_[2] = (cmd->use_msg_gains_flags & cmd->USE_MSG_GAINS_VELOCITY_Z) ? cmd->kv[2] : config_kv_[2];

  des_yaw_ = cmd->yaw;
  des_yaw_dot_ = cmd->yaw_dot;
  position_cmd_updated_ = true;
  // position_cmd_init_ = true;

  publishCommand();
}

void SO3TRPYControlNodelet::odom_callback(const nav_msgs::Odometry::ConstPtr &odom)
{
  if(!odom_set_)
    odom_set_ = true;

  const Eigen::Vector3f position(odom->pose.pose.position.x, odom->pose.pose.position.y, odom->pose.pose.position.z);
  const Eigen::Vector3f velocity(odom->twist.twist.linear.x, odom->twist.twist.linear.y, odom->twist.twist.linear.z);

  current_orientation_ = Eigen::Quaternionf(odom->pose.pose.orientation.w, odom->pose.pose.orientation.x,
                                            odom->pose.pose.orientation.y, odom->pose.pose.orientation.z);

  controller_.setPosition(position);
  controller_.setVelocity(velocity);
  controller_.setCurrentOrientation(current_orientation_);

  if(position_cmd_init_)
  {
    // We set position_cmd_updated_ = false and expect that the
    // position_cmd_callback would set it to true since typically a position_cmd
    // message would follow an odom message. If not, the position_cmd_callback
    // hasn't been called and we publish the so3 command ourselves
    // TODO: Fallback to hover if position_cmd hasn't been received for some time
    if(!position_cmd_updated_)
      publishCommand();
    position_cmd_updated_ = false;
  }
}

void SO3TRPYControlNodelet::enable_motors_callback(const std_msgs::Bool::ConstPtr &msg)
{
  if(msg->data)
    ROS_INFO("Enabling motors");
  else
    ROS_INFO("Disabling motors");

  enable_motors_ = msg->data;

  // Reset integrals when toggling motor state
  yaw_int_ = 0;
  controller_.resetIntegrals();
}

void SO3TRPYControlNodelet::corrections_callback(const kr_mav_msgs::Corrections::ConstPtr &msg)
{
  corrections_[0] = msg->kf_correction;
  corrections_[1] = msg->angle_corrections[0];
  corrections_[2] = msg->angle_corrections[1];
}

void SO3TRPYControlNodelet::cfg_callback(kr_mav_controllers::SO3Config &config, uint32_t level)
{
  if(level == 0)
  {
    NODELET_DEBUG_STREAM("Nothing changed. level: " << level);
    return;
  }

  if(level & (1 << 0))
  {
    config_kx_[0] = config.kp_x;
    config_kx_[1] = config.kp_y;
    config_kx_[2] = config.kp_z;

    config_kv_[0] = config.kd_x;
    config_kv_[1] = config.kd_y;
    config_kv_[2] = config.kd_z;

    NODELET_INFO("Position Gains set to kp: {%2.3g, %2.3g, %2.3g}, kd: {%2.3g, %2.3g, %2.3g}", config_kx_[0],
                 config_kx_[1], config_kx_[2], config_kv_[0], config_kv_[1], config_kv_[2]);
  }

  if(level & (1 << 1))
  {
    config_ki_[0] = config.ki_x;
    config_ki_[1] = config.ki_y;
    config_ki_[2] = config.ki_z;

    config_kib_[0] = config.kib_x;
    config_kib_[1] = config.kib_y;
    config_kib_[2] = config.kib_z;

    NODELET_INFO("Integral Gains set to ki: {%2.2g, %2.2g, %2.2g}, kib: {%2.2g, %2.2g, %2.2g}", config_ki_[0],
                 config_ki_[1], config_ki_[2], config_kib_[0], config_kib_[1], config_kib_[2]);
  }

  if(level & (1 << 2))
  {
    kR_[0] = config.rot_x;
    kR_[1] = config.rot_y;
    kR_[2] = config.rot_z;

    kOm_[0] = config.ang_x;
    kOm_[1] = config.ang_y;
    kOm_[2] = config.ang_z;

    NODELET_INFO("Attitude Gains set to kp: {%2.2g, %2.2g, %2.2g}, kd: {%2.2g, %2.2g, %2.2g}", kR_[0], kR_[1], kR_[2],
                 kOm_[0], kOm_[1], kOm_[2]);
  }

  if(level & (1 << 3))
  {
    corrections_[0] = config.kf_correction;
    corrections_[1] = config.roll_correction;
    corrections_[2] = config.pitch_correction;
    NODELET_INFO("Corrections set to kf: %2.2g, roll: %2.2g, pitch: %2.2g", corrections_[0], corrections_[1],
                 corrections_[2]);
  }

  if(level & (1 << 4))
  {
    controller_.setMaxIntegral(config.max_pos_int);
    controller_.setMaxIntegralBody(config.max_pos_int_b);
    controller_.setMaxTiltAngle(config.max_tilt_angle);

    NODELET_INFO("Maxes set to Integral: %2.2g, Integral Body: %2.2g, Tilt Angle (rad): %2.2g", config.max_pos_int,
                 config.max_pos_int_b, config.max_tilt_angle);
  }

  NODELET_WARN_STREAM_COND(level != std::numeric_limits<uint32_t>::max() && (level >= (1 << 5)),
                           "kr_mav_controllers dynamic reconfigure called, but with unknown level: " << level);
}

void SO3TRPYControlNodelet::onInit()
{
  ros::NodeHandle n(getPrivateNodeHandle());

  std::string quadrotor_name;
  n.param("quadrotor_name", quadrotor_name, std::string("quadrotor"));
  frame_id_ = "/" + quadrotor_name;

  float mass;
  n.param("mass", mass, 0.5f);
  mass_ = mass;
  controller_.setMass(mass_);
  controller_.setGravity(g_);

  // Dynamic reconfigure struct
  kr_mav_controllers::SO3Config config;

  n.param("use_external_yaw", use_external_yaw_, true);

  n.param("gains/pos/x", config_kx_[0], 7.4f);
  n.param("gains/pos/y", config_kx_[1], 7.4f);
  n.param("gains/pos/z", config_kx_[2], 10.4f);
  kx_[0] = config_kx_[0];
  kx_[1] = config_kx_[1];
  kx_[2] = config_kx_[2];
  config.kp_x = config_kx_[0];
  config.kp_y = config_kx_[1];
  config.kp_z = config_kx_[2];

  n.param("gains/vel/x", config_kv_[0], 4.8f);
  n.param("gains/vel/y", config_kv_[1], 4.8f);
  n.param("gains/vel/z", config_kv_[2], 6.0f);
  kv_[0] = config_kv_[0];
  kv_[1] = config_kv_[1];
  kv_[2] = config_kv_[2];
  config.kd_x = config_kv_[0];
  config.kd_y = config_kv_[1];
  config.kd_z = config_kv_[2];

  n.param("gains/ki/x", config_ki_[0], 0.0f);
  n.param("gains/ki/y", config_ki_[1], 0.0f);
  n.param("gains/ki/z", config_ki_[2], 0.0f);
  config.ki_x = config_ki_[0];
  config.ki_y = config_ki_[1];
  config.ki_z = config_ki_[2];

  n.param("gains/ki/yaw", ki_yaw_, 0.0f);

  n.param("gains/kib/x", config_kib_[0], 0.0f);
  n.param("gains/kib/y", config_kib_[1], 0.0f);
  n.param("gains/kib/z", config_kib_[2], 0.0f);
  config.kib_x = config_kib_[0];
  config.kib_y = config_kib_[1];
  config.kib_z = config_kib_[2];

  n.param("gains/rot/x", kR_[0], 1.5f);
  n.param("gains/rot/y", kR_[1], 1.5f);
  n.param("gains/rot/z", kR_[2], 1.0f);
  n.param("gains/ang/x", kOm_[0], 0.13f);
  n.param("gains/ang/y", kOm_[1], 0.13f);
  n.param("gains/ang/z", kOm_[2], 0.1f);
  config.rot_x = kR_[0];
  config.rot_y = kR_[1];
  config.rot_z = kR_[2];
  config.ang_x = kOm_[0];
  config.ang_y = kOm_[1];
  config.ang_z = kOm_[2];

  n.param("corrections/kf", corrections_[0], 0.0f);
  n.param("corrections/r", corrections_[1], 0.0f);
  n.param("corrections/p", corrections_[2], 0.0f);
  config.kf_correction = corrections_[0];
  config.roll_correction = corrections_[1];
  config.pitch_correction = corrections_[2];

  float max_pos_int, max_pos_int_b;
  n.param("max_pos_int", max_pos_int, 0.5f);
  n.param("mas_pos_int_b", max_pos_int_b, 0.5f);
  controller_.setMaxIntegral(max_pos_int);
  controller_.setMaxIntegralBody(max_pos_int_b);
  config.max_pos_int = max_pos_int;
  config.max_pos_int_b = max_pos_int_b;

  float max_tilt_angle;
  n.param("max_tilt_angle", max_tilt_angle, static_cast<float>(M_PI));
  controller_.setMaxTiltAngle(max_tilt_angle);
  config.max_tilt_angle = max_tilt_angle;

  // Initialize dynamic reconfigure
  reconfigure_server_ = boost::make_shared<ReconfigureServer>(config_mutex_, n);
  reconfigure_server_->updateConfig(config);
  reconfigure_server_->setCallback(boost::bind(&SO3TRPYControlNodelet::cfg_callback, this, _1, _2));

  trpy_command_pub_ = n.advertise<kr_mav_msgs::TRPYCommand>("trpy_cmd", 10);

  odom_sub_ = n.subscribe("odom", 10, &SO3TRPYControlNodelet::odom_callback, this, ros::TransportHints().tcpNoDelay());
  position_cmd_sub_ = n.subscribe("position_cmd", 10, &SO3TRPYControlNodelet::position_cmd_callback, this,
                                  ros::TransportHints().tcpNoDelay());
  enable_motors_sub_ = n.subscribe("motors", 2, &SO3TRPYControlNodelet::enable_motors_callback, this,
                                   ros::TransportHints().tcpNoDelay());
  corrections_sub_ = n.subscribe("corrections", 10, &SO3TRPYControlNodelet::corrections_callback, this,
                                 ros::TransportHints().tcpNoDelay());
}

#include <pluginlib/class_list_macros.h>
PLUGINLIB_EXPORT_CLASS(SO3TRPYControlNodelet, nodelet::Nodelet);


//    Copyright 2017 Rainer Gemulla
//
//    Licensed 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.
/*
 * averageOutMatrix.cc
 *
 *  Created on: Oct 4, 2011
 *      Author: chteflio
 */

#include <boost/filesystem.hpp>
#include <boost/program_options.hpp>

#include <mpi2/mpi2.h>
#include <mf/mf.h>

using namespace std;
using namespace mf;
using namespace mpi2;
using namespace boost::program_options;

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

	string inFilenameTrain;
	string outFilenameTrain;
	string inFilenameTest;
	string outFilenameTest;

	// read command line
	options_description desc("Options");
	desc.add_options()
		("help", "produce help message")
	    ("inTrain", value<string>(&inFilenameTrain), "input file with training data")
	    ("inTest", value<string>(&inFilenameTest), "input file with test data")
		("outTrain", value<string>(&outFilenameTrain), "output file with training data")
		("outTest", value<string>(&outFilenameTest), "output file with test data");
	;

	positional_options_description pdesc;
	pdesc.add("inTrain", 1);
	pdesc.add("inTest", 2);
	pdesc.add("outTrain", 3);
	pdesc.add("outTest", 4);

	variables_map vm;
	store(command_line_parser(argc, argv).options(desc).positional(pdesc).run(), vm);
	notify(vm);

	if (vm.count("help") || vm.count("inTrain")==0 || vm.count("outTrain")==0) {
		cout << "mfdblock [options] <inTrain> <outTrain>" << endl << endl;
	    cout << desc << endl;
	    return 1;
	}

	// read Train matrix
	std::cout<<"reading matrix: "<<std::endl;
	SparseMatrix v;
	readMatrix(inFilenameTrain,v);

	// calculate average according to Train matrix
	std::cout<<"calculating average: "<<std::endl;
	double sum=0;
	const SparseMatrix::value_array_type& values = v.value_data();
	for (mf_size_type p=0; p<v.nnz(); p++) {
		sum+=values[p];
	}

	double average=sum/v.nnz();

	std::cout<<"average value: "<<average<<std::endl;

//	// average out train matrix
//	for (mf_size_type i=0;i<v.size1();i++){
//		for (mf_size_type j=0;j<v.size2();j++){
//			v(i,j)-=average;
//		}
//	}
	std::cout<<"update matrix: "<<std::endl;
	for (mf_size_type p=0; p<v.nnz(); p++) {
		v.value_data()[p]-=average;
	}
	//v-=average;
	std::cout<<"writing train..."<<std::endl;
	writeMatrix(outFilenameTrain,v);

	// and now do average out the test matrix
	if (vm.count("inTest")!=0 && vm.count("outTest")!=0 ){
		std::cout<<"reading matrix: "<<std::endl;
		SparseMatrix vTest;
		readMatrix(inFilenameTest,vTest);
//		const SparseMatrix::index_array_type& indexTest1 = vTest.index1_data();
//		const SparseMatrix::index_array_type& indexTest2 = vTest.index2_data();

		std::cout<<"update matrix: "<<std::endl;
		for (mf_size_type p=0; p<vTest.nnz(); p++) {
			vTest.value_data()[p]-=average;
		}
		//vTest-=average;
		std::cout<<"writing test..."<<std::endl;
		writeMatrix(outFilenameTest,vTest);
	}

	return 0;
}


/**
* TESTING FILE
*
* Tests implemented with BOOST Library.
* Make sure boost is linked by adding in additional included libraries (VS).
*
*/

#define BOOST_TEST_MODULE _HullTesting

#include <boost/test/included/unit_test.hpp>
#include <boost/test/data/test_case.hpp>

#include "InitPoints.h"
#include "ReadFromFile.h"
#include "ConvexHull.h"

#define CQUEUE_SPLIT_TESTS 0
#if CQUEUE_SPLIT_TESTS
int paramsI[] = { 2,30,40,50 };
BOOST_DATA_TEST_CASE(CQueueSplitsTestI, boost::unit_test::data::make(paramsI), myVar) {
	std::vector<int> values = { 2,30,40,50 };
	Node<int>* leaf1 = new Node<int>(&values[0]);
	Node<int>* leaf2 = new Node<int>(&values[1]);
	Node<int>* leaf3 = new Node<int>(&values[2]);
	Node<int>* leaf4 = new Node<int>(&values[3]);

	std::vector<Node<int>*> leaves = {
	leaf1, leaf2, leaf3, leaf4
	};

	for (int i = 3; i > 0; --i) {
		leaves[i]->setChainLeft(leaves[i - 1]);
	}

	Node<int>* l_internal = new Node<int>(leaf1, leaf1, leaf2);
	Node<int>* r_internal = new Node<int>(leaf3, leaf3, leaf4);

	Node<int>* root = new Node<int>(leaf2, l_internal, r_internal);
	CQueue<int>* CQ = new CQueue<int>(root, 2, leaf1, leaf4);
	CQueue<int>* CQ_less;

	CQ_less = CQ->Split(myVar, true, true);

	CQ->isValidTree();
	CQ_less->isValidTree();

	delete CQ;
	delete CQ_less;
}

int paramsII[] = { 5,10,30,40,50,60 };
BOOST_DATA_TEST_CASE(CQueueSplitsTestII, boost::unit_test::data::make(paramsII), myVar) {
	std::vector<int> values = { 5,10,30,40,50,60 };
	Node<int>* leaf1 = new Node<int>(&values[0]);
	Node<int>* leaf2 = new Node<int>(&values[1]);
	Node<int>* leaf3 = new Node<int>(&values[2]);
	Node<int>* leaf4 = new Node<int>(&values[3]);
	Node<int>* leaf5 = new Node<int>(&values[4]);
	Node<int>* leaf6 = new Node<int>(&values[5]);

	std::vector<Node<int>*> leaves = {
	leaf1, leaf2, leaf3, leaf4, leaf5, leaf6
	};

	for (int i = 5; i > 0; --i) {
		leaves[i]->setChainLeft(leaves[i - 1]);
	}

	Node<int>* l_internal = new Node<int>(leaf1, nullptr, leaf2, leaf1, leaf2, nullptr, leaf3);
	Node<int>* r_internal = new Node<int>(leaf4, nullptr, leaf5, leaf4, leaf5, nullptr, leaf6);

	Node<int>* root = new Node<int>(leaf3, l_internal, r_internal);

	CQueue<int>* CQ = new CQueue<int>(root, 2, leaf1, leaf6);
	CQueue<int>* CQ_less;

	CQ_less = CQ->Split(myVar, true, true);

	CQ->isValidTree();
	CQ_less->isValidTree();

	delete CQ;
	delete CQ_less;

}

int paramsIII[] = { 2,5,20,30,35,40,50,60,65,70,75,80 };

BOOST_DATA_TEST_CASE(CQueueSplitsTestIII, boost::unit_test::data::make(paramsIII), myVar) {
	std::vector<int> values = { 2,5,20,30,35,40,50,60,65,70,75,80 };
	Node<int>* leaf1 = new Node<int>(&values[0], &values[1]);
	Node<int>* leaf2 = new Node<int>(&values[2], &values[3]);
	Node<int>* leaf3 = new Node<int>(&values[4], &values[5]);
	Node<int>* leaf4 = new Node<int>(&values[6], &values[7]);
	Node<int>* leaf5 = new Node<int>(&values[8], &values[9]);
	Node<int>* leaf6 = new Node<int>(&values[10], &values[11]);

	std::vector<Node<int>*> leaves = {
	leaf1, leaf2, leaf3, leaf4, leaf5, leaf6
	};

	for (int i = 5; i > 0; --i) {
		leaves[i]->setChainLeft(leaves[i - 1]);
	}

	Node<int>* l_internal = new Node<int>(leaf1, leaf1, leaf2);
	Node<int>* m_internal = new Node<int>(leaf3, leaf3, leaf4);
	Node<int>* r_internal = new Node<int>(leaf5, leaf5, leaf6);

	Node<int>* root = new Node<int>(leaf2, nullptr, leaf4, l_internal, m_internal, nullptr, r_internal);
	CQueue<int>* CQ = new CQueue<int>(root, 2, leaf1, leaf6);
	CQueue<int>* CQ_less;

	CQ_less = CQ->Split(myVar, true, true);

	CQ->isValidTree();
	CQ_less->isValidTree();

	delete CQ;
	delete CQ_less;
}

int paramsIV[] = { 2,5,8,10,15,20,25,30,32,35,38,40,45,50,55,60,65,70,72,75,78,80,85,90,95,100,105,110,115,120,125,130 };
BOOST_DATA_TEST_CASE(CQueueSplitsTestIV, boost::unit_test::data::make(paramsIV), myVar) {
	std::vector<int> values = { 2,5,8,10,15,20,25,30,32,35,38,40,45,50,55,60,65,70,72,75,78,80,85,90,95,100,105,110,115,120,125,130 };
	Node<int>* leaf1 = new Node<int>(&values[0], &values[1]);
	Node<int>* leaf2 = new Node<int>(&values[2], &values[3]);
	Node<int>* leaf3 = new Node<int>(&values[4], &values[5]);
	Node<int>* leaf4 = new Node<int>(&values[6], &values[7]);
	Node<int>* leaf5 = new Node<int>(&values[8], &values[9]);
	Node<int>* leaf6 = new Node<int>(&values[10], &values[11]);
	Node<int>* leaf7 = new Node<int>(&values[12], &values[13]);
	Node<int>* leaf8 = new Node<int>(&values[14], &values[15]);
	Node<int>* leaf9 = new Node<int>(&values[16], &values[17]);
	Node<int>* leaf10 = new Node<int>(&values[18], &values[19]);
	Node<int>* leaf11 = new Node<int>(&values[20], &values[21]);
	Node<int>* leaf12 = new Node<int>(&values[22], &values[23]);
	Node<int>* leaf13 = new Node<int>(&values[24], &values[25]);
	Node<int>* leaf14 = new Node<int>(&values[26], &values[27]);
	Node<int>* leaf15 = new Node<int>(&values[28], &values[29]);
	Node<int>* leaf16 = new Node<int>(&values[30], &values[31]);

	std::vector<Node<int>*> leaves = {
		leaf1, leaf2, leaf3, leaf4, leaf5, leaf6, leaf7, leaf8, leaf9, leaf10, leaf11, leaf12, leaf13, leaf14, leaf15, leaf16
	};

	for (int i = 15; i > 0; --i) {
		leaves[i]->setChainLeft(leaves[i - 1]);
	}

	Node<int>* l_internal = new Node<int>(leaf1, leaf2, leaf3, leaf1, leaf2, leaf3, leaf4);
	Node<int>* m1_internal = new Node<int>(leaf5, leaf6, leaf7, leaf5, leaf6, leaf7, leaf8);
	Node<int>* m2_internal = new Node<int>(leaf9, leaf10, leaf11, leaf9, leaf10, leaf11, leaf12);
	Node<int>* r_internal = new Node<int>(leaf13, leaf14, leaf15, leaf13, leaf14, leaf15, leaf16);

	Node<int>* root = new Node<int>(leaf4, leaf8, leaf12, l_internal, m1_internal, m2_internal, r_internal);

	CQueue<int>* CQ = new CQueue<int>(root, 2, leaf1, leaf16);
	CQueue<int>* CQ_less;

	CQ_less = CQ->Split(myVar, true, true);

	CQ->isValidTree();
	CQ_less->isValidTree();

	delete CQ;
	delete CQ_less;
}

int paramsV[] = { 0,2,5,20,25,30,35,37,40,45,47,50,52,53,55,56,58,60,64,68,70,72,76,80,82,86,90,92,96,100,105,110,120,130,140,150 };

BOOST_DATA_TEST_CASE(CQueueSplitsTestV, boost::unit_test::data::make(paramsV), myVar) {
	std::vector<int> values = { 0,2,5,20,25,30,35,37,40,45,47,50,52,53,55,56,58,60,64,68,70,72,76,80,82,86,90,92,96,100,105,110,120,130,140,150 };

	Node<int>* leaf1 = new Node<int>(&values[0], &values[1], &values[2]);
	Node<int>* leaf2 = new Node<int>(&values[3], &values[4], &values[5]);
	Node<int>* leaf3 = new Node<int>(&values[6], &values[7], &values[8]);
	Node<int>* leaf4 = new Node<int>(&values[9], &values[10], &values[11]);
	Node<int>* leaf5 = new Node<int>(&values[12], &values[13], &values[14]);
	Node<int>* leaf6 = new Node<int>(&values[15], &values[16], &values[17]);
	Node<int>* leaf7 = new Node<int>(&values[18], &values[19], &values[20]);
	Node<int>* leaf8 = new Node<int>(&values[21], &values[22], &values[23]);
	Node<int>* leaf9 = new Node<int>(&values[24], &values[25], &values[26]);
	Node<int>* leaf10 = new Node<int>(&values[27], &values[28], &values[29]);
	Node<int>* leaf11 = new Node<int>(&values[30], &values[31], &values[32]);
	Node<int>* leaf12 = new Node<int>(&values[33], &values[34], &values[35]);

	std::vector<Node<int>*> leaves = {
	leaf1, leaf2, leaf3, leaf4, leaf5, leaf6, leaf7, leaf8, leaf9, leaf10, leaf11, leaf12
	};

	for (int i = 11; i > 0; --i) {
		leaves[i]->setChainLeft(leaves[i - 1]);
	}

	Node<int>* lowerl1internal = new Node<int>(leaf1, leaf1, leaf2);
	Node<int>* lowerm1internal = new Node<int>(leaf3, leaf3, leaf4);
	Node<int>* lowerr1internal = new Node<int>(leaf5, leaf5, leaf6);
	Node<int>* lowerl2internal = new Node<int>(leaf7, leaf7, leaf8);
	Node<int>* lowerm2internal = new Node<int>(leaf9, leaf9, leaf10);
	Node<int>* lowerr2internal = new Node<int>(leaf11, leaf11, leaf12);

	Node<int>* l_internal = new Node<int>(leaf2, nullptr, leaf4, lowerl1internal, lowerm1internal, nullptr, lowerr1internal);
	Node<int>* r_internal = new Node<int>(leaf8, nullptr, leaf10, lowerl2internal, lowerm2internal, nullptr, lowerr2internal);

	Node<int>* root = new Node<int>(leaf6, l_internal, r_internal);

	CQueue<int>* CQ = new CQueue<int>(root, 3, leaf1, leaf12);
	CQueue<int>* CQ_less;

	CQ_less = CQ->Split(myVar, true, true);

	CQ->isValidTree();
	CQ_less->isValidTree();

	delete CQ;
	delete CQ_less;

	BOOST_TEST_MESSAGE("Paramvalue" << myVar);
	BOOST_TEST(true);
}

int paramsVI[] = { 5, 10,30, 40, 50, 60, 70, 80, 90, 100, 110, 120 };
BOOST_DATA_TEST_CASE(CQueueSplitsTestVI, boost::unit_test::data::make(paramsVI), myVar) {
	std::vector<int> values = { 5, 10,30, 40, 50, 60, 70, 80, 90, 100, 110, 120 };

	Node<int>* leaf1 = new Node<int>(&values[0]);
	Node<int>* leaf2 = new Node<int>(&values[1]);
	Node<int>* leaf3 = new Node<int>(&values[2]);
	Node<int>* leaf4 = new Node<int>(&values[3]);
	Node<int>* leaf5 = new Node<int>(&values[4]);
	Node<int>* leaf6 = new Node<int>(&values[5]);
	Node<int>* leaf7 = new Node<int>(&values[6]);
	Node<int>* leaf8 = new Node<int>(&values[7]);
	Node<int>* leaf9 = new Node<int>(&values[8]);
	Node<int>* leaf10 = new Node<int>(&values[9]);
	Node<int>* leaf11 = new Node<int>(&values[10]);
	Node<int>* leaf12 = new Node<int>(&values[11]);

	std::vector<Node<int>*> leaves = {
	leaf1, leaf2, leaf3, leaf4, leaf5, leaf6, leaf7, leaf8, leaf9, leaf10, leaf11, leaf12
	};

	for (int i = 11; i > 0; --i) {
		leaves[i]->setChainLeft(leaves[i - 1]);
	}

	Node<int>* l_internal = new Node<int>(leaf1, nullptr, leaf2, leaf1, leaf2, nullptr, leaf3);
	Node<int>* m1_internal = new Node<int>(leaf4, nullptr, leaf5, leaf4, leaf5, nullptr, leaf6);
	Node<int>* m2_internal = new Node<int>(leaf7, nullptr, leaf8, leaf7, leaf8, nullptr, leaf9);
	Node<int>* r_internal = new Node<int>(leaf10, nullptr, leaf11, leaf10, leaf11, nullptr, leaf12);

	Node<int>* root = new Node<int>(leaf3, leaf6, leaf9, l_internal, m1_internal, m2_internal, r_internal);

	CQueue<int>* CQ = new CQueue<int>(root, 2, leaf1, leaf12);
	CQueue<int>* CQ_less;

	CQ_less = CQ->Split(myVar, true, true);

	CQ->isValidTree();
	CQ_less->isValidTree();

	delete CQ;
	delete CQ_less;

}

int paramsVII[] = { 10, 30, 50, 60, 80, 90, 100, 110 };

BOOST_DATA_TEST_CASE(CQueueSplitsTestVII, boost::unit_test::data::make(paramsVII), myVar) {
	std::vector<int> values = { 10, 30, 50, 60, 80, 90, 100, 110 };

	Node<int>* leaf1 = new Node<int>(&values[0]);
	Node<int>* leaf2 = new Node<int>(&values[1]);
	Node<int>* leaf3 = new Node<int>(&values[2]);
	Node<int>* leaf4 = new Node<int>(&values[3]);
	Node<int>* leaf5 = new Node<int>(&values[4]);
	Node<int>* leaf6 = new Node<int>(&values[5]);
	Node<int>* leaf7 = new Node<int>(&values[6]);
	Node<int>* leaf8 = new Node<int>(&values[7]);

	std::vector<Node<int>*> leaves = {
	leaf1, leaf2, leaf3, leaf4, leaf5, leaf6, leaf7, leaf8
	};

	for (int i = 7; i > 0; --i) {
		leaves[i]->setChainLeft(leaves[i - 1]);
	}

	Node<int>* l_internal = new Node<int>(leaf1, leaf1, leaf2);
	Node<int>* m1_internal = new Node<int>(leaf3, leaf3, leaf4);
	Node<int>* m2_internal = new Node<int>(leaf5, leaf5, leaf6);
	Node<int>* r_internal = new Node<int>(leaf7, leaf7, leaf8);

	Node<int>* root = new Node<int>(leaf2, leaf4, leaf6, l_internal, m1_internal, m2_internal, r_internal);

	CQueue<int>* CQ = new CQueue<int>(root, 2, leaf1, leaf8);
	CQueue<int>* CQ_less;

	CQ_less = CQ->Split(myVar, true, true);

	CQ->isValidTree();
	CQ_less->isValidTree();

	delete CQ;
	delete CQ_less;

}

#endif

#define ONLINE 0
#if ONLINE
int dataSetSize[] = { 100000, 200000, 300000, 400000, 500000, 600000, 700000, 800000, 900000, 1000000 };
BOOST_DATA_TEST_CASE(OnlineRandomPointsConvexHullTest, boost::unit_test::data::make(dataSetSize), numOfPoints)
{

	Instrumentor::Get().BeginSession("Profile", "profiling_results/results_" + std::to_string(numOfPoints) + ".json");

	std::random_device rd;
	std::mt19937 g(rd());
	std::uniform_real_distribution<double> distribution(-1000000, 1000000);

	Point_2D* p;
	ConvexHull<Point_2D> ch;
	std::vector<Point_2D*> points;

	{
		PROFILE_FUNCTION();

		for (int i = 0; i < numOfPoints; ++i) {
			p = new Point_2D(0.0 + distribution(g), 1.0 * distribution(g));
			ch.Insert(p);
		}
	}

	ch.OutputHull("test_results/Output_" + std::to_string(numOfPoints) + ".txt");

	ch.OutputPoints("test_points/Test_Points_" + std::to_string(numOfPoints) + ".txt");

	ch.DeletePoints();

	Instrumentor::Get().EndSession();
}
#endif

#define CIRCLE_DISTR 0
#if CIRCLE_DISTR
int dataCircleSetSize[] = { 100000, 200000, 300000, 400000, 500000, 600000, 700000, 800000, 900000, 1000000 };
BOOST_DATA_TEST_CASE(RandomCirclePointsConvexHullTest, boost::unit_test::data::make(dataCircleSetSize), numOfPoints)
{

	Instrumentor::Get().BeginSession("Profile", "profiling_results/circle_results_" + std::to_string(numOfPoints) + ".json");

	unsigned seed = std::chrono::system_clock::now().time_since_epoch().count();
	std::mt19937 rngAngle(seed);
	seed = std::chrono::system_clock::now().time_since_epoch().count();
	std::mt19937 rngRadius(seed);

	std::uniform_real_distribution<double> angleDistribution(0, 360);
	std::uniform_real_distribution<double> radiusDistribution(0, 1000000);
	double angle, radius, x, y;

	Point_2D* p;
	ConvexHull<Point_2D> ch;
	std::vector<Point_2D*> points;

	{
		PROFILE_FUNCTION();

		for (int i = 0; i < numOfPoints; ++i) {
			angle = angleDistribution(rngAngle);
			radius = radiusDistribution(rngRadius);

			x = radius * sin(angle);
			y = radius * cos(angle);

			p = new Point_2D(x, y);
			ch.Insert(p);
		}
	}

	ch.OutputHull("test_results/Circle_Output_" + std::to_string(numOfPoints) + ".txt");

	ch.OutputPoints("test_points/Circle_Test_Points_" + std::to_string(numOfPoints) + ".txt");

	ch.DeletePoints();

	Instrumentor::Get().EndSession();
}
#endif

#define OFFLINE 0
#if OFFLINE
BOOST_AUTO_TEST_CASE(ReadFromFileOfflineConvexHullTest)
{
	int numOfPoints = 1000000;
	std::string timestamp(__TIME__);
	std::replace(timestamp.begin(), timestamp.end(), ':', '-');

	std::string testID(std::to_string(numOfPoints) + "__" + timestamp);

	Instrumentor::Get().BeginSession("Profile", "profiling_results/results_" + testID + ".json");

	std::pair<int, std::string> testVars = InitializeRandomPointsFile(numOfPoints, 50000);

	std::vector<Point_2D*> points = readPointsFromFile(testVars.first, testVars.second);

	ConvexHull<Point_2D> CH;
	{
		PROFILE_FUNCTION();
		for (Point_2D* p : points) {
			CH.Insert(p);
		}
	}

	CH.OutputHull("test_results/Output_" + testID + ".txt");

	for (Point_2D* p : points) {
		delete p;
	}

	Instrumentor::Get().EndSession();
}
#endif

#define DELETIONS 0
#if DELETIONS
BOOST_AUTO_TEST_CASE(Deletions)
{

	int numOfPoints = 200000;
	Instrumentor::Get().BeginSession("Profile", "profiling_results/deletions_results_" + std::to_string(numOfPoints) + ".json");

	std::random_device rd;
	std::mt19937 g(rd());
	std::uniform_real_distribution<double> distribution(-1000000, 1000000);

	Point_2D* p;
	ConvexHull<Point_2D> ch;
	std::vector<Point_2D*> points;

	{
		PROFILE_FUNCTION();

		for (int i = 0; i < numOfPoints; ++i) {
			p = new Point_2D(0.0 + distribution(g), 1.0 * distribution(g));
			points.push_back(p);
			ch.Insert(p);
		}
	}

	std::shuffle(points.begin(), points.end(), g);

	{
		PROFILE_FUNCTION();

		for (int i = 0; i < 100000; ++i) {
			ch.Delete(*points[i]);
		}
	}

	for (Point_2D* p : points) {
		delete p;
	}

	Instrumentor::Get().EndSession();
}
#endif