#include "HoloRender3DObject.hpp"
#include <boost/asio.hpp>
#include <future>
#include "Miniball.hpp"
#include <boost/asio.hpp>

using namespace holo;
using namespace holo::net;
using namespace holo::render;

HoloRender3DObject::HoloRender3DObject() :
	objectName_(),
	objectHeader_(),
	objectTransform_(),
	vertSize_(0),
	normalSize_(0),
	colorSize_(0),
	stringSize_(0),
	amOwner_(true),
	hasGLBuffers_(false),
	isLocal_(true)
{
	objectTransform_.bounding_sphere = {};
	objectTransform_.rotation = {};
	objectTransform_.scale = {};
	objectTransform_.translate = {};
}

HoloRender3DObject::HoloRender3DObject(const std::string objectName, unsigned int numIndecies, unsigned int numVertices, float *vertices, float * normals, float *colors, unsigned int numVertexDimensions, unsigned int numColorChannels) : HoloRender3DObject()
{
	isLocal_ = true;

	objectName_ = objectName;
	objectHeader_.num_indecies = numIndecies;
	objectHeader_.num_vertices = numVertices;
	objectHeader_.num_color_channels = numColorChannels;
	objectHeader_.num_points_per_vertex = numVertexDimensions;
	objectHeader_.vertex_stride = objectHeader_.num_points_per_vertex * sizeof(float);
	objectHeader_.color_stride = objectHeader_.num_color_channels * sizeof(float);

	// Launch vert/normal/color processing simultaneously
	std::future<void> vertFuture =
		std::async(std::launch::async,
		[&]()
	{
		vertSize_ = objectHeader_.vertex_stride * objectHeader_.num_vertices;
		vertices_ = new unsigned char[vertSize_];
		// copy and vertices floats from object file buffer
		memcpy(vertices_, vertices, vertSize_);
	});

	std::future<void> normalFuture =
		std::async(std::launch::async,
		[&]()
	{
		if (normals)
		{
			normalSize_ = objectHeader_.vertex_stride * objectHeader_.num_vertices;
			normals_ = new unsigned char[normalSize_];
			// copy normals from object file buffer
			memcpy(normals_, normals, normalSize_);
		}
		else
			normals_ = nullptr;
	});

	std::future<void> colorFuture =
		std::async(std::launch::async,
		[&]()
	{
		if (colors)
		{
			colorSize_ = objectHeader_.color_stride * objectHeader_.num_vertices;
			colors_ = new unsigned char[colorSize_];
			// copy normals from object file buffer
			memcpy(colors_, colors, colorSize_);
		}
		else
			colors_ = nullptr;
	});

	//Use miniball algorithm to get the bounding sphere of the object quickly
	float **ap = new float*[objectHeader_.num_vertices];
	float * pv = vertices;
	for (int i = 0; i < objectHeader_.num_vertices; ++i) {
		ap[i] = pv;
		pv += objectHeader_.num_points_per_vertex;
	}

	// miniball uses a quick method of determining the bounding sphere of all the vertices
	auto miniball3f = Miniball::Miniball<Miniball::CoordAccessor<float**, float*>>(3, (float**)ap, (float**)(ap + numVertices));
	objectTransform_.bounding_sphere.x = miniball3f.center()[0];
	objectTransform_.bounding_sphere.y = miniball3f.center()[1];
	objectTransform_.bounding_sphere.z = miniball3f.center()[2];
	objectTransform_.bounding_sphere.w = miniball3f.squared_radius();

	objectTransform_.scale.x = 1.f;
	objectTransform_.scale.y = 1.f;
	objectTransform_.scale.z = 1.f;

	delete[] ap;

	stringSize_ = objectName_.size();

	vertFuture.wait();
	normalFuture.wait();
	colorFuture.wait();
}

HoloRender3DObject::HoloRender3DObject(const boost::shared_ptr<HoloNetPacket>& objectPacket) : HoloRender3DObject()
{
	amOwner_ = false;
	isLocal_ = false;

	auto packetValue = objectPacket->GetPacketValue();

	memcpy(&objectHeader_, packetValue, sizeof(objectHeader_));
	objectHeader_.num_vertices = boost::asio::detail::socket_ops::network_to_host_long(objectHeader_.num_vertices);
	objectHeader_.num_points_per_vertex = boost::asio::detail::socket_ops::network_to_host_long(objectHeader_.num_points_per_vertex);
	objectHeader_.num_color_channels = boost::asio::detail::socket_ops::network_to_host_long(objectHeader_.num_color_channels);
	objectHeader_.vertex_stride = boost::asio::detail::socket_ops::network_to_host_long(objectHeader_.vertex_stride);
	objectHeader_.color_stride = boost::asio::detail::socket_ops::network_to_host_long(objectHeader_.color_stride);
	objectHeader_.num_indecies = boost::asio::detail::socket_ops::network_to_host_long(objectHeader_.num_indecies);

	memcpy(&objectTransform_, packetValue + sizeof(objectHeader_), sizeof(objectTransform_));

	unsigned int x, y, z, w = 0;

	x = boost::asio::detail::socket_ops::network_to_host_long(reinterpret_cast<unsigned int&>(objectTransform_.translate.x));
	y = boost::asio::detail::socket_ops::network_to_host_long(reinterpret_cast<unsigned int&>(objectTransform_.translate.y));
	z = boost::asio::detail::socket_ops::network_to_host_long(reinterpret_cast<unsigned int&>(objectTransform_.translate.z));

	objectTransform_.translate.x = reinterpret_cast<float&>(x);
	objectTransform_.translate.y = reinterpret_cast<float&>(y);
	objectTransform_.translate.z = reinterpret_cast<float&>(z);

	x = boost::asio::detail::socket_ops::network_to_host_long(reinterpret_cast<unsigned int&>(objectTransform_.rotation.x));
	y = boost::asio::detail::socket_ops::network_to_host_long(reinterpret_cast<unsigned int&>(objectTransform_.rotation.y));
	z = boost::asio::detail::socket_ops::network_to_host_long(reinterpret_cast<unsigned int&>(objectTransform_.rotation.z));
	w = boost::asio::detail::socket_ops::network_to_host_long(reinterpret_cast<unsigned int&>(objectTransform_.rotation.w));

	objectTransform_.rotation.x = reinterpret_cast<float&>(x);
	objectTransform_.rotation.y = reinterpret_cast<float&>(y);
	objectTransform_.rotation.z = reinterpret_cast<float&>(z);
	objectTransform_.rotation.w = reinterpret_cast<float&>(w);

	x = boost::asio::detail::socket_ops::network_to_host_long(reinterpret_cast<unsigned int&>(objectTransform_.scale.x));
	y = boost::asio::detail::socket_ops::network_to_host_long(reinterpret_cast<unsigned int&>(objectTransform_.scale.y));
	z = boost::asio::detail::socket_ops::network_to_host_long(reinterpret_cast<unsigned int&>(objectTransform_.scale.z));

	objectTransform_.scale.x = reinterpret_cast<float&>(x);
	objectTransform_.scale.y = reinterpret_cast<float&>(y);
	objectTransform_.scale.z = reinterpret_cast<float&>(z);

	x = boost::asio::detail::socket_ops::network_to_host_long(reinterpret_cast<unsigned int&>(objectTransform_.bounding_sphere.x));
	y = boost::asio::detail::socket_ops::network_to_host_long(reinterpret_cast<unsigned int&>(objectTransform_.bounding_sphere.y));
	z = boost::asio::detail::socket_ops::network_to_host_long(reinterpret_cast<unsigned int&>(objectTransform_.bounding_sphere.z));
	w = boost::asio::detail::socket_ops::network_to_host_long(reinterpret_cast<unsigned int&>(objectTransform_.bounding_sphere.w));

	objectTransform_.bounding_sphere.x = reinterpret_cast<float&>(x);
	objectTransform_.bounding_sphere.y = reinterpret_cast<float&>(y);
	objectTransform_.bounding_sphere.z = reinterpret_cast<float&>(z);
	objectTransform_.bounding_sphere.w = reinterpret_cast<float&>(w);

	memcpy(&vertSize_, packetValue + sizeof(objectHeader_)+sizeof(objectTransform_), sizeof(vertSize_));
	memcpy(&normalSize_, packetValue + sizeof(objectHeader_)+sizeof(objectTransform_)+sizeof(vertSize_), sizeof(normalSize_));
	memcpy(&colorSize_, packetValue + sizeof(objectHeader_)+sizeof(objectTransform_)+sizeof(vertSize_)+sizeof(normalSize_), sizeof(colorSize_));
	memcpy(&stringSize_, packetValue + sizeof(objectHeader_)+sizeof(objectTransform_)+sizeof(vertSize_)+sizeof(normalSize_)+sizeof(stringSize_), sizeof(stringSize_));

	vertSize_ = boost::asio::detail::socket_ops::network_to_host_long(vertSize_);
	normalSize_ = boost::asio::detail::socket_ops::network_to_host_long(normalSize_);
	colorSize_ = boost::asio::detail::socket_ops::network_to_host_long(colorSize_);
	stringSize_ = boost::asio::detail::socket_ops::network_to_host_long(stringSize_);

	// Launch vert/normal/color processing simultaneously
	std::future<void> vertFuture =
		std::async(std::launch::async,
		[&]()
	{
		vertices_ = new unsigned char[vertSize_];
		// copy and convert vertices floats from network to host endianness
		memcpy(vertices_, packetValue + sizeof(objectHeader_)+sizeof(objectTransform_)+sizeof(vertSize_)+sizeof(normalSize_)+sizeof(colorSize_)+sizeof(stringSize_), vertSize_);
		float * realVal = (float*)vertices_;
		for (unsigned int i = 0; i < objectHeader_.num_vertices * objectHeader_.num_points_per_vertex; i++)
		{
			unsigned int netVal = boost::asio::detail::socket_ops::network_to_host_long(*reinterpret_cast<unsigned int*>(realVal));
			*realVal = reinterpret_cast<float&>(netVal);
			realVal++;
		}
	});

	std::future<void> normFuture =
		std::async(std::launch::async,
		[&]()
	{
		if (normalSize_ > 0)
		{
			normals_ = new unsigned char[normalSize_];
			// copy and convert normal floats from network to host endianness
			memcpy(normals_, packetValue + sizeof(objectHeader_)+sizeof(objectTransform_)+sizeof(vertSize_)+sizeof(normalSize_)+sizeof(colorSize_)+sizeof(stringSize_)+vertSize_, normalSize_);
			float * realVal = (float*)normals_;
			for (unsigned int i = 0; i < objectHeader_.num_vertices * objectHeader_.num_points_per_vertex; i++)
			{
				unsigned int netVal = boost::asio::detail::socket_ops::network_to_host_long(*reinterpret_cast<unsigned int*>(realVal));
				*realVal = reinterpret_cast<float&>(netVal);
				realVal++;
			}
		}
		else
			normals_ = nullptr;
	});

	std::future<void> colorFuture =
		std::async(std::launch::async,
		[&]()
	{
		if (colorSize_ > 0)
		{
			colors_ = new unsigned char[colorSize_];
			// copy and convert color floats from network to host endianness
			memcpy(colors_, packetValue + sizeof(objectHeader_)+sizeof(objectTransform_)+sizeof(vertSize_)+sizeof(normalSize_)+sizeof(colorSize_)+sizeof(stringSize_)+vertSize_ + normalSize_, colorSize_);
			float * realVal = (float*)colors_;
			for (unsigned int i = 0; i < objectHeader_.num_vertices * objectHeader_.num_color_channels; i++)
			{
				unsigned int netVal = boost::asio::detail::socket_ops::network_to_host_long(*reinterpret_cast<unsigned int*>(realVal));
				*realVal = reinterpret_cast<float&>(netVal);
				realVal++;
			}
		}
		else
			colors_ = nullptr;
	});

	// create the name string
	objectName_.resize(stringSize_);
	memcpy((void*)objectName_.data(), packetValue + sizeof(objectHeader_)+sizeof(objectTransform_)+sizeof(vertSize_)+sizeof(normalSize_)+sizeof(colorSize_)+sizeof(stringSize_)+vertSize_ + normalSize_ + colorSize_, stringSize_);

	// Wait for them to be finished
	vertFuture.wait();
	normFuture.wait();
	colorFuture.wait();
}

boost::shared_ptr<HoloRender3DObject> HoloRender3DObject::CreateSimpleObject(const std::string objectName, SIMPLE_OBJECT objectType, float x, float y, float z, float size)
{

	HoloTransform trans = { 0 };
	boost::shared_ptr<HoloRender3DObject> obj;
	unsigned int numIndecies = 0;
	unsigned int numVertices = 0;

	std::vector<float> vertices;
	std::vector<float> normals;
	std::vector<float> colors;

	switch (objectType)
	{
	case SIMPLE_OBJECT_CIRCLE:
		numIndecies = 4;
		numVertices = 4 * 6;
		vertices = {
			1.f, 1.f, -1.f, -1.f, 1.f, -1.f, -1.f, 1.f, 1.f, 1.f, 1.f, 1.f,		//top panel
			1.f, -1.f, 1.f, -1.f, -1.f, 1.f, -1.f, -1.f, -1.f, 1.f, -1.f, -1.f,	//bottom panel
			1.f, 1.f, 1.f, -1.f, 1.f, 1.f, -1.f, -1.f, 1.f, 1.f, -1.f, 1.f,		//front panel
			1.f, -1.f, -1.f, -1.f, -1.f, -1.f, -1.f, 1.f, -1.f, 1.f, 1.f, -1.f,	//back panel
			-1.f, 1.f, 1.f, -1.f, 1.f, -1.f, -1.f, -1.f, -1.f, -1.f, -1.f, 1.f,	//left panel
			1.f, 1.f, -1.f, 1.f, 1.f, 1.f, 1.f, -1.f, 1.f, 1.f, -1.f, -1.f			//right panel
		};

		normals = {
			0.f, 1.f, 0.f, 0.f, 1.f, 0.f, 0.f, 1.f, 0.f, 0.f, 1.f, 0.f,
			0.f, -1.f, 0.f, 0.f, -1.f, 0.f, 0.f, -1.f, 0.f, 0.f, -1.f, 0.f,
			0.f, 0.f, 1.f, 0.f, 0.f, 1.f, 0.f, 0.f, 1.f, 0.f, 0.f, 1.f,
			0.f, 0.f, -1.f, 0.f, 0.f, -1.f, 0.f, 0.f, -1.f, 0.f, 0.f, -1.f,
			-1.f, 0.f, 0.f, -1.f, 0.f, 0.f, -1.f, 0.f, 0.f, -1.f, 0.f, 0.f,
			1.f, 0.f, 0.f, 1.f, 0.f, 0.f, 1.f, 0.f, 0.f, 1.f, 0.f, 0.f
		};

		colors = {
			0.f, 1.f, 0.f, 0.f, 1.f, 0.f, 0.f, 1.f, 0.f, 0.f, 1.f, 0.f,
			1.f, 0.5f, 0.f, 1.f, 0.5f, 0.f, 1.f, 0.5f, 0.f, 1.f, 0.5f, 0.f,
			1.f, 0.f, 0.f, 1.f, 0.f, 0.f, 1.f, 0.f, 0.f, 1.f, 0.f, 0.f,
			1.f, 1.f, 0.f, 1.f, 1.f, 0.f, 1.f, 1.f, 0.f, 1.f, 1.f, 0.f,
			0.f, 0.f, 1.f, 0.f, 0.f, 1.f, 0.f, 0.f, 1.f, 0.f, 0.f, 1.f,
			1.f, 0.f, 1.f, 1.f, 0.f, 1.f, 1.f, 0.f, 1.f, 1.f, 0.f, 1.f
		};

		trans.bounding_sphere.x = 0.f;
		trans.bounding_sphere.y = 0.f;
		trans.bounding_sphere.z = 0.f;
		trans.bounding_sphere.w = 4.f;

		trans.translate.x = x;
		trans.translate.y = y;
		trans.translate.z = z;

		trans.scale.x = size;
		trans.scale.y = size;
		trans.scale.z = 0.f;
		break;
	case SIMPLE_OBJECT_SQUARE:
	{
		numIndecies = 4;
		numVertices = 4 * 6;
		vertices = {
			1.f, 1.f, -1.f, -1.f, 1.f, -1.f, -1.f, 1.f, 1.f, 1.f, 1.f, 1.f,		//top panel
			1.f, -1.f, 1.f, -1.f, -1.f, 1.f, -1.f, -1.f, -1.f, 1.f, -1.f, -1.f,	//bottom panel
			1.f, 1.f, 1.f, -1.f, 1.f, 1.f, -1.f, -1.f, 1.f, 1.f, -1.f, 1.f,		//front panel
			1.f, -1.f, -1.f, -1.f, -1.f, -1.f, -1.f, 1.f, -1.f, 1.f, 1.f, -1.f,	//back panel
			-1.f, 1.f, 1.f, -1.f, 1.f, -1.f, -1.f, -1.f, -1.f, -1.f, -1.f, 1.f,	//left panel
			1.f, 1.f, -1.f, 1.f, 1.f, 1.f, 1.f, -1.f, 1.f, 1.f, -1.f, -1.f			//right panel
		};

		normals = {
			0.f, 1.f, 0.f, 0.f, 1.f, 0.f, 0.f, 1.f, 0.f, 0.f, 1.f, 0.f,
			0.f, -1.f, 0.f, 0.f, -1.f, 0.f, 0.f, -1.f, 0.f, 0.f, -1.f, 0.f,
			0.f, 0.f, 1.f, 0.f, 0.f, 1.f, 0.f, 0.f, 1.f, 0.f, 0.f, 1.f,
			0.f, 0.f, -1.f, 0.f, 0.f, -1.f, 0.f, 0.f, -1.f, 0.f, 0.f, -1.f,
			-1.f, 0.f, 0.f, -1.f, 0.f, 0.f, -1.f, 0.f, 0.f, -1.f, 0.f, 0.f,
			1.f, 0.f, 0.f, 1.f, 0.f, 0.f, 1.f, 0.f, 0.f, 1.f, 0.f, 0.f
		};

		colors = {
			0.f, 1.f, 0.f, 0.f, 1.f, 0.f, 0.f, 1.f, 0.f, 0.f, 1.f, 0.f,
			1.f, 0.5f, 0.f, 1.f, 0.5f, 0.f, 1.f, 0.5f, 0.f, 1.f, 0.5f, 0.f,
			1.f, 0.f, 0.f, 1.f, 0.f, 0.f, 1.f, 0.f, 0.f, 1.f, 0.f, 0.f,
			1.f, 1.f, 0.f, 1.f, 1.f, 0.f, 1.f, 1.f, 0.f, 1.f, 1.f, 0.f,
			0.f, 0.f, 1.f, 0.f, 0.f, 1.f, 0.f, 0.f, 1.f, 0.f, 0.f, 1.f,
			1.f, 0.f, 1.f, 1.f, 0.f, 1.f, 1.f, 0.f, 1.f, 1.f, 0.f, 1.f
		};

		trans.bounding_sphere.x = 0.f;
		trans.bounding_sphere.y = 0.f;
		trans.bounding_sphere.z = 0.f;
		trans.bounding_sphere.w = 4.f;

		trans.translate.x = x;
		trans.translate.y = y;
		trans.translate.z = z;

		trans.scale.x = size;
		trans.scale.y = size;
		trans.scale.z = 0.f;
	}

		break;
	case SIMPLE_OBJECT_TRIANGLE:
	{
		numIndecies = 3;
		numVertices = 6;

		vertices = {
			-0.5f, 0.f, 0.5f,
			0.f, 1.f, 0.5f,
			0.5f, 1.f, 0.5f,
			-0.5f, 0.f, -0.5f,
			0.f, 1.f, -0.5f,
			0.5f, 1.f, -0.5f
		};

		normals = {
			0.f, 0.f, 1.f,
			0.f, 0.f, 1.f,
			0.f, 0.f, 1.f,
			0.f, 0.f, -1.f,
			0.f, 0.f, -1.f,
			0.f, 0.f, -1.f
		};

		colors = {
			1.f,0.f,0.f,
			0.f,1.f,0.f,
			0.f,0.f,1.f,
			1.f, 0.f, 0.f,
			0.f, 1.f, 0.f,
			0.f, 0.f, 1.f
		};

		trans.bounding_sphere.x = 0.f;
		trans.bounding_sphere.y = 0.5f;
		trans.bounding_sphere.z = 0.f;
		trans.bounding_sphere.w = 4.f;

		trans.translate.x = x;
		trans.translate.y = y;
		trans.translate.z = z;

		trans.scale.x = size;
		trans.scale.y = size;
		trans.scale.z = size;
	}

		break;
	case SIMPLE_OBJECT_POINTER:
		numIndecies = 3;
		numVertices = 6;

		vertices = {
			-0.5f, 0.f, 0.5f,
			0.f, 1.f, 0.5f,
			0.5f, 1.f, 0.5f,
			-0.5f, 0.f, -0.5f,
			0.f, 1.f, -0.5f,
			0.5f, 1.f, -0.5f
		};

		normals = {
			0.f, 0.f, 1.f,
			0.f, 0.f, 1.f,
			0.f, 0.f, 1.f,
			0.f, 0.f, -1.f,
			0.f, 0.f, -1.f,
			0.f, 0.f, -1.f
		};

		colors = {
			1.f, 0.f, 0.f,
			0.f, 1.f, 0.f,
			0.f, 0.f, 1.f,
			1.f, 0.f, 0.f,
			0.f, 1.f, 0.f,
			0.f, 0.f, 1.f
		};

		trans.bounding_sphere.x = 0.f;
		trans.bounding_sphere.y = 0.f;
		trans.bounding_sphere.z = 0.f;
		trans.bounding_sphere.w = 4.f;

		trans.translate.x = x;
		trans.translate.y = y;
		trans.translate.z = z;

		trans.scale.x = size;
		trans.scale.y = size;
		trans.scale.z = size;
		break;

	default:
		break;
	}

	obj = boost::shared_ptr<HoloRender3DObject>(new HoloRender3DObject(objectName,
		numIndecies,
		numVertices,
		vertices.data(),
		normals.data(),
		colors.size() > 0 ? colors.data() : nullptr,
		3,
		3));
	obj->SetTransform(trans);

	return obj;
}

const boost::shared_ptr<HoloNetPacket> HoloRender3DObject::CreateNetPacket() const
{
	auto netPacket = boost::shared_ptr<HoloNetPacket>(new HoloNetPacket(HOLO_NET_PACKET_TYPE_OBJECT_ADD, sizeof(objectHeader_)+sizeof(objectTransform_) + 4 * sizeof(unsigned int) + vertSize_ + normalSize_ + colorSize_ + stringSize_));
	auto packetValue = netPacket->GetPacketValue();
	//netPacket->type = HOLO_NET_PACKET_TYPE_OBJECT_ADD;
	//netPacket->length = sizeof(objectHeader_) + 4 * sizeof(unsigned int)+vertSize_ + normalSize_ + colorSize_ + vertSize_ + stringSize_;
	//netPacket->value.resize(netPacket->length);

	// convert info to network byte-type, and copy to value buffer
	auto objHeader = objectHeader_;
	objHeader.num_vertices = boost::asio::detail::socket_ops::host_to_network_long(objHeader.num_vertices);
	objHeader.num_points_per_vertex = boost::asio::detail::socket_ops::host_to_network_long(objHeader.num_points_per_vertex);
	objHeader.num_color_channels = boost::asio::detail::socket_ops::host_to_network_long(objHeader.num_color_channels);
	objHeader.vertex_stride = boost::asio::detail::socket_ops::host_to_network_long(objHeader.vertex_stride);
	objHeader.color_stride = boost::asio::detail::socket_ops::host_to_network_long(objHeader.color_stride);
	objHeader.num_indecies = boost::asio::detail::socket_ops::host_to_network_long(objHeader.num_indecies);
	memcpy(packetValue, &objHeader, sizeof(objHeader));

	auto objectTransform = objectTransform_;

	unsigned int x, y, z, w = 0;

	x = boost::asio::detail::socket_ops::host_to_network_long(reinterpret_cast<unsigned int&>(objectTransform.translate.x));
	y = boost::asio::detail::socket_ops::host_to_network_long(reinterpret_cast<unsigned int&>(objectTransform.translate.y));
	z = boost::asio::detail::socket_ops::host_to_network_long(reinterpret_cast<unsigned int&>(objectTransform.translate.z));

	objectTransform.translate.x = reinterpret_cast<float&>(x);
	objectTransform.translate.y = reinterpret_cast<float&>(y);
	objectTransform.translate.z = reinterpret_cast<float&>(z);

	x = boost::asio::detail::socket_ops::host_to_network_long(reinterpret_cast<unsigned int&>(objectTransform.rotation.x));
	y = boost::asio::detail::socket_ops::host_to_network_long(reinterpret_cast<unsigned int&>(objectTransform.rotation.y));
	z = boost::asio::detail::socket_ops::host_to_network_long(reinterpret_cast<unsigned int&>(objectTransform.rotation.z));
	w = boost::asio::detail::socket_ops::host_to_network_long(reinterpret_cast<unsigned int&>(objectTransform.rotation.w));

	objectTransform.rotation.x = reinterpret_cast<float&>(x);
	objectTransform.rotation.y = reinterpret_cast<float&>(y);
	objectTransform.rotation.z = reinterpret_cast<float&>(z);
	objectTransform.rotation.w = reinterpret_cast<float&>(w);

	x = boost::asio::detail::socket_ops::host_to_network_long(reinterpret_cast<unsigned int&>(objectTransform.scale.x));
	y = boost::asio::detail::socket_ops::host_to_network_long(reinterpret_cast<unsigned int&>(objectTransform.scale.y));
	z = boost::asio::detail::socket_ops::host_to_network_long(reinterpret_cast<unsigned int&>(objectTransform.scale.z));

	objectTransform.scale.x = reinterpret_cast<float&>(x);
	objectTransform.scale.y = reinterpret_cast<float&>(y);
	objectTransform.scale.z = reinterpret_cast<float&>(z);

	x = boost::asio::detail::socket_ops::host_to_network_long(reinterpret_cast<unsigned int&>(objectTransform.bounding_sphere.x));
	y = boost::asio::detail::socket_ops::host_to_network_long(reinterpret_cast<unsigned int&>(objectTransform.bounding_sphere.y));
	z = boost::asio::detail::socket_ops::host_to_network_long(reinterpret_cast<unsigned int&>(objectTransform.bounding_sphere.z));
	w = boost::asio::detail::socket_ops::host_to_network_long(reinterpret_cast<unsigned int&>(objectTransform.bounding_sphere.w));

	objectTransform.bounding_sphere.x = reinterpret_cast<float&>(x);
	objectTransform.bounding_sphere.y = reinterpret_cast<float&>(y);
	objectTransform.bounding_sphere.z = reinterpret_cast<float&>(z);
	objectTransform.bounding_sphere.w = reinterpret_cast<float&>(w);

	memcpy(packetValue + sizeof(objHeader), &objectTransform, sizeof(objectTransform));

	// convert size to network byte endianness, and copy to value buffer
	auto vertSize = boost::asio::detail::socket_ops::host_to_network_long(vertSize_);
	auto normalSize = boost::asio::detail::socket_ops::host_to_network_long(normalSize_);
	auto colorSize = boost::asio::detail::socket_ops::host_to_network_long(colorSize_);
	auto stringSize = boost::asio::detail::socket_ops::host_to_network_long(stringSize_);

	memcpy(packetValue + sizeof(objHeader)+sizeof(objectTransform), &vertSize, sizeof(vertSize));
	memcpy(packetValue + sizeof(objHeader)+sizeof(objectTransform)+sizeof(vertSize), &normalSize, sizeof(normalSize));
	memcpy(packetValue + sizeof(objHeader)+sizeof(objectTransform)+sizeof(vertSize)+sizeof(normalSize), &colorSize, sizeof(colorSize));
	memcpy(packetValue + sizeof(objHeader)+sizeof(objectTransform)+sizeof(vertSize)+sizeof(normalSize)+sizeof(colorSize), &stringSize, sizeof(stringSize));

	// Launch vert/normal/color processing simultaneously
	std::future<void> vertFuture =
		std::async(std::launch::async,
		[&]()
	{
		unsigned char *vertices = new unsigned char[vertSize_];
		unsigned int *vp = (unsigned int*)vertices;

		// copy and convert vertices floats to network endianness
		float * realVal = (float*)vertices_;
		for (unsigned int i = 0; i < objectHeader_.num_vertices * objectHeader_.num_points_per_vertex; i++)
		{
			*vp = boost::asio::detail::socket_ops::host_to_network_long(*reinterpret_cast<unsigned int*>(realVal));
			vp++;
			realVal++;
		}

		memcpy(packetValue + sizeof(objHeader)+sizeof(objectTransform)+sizeof(vertSize)+sizeof(normalSize)+sizeof(colorSize)+sizeof(stringSize), vertices, vertSize_);

		delete[] vertices;
	});

	std::future<void> normFuture =
		std::async(std::launch::async,
		[&]()
	{
		if (normalSize_ > 0)
		{

			unsigned char * normals = new unsigned char[normalSize_];
			unsigned int * np = (unsigned int*)normals;

			// copy and convert normal floats to network endianness
			float * realVal = (float*)normals_;
			for (unsigned int i = 0; i < objectHeader_.num_vertices * objectHeader_.num_points_per_vertex; i++)
			{
				*np = boost::asio::detail::socket_ops::host_to_network_long(*reinterpret_cast<unsigned int*>(realVal));
				np++;
				realVal++;
			}

			memcpy(packetValue + sizeof(objHeader)+sizeof(objectTransform)+sizeof(vertSize)+sizeof(normalSize)+sizeof(colorSize)+sizeof(stringSize)+vertSize_, normals, normalSize_);

			delete[] normals;
		}
	});

	std::future<void> colorFuture =
		std::async(std::launch::async,
		[&]()
	{
		if (colorSize_ > 0)
		{
			unsigned char * colors = new unsigned char[colorSize_];
			unsigned int *cp = (unsigned int*)colors;

			// copy and convert color floats from host to network endianness
			float *realVal = (float*)colors_;
			for (unsigned int i = 0; i < objectHeader_.num_vertices * objectHeader_.num_color_channels; i++)
			{
				*cp = boost::asio::detail::socket_ops::host_to_network_long(*reinterpret_cast<unsigned int*>(realVal));
				cp++;
				realVal++;
			}

			memcpy(packetValue + sizeof(objHeader)+sizeof(objectTransform)+sizeof(vertSize)+sizeof(normalSize)+sizeof(colorSize)+sizeof(stringSize)+vertSize_ + normalSize_, colors, colorSize_);

			delete[] colors;
		}
	});

	memcpy(packetValue + sizeof(objHeader)+sizeof(objectTransform)+sizeof(vertSize)+sizeof(normalSize)+sizeof(colorSize)+sizeof(stringSize)+vertSize_ + normalSize_ + colorSize_, objectName_.data(), objectName_.size());

	// Wait for them to be finished
	vertFuture.wait();
	normFuture.wait();
	colorFuture.wait();

	return netPacket;

};

const std::tuple<std::string, HoloTransform> HoloRender3DObject::GetTransformFromPacket(const boost::shared_ptr<holo::net::HoloNetPacket>& transformNetPacket)
{
	HoloTransform objectTransform;
	std::string objectName;

	auto packetValue = transformNetPacket->GetPacketValue();

	memcpy(&objectTransform, packetValue, sizeof(objectTransform));

	unsigned int x, y, z, w = 0;

	x = boost::asio::detail::socket_ops::network_to_host_long(reinterpret_cast<unsigned int&>(objectTransform.translate.x));
	y = boost::asio::detail::socket_ops::network_to_host_long(reinterpret_cast<unsigned int&>(objectTransform.translate.y));
	z = boost::asio::detail::socket_ops::network_to_host_long(reinterpret_cast<unsigned int&>(objectTransform.translate.z));

	objectTransform.translate.x = reinterpret_cast<float&>(x);
	objectTransform.translate.y = reinterpret_cast<float&>(y);
	objectTransform.translate.z = reinterpret_cast<float&>(z);

	x = boost::asio::detail::socket_ops::network_to_host_long(reinterpret_cast<unsigned int&>(objectTransform.rotation.x));
	y = boost::asio::detail::socket_ops::network_to_host_long(reinterpret_cast<unsigned int&>(objectTransform.rotation.y));
	z = boost::asio::detail::socket_ops::network_to_host_long(reinterpret_cast<unsigned int&>(objectTransform.rotation.z));
	w = boost::asio::detail::socket_ops::network_to_host_long(reinterpret_cast<unsigned int&>(objectTransform.rotation.w));

	objectTransform.rotation.x = reinterpret_cast<float&>(x);
	objectTransform.rotation.y = reinterpret_cast<float&>(y);
	objectTransform.rotation.z = reinterpret_cast<float&>(z);
	objectTransform.rotation.w = reinterpret_cast<float&>(w);

	x = boost::asio::detail::socket_ops::network_to_host_long(reinterpret_cast<unsigned int&>(objectTransform.scale.x));
	y = boost::asio::detail::socket_ops::network_to_host_long(reinterpret_cast<unsigned int&>(objectTransform.scale.y));
	z = boost::asio::detail::socket_ops::network_to_host_long(reinterpret_cast<unsigned int&>(objectTransform.scale.z));

	objectTransform.scale.x = reinterpret_cast<float&>(x);
	objectTransform.scale.y = reinterpret_cast<float&>(y);
	objectTransform.scale.z = reinterpret_cast<float&>(z);

	x = boost::asio::detail::socket_ops::network_to_host_long(reinterpret_cast<unsigned int&>(objectTransform.bounding_sphere.x));
	y = boost::asio::detail::socket_ops::network_to_host_long(reinterpret_cast<unsigned int&>(objectTransform.bounding_sphere.y));
	z = boost::asio::detail::socket_ops::network_to_host_long(reinterpret_cast<unsigned int&>(objectTransform.bounding_sphere.z));
	w = boost::asio::detail::socket_ops::network_to_host_long(reinterpret_cast<unsigned int&>(objectTransform.bounding_sphere.w));

	objectTransform.bounding_sphere.x = reinterpret_cast<float&>(x);
	objectTransform.bounding_sphere.y = reinterpret_cast<float&>(y);
	objectTransform.bounding_sphere.z = reinterpret_cast<float&>(z);
	objectTransform.bounding_sphere.w = reinterpret_cast<float&>(w);

	unsigned int stringSize = 0;
	memcpy(&stringSize, packetValue + sizeof(objectTransform), sizeof(stringSize));
	stringSize = boost::asio::detail::socket_ops::network_to_host_long(stringSize);

	objectName.resize(stringSize);
	memcpy((void*)objectName.data(), packetValue + sizeof(objectTransform)+sizeof(stringSize), stringSize);

	return std::make_tuple(objectName, objectTransform);
}

const boost::shared_ptr<holo::net::HoloNetPacket> HoloRender3DObject::CreateNetPacketFromTransform(const std::tuple<std::string, HoloTransform>& objInfo)
{

	std::string objectName;
	HoloTransform objectTransform;

	std::tie(objectName, objectTransform) = objInfo;

	unsigned int x, y, z, w = 0;

	x = boost::asio::detail::socket_ops::host_to_network_long(reinterpret_cast<unsigned int&>(objectTransform.translate.x));
	y = boost::asio::detail::socket_ops::host_to_network_long(reinterpret_cast<unsigned int&>(objectTransform.translate.y));
	z = boost::asio::detail::socket_ops::host_to_network_long(reinterpret_cast<unsigned int&>(objectTransform.translate.z));

	objectTransform.translate.x = reinterpret_cast<float&>(x);
	objectTransform.translate.y = reinterpret_cast<float&>(y);
	objectTransform.translate.z = reinterpret_cast<float&>(z);

	x = boost::asio::detail::socket_ops::host_to_network_long(reinterpret_cast<unsigned int&>(objectTransform.rotation.x));
	y = boost::asio::detail::socket_ops::host_to_network_long(reinterpret_cast<unsigned int&>(objectTransform.rotation.y));
	z = boost::asio::detail::socket_ops::host_to_network_long(reinterpret_cast<unsigned int&>(objectTransform.rotation.z));
	w = boost::asio::detail::socket_ops::host_to_network_long(reinterpret_cast<unsigned int&>(objectTransform.rotation.w));

	objectTransform.rotation.x = reinterpret_cast<float&>(x);
	objectTransform.rotation.y = reinterpret_cast<float&>(y);
	objectTransform.rotation.z = reinterpret_cast<float&>(z);
	objectTransform.rotation.w = reinterpret_cast<float&>(w);

	x = boost::asio::detail::socket_ops::host_to_network_long(reinterpret_cast<unsigned int&>(objectTransform.scale.x));
	y = boost::asio::detail::socket_ops::host_to_network_long(reinterpret_cast<unsigned int&>(objectTransform.scale.y));
	z = boost::asio::detail::socket_ops::host_to_network_long(reinterpret_cast<unsigned int&>(objectTransform.scale.z));

	objectTransform.scale.x = reinterpret_cast<float&>(x);
	objectTransform.scale.y = reinterpret_cast<float&>(y);
	objectTransform.scale.z = reinterpret_cast<float&>(z);

	x = boost::asio::detail::socket_ops::host_to_network_long(reinterpret_cast<unsigned int&>(objectTransform.bounding_sphere.x));
	y = boost::asio::detail::socket_ops::host_to_network_long(reinterpret_cast<unsigned int&>(objectTransform.bounding_sphere.y));
	z = boost::asio::detail::socket_ops::host_to_network_long(reinterpret_cast<unsigned int&>(objectTransform.bounding_sphere.z));
	w = boost::asio::detail::socket_ops::host_to_network_long(reinterpret_cast<unsigned int&>(objectTransform.bounding_sphere.w));

	objectTransform.bounding_sphere.x = reinterpret_cast<float&>(x);
	objectTransform.bounding_sphere.y = reinterpret_cast<float&>(y);
	objectTransform.bounding_sphere.z = reinterpret_cast<float&>(z);
	objectTransform.bounding_sphere.w = reinterpret_cast<float&>(w);

	unsigned int stringSize = boost::asio::detail::socket_ops::network_to_host_long(objectName.size());

	auto netPacket = boost::shared_ptr<holo::net::HoloNetPacket>(new holo::net::HoloNetPacket(HOLO_NET_PACKET_TYPE_OBJECT_UPDATE, sizeof(objectTransform)+sizeof(stringSize)+objectName.size()));
	//netPacket->type = HOLO_NET_PACKET_TYPE_OBJECT_UPDATE;
	//netPacket->length = sizeof(objectTransform)+sizeof(stringSize)+objectName.size();
	//netPacket->value.resize(netPacket->length);
	auto packetValue = netPacket->GetPacketValue();
	memcpy(packetValue, &objectTransform, sizeof(objectTransform));
	memcpy(packetValue + sizeof(objectTransform), &stringSize, sizeof(stringSize));
	memcpy(packetValue + sizeof(objectTransform)+sizeof(stringSize), objectName.data(), objectName.size());

	return netPacket;
}

const boost::shared_ptr<holo::net::HoloNetPacket> HoloRender3DObject::ToggleOwnerAndGetOwnerChangePacket()
{
	auto ownerChangePacket = boost::shared_ptr<holo::net::HoloNetPacket>(new HoloNetPacket(holo::net::HOLO_NET_PACKET_TYPE_OBJECT_CHANGE_OWNER, 2 * sizeof(unsigned int)+stringSize_));
	auto packetValue = ownerChangePacket->GetPacketValue();
	//ownerChangePacket->type = holo::net::HOLO_NET_PACKET_TYPE_OBJECT_CHANGE_OWNER;
	//ownerChangePacket->length = 2*sizeof(unsigned int)+stringSize_;
	//ownerChangePacket->value.resize(ownerChangePacket->length);

	unsigned int amOwnerInt = static_cast<unsigned int>(amOwner_.load());
	amOwnerInt = boost::asio::detail::socket_ops::host_to_network_long(amOwnerInt);
	unsigned int stringSize = boost::asio::detail::socket_ops::host_to_network_long(stringSize_);

	memcpy(packetValue, &amOwnerInt, sizeof(amOwnerInt));
	memcpy(packetValue + sizeof(amOwnerInt), &stringSize, sizeof(stringSize));
	memcpy(packetValue + sizeof(amOwnerInt)+sizeof(stringSize), objectName_.data(), stringSize_);

	amOwner_.store(!amOwner_.load());

	return ownerChangePacket;
}

const std::tuple<std::string, bool> HoloRender3DObject::GetChangeOwnerInfoFromPacket(const boost::shared_ptr<holo::net::HoloNetPacket>& changeOwnerPacket)
{
	unsigned int amOwnerInt = 2;
	unsigned int stringSize = 0;
	std::string objectName;

	auto packetValue = changeOwnerPacket->GetPacketValue();

	memcpy(&amOwnerInt, packetValue, sizeof(amOwnerInt));
	memcpy(&stringSize, packetValue + sizeof(amOwnerInt), sizeof(stringSize));

	stringSize = boost::asio::detail::socket_ops::network_to_host_long(stringSize);

	objectName.resize(stringSize);

	memcpy((void*)objectName.data(), packetValue + sizeof(amOwnerInt)+sizeof(stringSize), stringSize);

	return std::make_tuple(objectName, amOwnerInt);
}

HoloRender3DObject::~HoloRender3DObject()
{
	delete[] colors_;
	delete[] vertices_;
	delete[] normals_;
}


// Boost.Geometry (aka GGL, Generic Geometry Library)

// Copyright (c) 2007-2012 Barend Gehrels, Amsterdam, the Netherlands.
// Copyright (c) 2008-2012 Bruno Lalande, Paris, France.
// Copyright (c) 2009-2012 Mateusz Loskot, London, UK.
// Copyright (c) 2014 Adam Wulkiewicz, Lodz, Poland.

// Parts of Boost.Geometry are redesigned from Geodan's Geographic Library
// (geolib/GGL), copyright (c) 1995-2010 Geodan, Amsterdam, the Netherlands.

// Use, modification and distribution is 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)

#ifndef BOOST_GEOMETRY_ALGORITHMS_TRANSFORM_HPP
#define BOOST_GEOMETRY_ALGORITHMS_TRANSFORM_HPP

#include <cmath>
#include <iterator>

#include <boost/range.hpp>
#include <boost/type_traits/remove_reference.hpp>

#include <boost/variant/apply_visitor.hpp>
#include <boost/variant/static_visitor.hpp>
#include <boost/variant/variant_fwd.hpp>

#include <boost/geometry/algorithms/assign.hpp>
#include <boost/geometry/algorithms/clear.hpp>
#include <boost/geometry/algorithms/detail/interior_iterator.hpp>
#include <boost/geometry/algorithms/num_interior_rings.hpp>

#include <boost/geometry/core/cs.hpp>
#include <boost/geometry/core/exterior_ring.hpp>
#include <boost/geometry/core/interior_rings.hpp>
#include <boost/geometry/core/mutable_range.hpp>
#include <boost/geometry/core/ring_type.hpp>
#include <boost/geometry/core/tag_cast.hpp>
#include <boost/geometry/core/tags.hpp>
#include <boost/geometry/geometries/concepts/check.hpp>
#include <boost/geometry/strategies/default_strategy.hpp>
#include <boost/geometry/strategies/transform.hpp>

namespace boost { namespace geometry
{

#ifndef DOXYGEN_NO_DETAIL
namespace detail { namespace transform
{

struct transform_point
{
    template <typename Point1, typename Point2, typename Strategy>
    static inline bool apply(Point1 const& p1, Point2& p2,
                Strategy const& strategy)
    {
        return strategy.apply(p1, p2);
    }
};

struct transform_box
{
    template <typename Box1, typename Box2, typename Strategy>
    static inline bool apply(Box1 const& b1, Box2& b2,
                Strategy const& strategy)
    {
        typedef typename point_type<Box1>::type point_type1;
        typedef typename point_type<Box2>::type point_type2;

        point_type1 lower_left, upper_right;
        geometry::detail::assign::assign_box_2d_corner<min_corner, min_corner>(
                    b1, lower_left);
        geometry::detail::assign::assign_box_2d_corner<max_corner, max_corner>(
                    b1, upper_right);

        point_type2 p1, p2;
        if (strategy.apply(lower_left, p1) && strategy.apply(upper_right, p2))
        {
            // Create a valid box and therefore swap if necessary
            typedef typename coordinate_type<point_type2>::type coordinate_type;
            coordinate_type x1 = geometry::get<0>(p1)
                    , y1  = geometry::get<1>(p1)
                    , x2  = geometry::get<0>(p2)
                    , y2  = geometry::get<1>(p2);

            if (x1 > x2) { std::swap(x1, x2); }
            if (y1 > y2) { std::swap(y1, y2); }

            geometry::set<min_corner, 0>(b2, x1);
            geometry::set<min_corner, 1>(b2, y1);
            geometry::set<max_corner, 0>(b2, x2);
            geometry::set<max_corner, 1>(b2, y2);

            return true;
        }
        return false;
    }
};

struct transform_box_or_segment
{
    template <typename Geometry1, typename Geometry2, typename Strategy>
    static inline bool apply(Geometry1 const& source, Geometry2& target,
                Strategy const& strategy)
    {
        typedef typename point_type<Geometry1>::type point_type1;
        typedef typename point_type<Geometry2>::type point_type2;

        point_type1 source_point[2];
        geometry::detail::assign_point_from_index<0>(source, source_point[0]);
        geometry::detail::assign_point_from_index<1>(source, source_point[1]);

        point_type2 target_point[2];
        if (strategy.apply(source_point[0], target_point[0])
            && strategy.apply(source_point[1], target_point[1]))
        {
            geometry::detail::assign_point_to_index<0>(target_point[0], target);
            geometry::detail::assign_point_to_index<1>(target_point[1], target);
            return true;
        }
        return false;
    }
};

template
<
    typename PointOut,
    typename OutputIterator,
    typename Range,
    typename Strategy
>
inline bool transform_range_out(Range const& range,
    OutputIterator out, Strategy const& strategy)
{
    PointOut point_out;
    for(typename boost::range_iterator<Range const>::type
        it = boost::begin(range);
        it != boost::end(range);
        ++it)
    {
        if (! transform_point::apply(*it, point_out, strategy))
        {
            return false;
        }
        *out++ = point_out;
    }
    return true;
}

struct transform_polygon
{
    template <typename Polygon1, typename Polygon2, typename Strategy>
    static inline bool apply(Polygon1 const& poly1, Polygon2& poly2,
                Strategy const& strategy)
    {
        typedef typename point_type<Polygon2>::type point2_type;

        geometry::clear(poly2);

        if (!transform_range_out<point2_type>(geometry::exterior_ring(poly1),
                    range::back_inserter(geometry::exterior_ring(poly2)), strategy))
        {
            return false;
        }

        // Note: here a resizeable container is assumed.
        traits::resize
            <
                typename boost::remove_reference
                <
                    typename traits::interior_mutable_type<Polygon2>::type
                >::type
            >::apply(geometry::interior_rings(poly2),
                     geometry::num_interior_rings(poly1));

        typename geometry::interior_return_type<Polygon1 const>::type
            rings1 = geometry::interior_rings(poly1);
        typename geometry::interior_return_type<Polygon2>::type
            rings2 = geometry::interior_rings(poly2);

        typename detail::interior_iterator<Polygon1 const>::type
            it1 = boost::begin(rings1);
        typename detail::interior_iterator<Polygon2>::type
            it2 = boost::begin(rings2);
        for ( ; it1 != boost::end(rings1); ++it1, ++it2)
        {
            if ( ! transform_range_out<point2_type>(*it1,
                                                    range::back_inserter(*it2),
                                                    strategy) )
            {
                return false;
            }
        }

        return true;
    }
};

template <typename Point1, typename Point2>
struct select_strategy
{
    typedef typename strategy::transform::services::default_strategy
        <
            typename cs_tag<Point1>::type,
            typename cs_tag<Point2>::type,
            typename coordinate_system<Point1>::type,
            typename coordinate_system<Point2>::type,
            dimension<Point1>::type::value,
            dimension<Point2>::type::value,
            typename point_type<Point1>::type,
            typename point_type<Point2>::type
        >::type type;
};

struct transform_range
{
    template <typename Range1, typename Range2, typename Strategy>
    static inline bool apply(Range1 const& range1,
            Range2& range2, Strategy const& strategy)
    {
        typedef typename point_type<Range2>::type point_type;

        // Should NOT be done here!
        // geometry::clear(range2);
        return transform_range_out<point_type>(range1,
                range::back_inserter(range2), strategy);
    }
};

/*!
    \brief Is able to transform any multi-geometry, calling the single-version as policy
*/
template <typename Policy>
struct transform_multi
{
    template <typename Multi1, typename Multi2, typename S>
    static inline bool apply(Multi1 const& multi1, Multi2& multi2, S const& strategy)
    {
        traits::resize<Multi2>::apply(multi2, boost::size(multi1));

        typename boost::range_iterator<Multi1 const>::type it1
                = boost::begin(multi1);
        typename boost::range_iterator<Multi2>::type it2
                = boost::begin(multi2);

        for (; it1 != boost::end(multi1); ++it1, ++it2)
        {
            if (! Policy::apply(*it1, *it2, strategy))
            {
                return false;
            }
        }

        return true;
    }
};

}} // namespace detail::transform
#endif // DOXYGEN_NO_DETAIL

#ifndef DOXYGEN_NO_DISPATCH
namespace dispatch
{

template
<
    typename Geometry1, typename Geometry2,
    typename Tag1 = typename tag_cast<typename tag<Geometry1>::type, multi_tag>::type,
    typename Tag2 = typename tag_cast<typename tag<Geometry2>::type, multi_tag>::type
>
struct transform {};

template <typename Point1, typename Point2>
struct transform<Point1, Point2, point_tag, point_tag>
    : detail::transform::transform_point
{
};

template <typename Linestring1, typename Linestring2>
struct transform
    <
        Linestring1, Linestring2,
        linestring_tag, linestring_tag
    >
    : detail::transform::transform_range
{
};

template <typename Range1, typename Range2>
struct transform<Range1, Range2, ring_tag, ring_tag>
    : detail::transform::transform_range
{
};

template <typename Polygon1, typename Polygon2>
struct transform<Polygon1, Polygon2, polygon_tag, polygon_tag>
    : detail::transform::transform_polygon
{
};

template <typename Box1, typename Box2>
struct transform<Box1, Box2, box_tag, box_tag>
    : detail::transform::transform_box
{
};

template <typename Segment1, typename Segment2>
struct transform<Segment1, Segment2, segment_tag, segment_tag>
    : detail::transform::transform_box_or_segment
{
};

template <typename Multi1, typename Multi2>
struct transform
    <
        Multi1, Multi2,
        multi_tag, multi_tag
    >
    : detail::transform::transform_multi
        <
            dispatch::transform
                <
                    typename boost::range_value<Multi1>::type,
                    typename boost::range_value<Multi2>::type
                >
        >
{};

} // namespace dispatch
#endif // DOXYGEN_NO_DISPATCH

namespace resolve_strategy {

struct transform
{
    template <typename Geometry1, typename Geometry2, typename Strategy>
    static inline bool apply(Geometry1 const& geometry1,
                             Geometry2& geometry2,
                             Strategy const& strategy)
    {
        concept::check<Geometry1 const>();
        concept::check<Geometry2>();

        return dispatch::transform<Geometry1, Geometry2>::apply(
            geometry1,
            geometry2,
            strategy
        );
    }

    template <typename Geometry1, typename Geometry2>
    static inline bool apply(Geometry1 const& geometry1,
                             Geometry2& geometry2,
                             default_strategy)
    {
        return apply(
            geometry1,
            geometry2,
            typename detail::transform::select_strategy<Geometry1, Geometry2>::type()
        );
    }
};

} // namespace resolve_strategy

namespace resolve_variant {

template <typename Geometry1, typename Geometry2>
struct transform
{
    template <typename Strategy>
    static inline bool apply(Geometry1 const& geometry1,
                             Geometry2& geometry2,
                             Strategy const& strategy)
    {
        return resolve_strategy::transform::apply(
            geometry1,
            geometry2,
            strategy
        );
    }
};

template <BOOST_VARIANT_ENUM_PARAMS(typename T), typename Geometry2>
struct transform<boost::variant<BOOST_VARIANT_ENUM_PARAMS(T)>, Geometry2>
{
    template <typename Strategy>
    struct visitor: static_visitor<bool>
    {
        Geometry2& m_geometry2;
        Strategy const& m_strategy;

        visitor(Geometry2& geometry2, Strategy const& strategy)
            : m_geometry2(geometry2)
            , m_strategy(strategy)
        {}

        template <typename Geometry1>
        inline bool operator()(Geometry1 const& geometry1) const
        {
            return transform<Geometry1, Geometry2>::apply(
                geometry1,
                m_geometry2,
                m_strategy
            );
        }
    };

    template <typename Strategy>
    static inline bool apply(
        boost::variant<BOOST_VARIANT_ENUM_PARAMS(T)> const& geometry1,
        Geometry2& geometry2,
        Strategy const& strategy
    )
    {
        return boost::apply_visitor(visitor<Strategy>(geometry2, strategy), geometry1);
    }
};

} // namespace resolve_variant

/*!
\brief Transforms from one geometry to another geometry  \brief_strategy
\ingroup transform
\tparam Geometry1 \tparam_geometry
\tparam Geometry2 \tparam_geometry
\tparam Strategy strategy
\param geometry1 \param_geometry
\param geometry2 \param_geometry
\param strategy The strategy to be used for transformation
\return True if the transformation could be done

\qbk{distinguish,with strategy}

\qbk{[include reference/algorithms/transform_with_strategy.qbk]}
 */
template <typename Geometry1, typename Geometry2, typename Strategy>
inline bool transform(Geometry1 const& geometry1, Geometry2& geometry2,
            Strategy const& strategy)
{
    return resolve_variant::transform<Geometry1, Geometry2>
                          ::apply(geometry1, geometry2, strategy);
}

/*!
\brief Transforms from one geometry to another geometry using a strategy
\ingroup transform
\tparam Geometry1 \tparam_geometry
\tparam Geometry2 \tparam_geometry
\param geometry1 \param_geometry
\param geometry2 \param_geometry
\return True if the transformation could be done

\qbk{[include reference/algorithms/transform.qbk]}
 */
template <typename Geometry1, typename Geometry2>
inline bool transform(Geometry1 const& geometry1, Geometry2& geometry2)
{
    return geometry::transform(geometry1, geometry2, default_strategy());
}

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_ALGORITHMS_TRANSFORM_HPP


//
// Copyright (c) 2009 Rutger ter Borg
//
// 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)
//

#ifndef BOOST_NUMERIC_BINDINGS_BLAS_LEVEL2_HPP
#define BOOST_NUMERIC_BINDINGS_BLAS_LEVEL2_HPP

#include <boost/numeric/bindings/blas/level2/gbmv.hpp>
#include <boost/numeric/bindings/blas/level2/gemv.hpp>
#include <boost/numeric/bindings/blas/level2/gerc.hpp>
#include <boost/numeric/bindings/blas/level2/ger.hpp>
#include <boost/numeric/bindings/blas/level2/geru.hpp>
#include <boost/numeric/bindings/blas/level2/hbmv.hpp>
#include <boost/numeric/bindings/blas/level2/hemv.hpp>
#include <boost/numeric/bindings/blas/level2/her2.hpp>
#include <boost/numeric/bindings/blas/level2/her.hpp>
#include <boost/numeric/bindings/blas/level2/hpmv.hpp>
#include <boost/numeric/bindings/blas/level2/hpr2.hpp>
#include <boost/numeric/bindings/blas/level2/hpr.hpp>
#include <boost/numeric/bindings/blas/level2/sbmv.hpp>
#include <boost/numeric/bindings/blas/level2/spmv.hpp>
#include <boost/numeric/bindings/blas/level2/spr2.hpp>
#include <boost/numeric/bindings/blas/level2/spr.hpp>
#include <boost/numeric/bindings/blas/level2/symv.hpp>
#include <boost/numeric/bindings/blas/level2/syr2.hpp>
#include <boost/numeric/bindings/blas/level2/syr.hpp>
#include <boost/numeric/bindings/blas/level2/tbmv.hpp>
#include <boost/numeric/bindings/blas/level2/tbsv.hpp>
#include <boost/numeric/bindings/blas/level2/tpmv.hpp>
#include <boost/numeric/bindings/blas/level2/tpsv.hpp>
#include <boost/numeric/bindings/blas/level2/trmv.hpp>
#include <boost/numeric/bindings/blas/level2/trsv.hpp>

#endif


/**
 ** Isaac Genome Alignment Software
 ** Copyright (c) 2010-2017 Illumina, Inc.
 ** All rights reserved.
 **
 ** This software is provided under the terms and conditions of the
 ** GNU GENERAL PUBLIC LICENSE Version 3
 **
 ** You should have received a copy of the GNU GENERAL PUBLIC LICENSE Version 3
 ** along with this program. If not, see
 ** <https://github.com/illumina/licenses/>.
 **
 ** \file Quality.hh
 **
 ** \brief Various functions and tables to support alignment quality.
 **
 ** \author Come Raczy
 **/

#ifndef iSAAC_ALIGNMENT_QUALITY_HH
#define iSAAC_ALIGNMENT_QUALITY_HH

#include <limits>
#include <vector>

#include <boost/format.hpp>

#include "alignment/Cluster.hh"
#include "common/MathCompatibility.hh"

namespace isaac
{
namespace alignment
{

static const unsigned REPEAT_ALIGNMENT_SCORE = 3;
static const unsigned UNKNOWN_ALIGNMENT_SCORE = -1U;
static const unsigned char UNKNOWN_MAPQ = 255;
static const unsigned char MAX_MAPQ = 60;

inline bool isUnique(const unsigned alignmentScore) {return REPEAT_ALIGNMENT_SCORE < alignmentScore;}

inline unsigned char alignmentScoreToMapq(const unsigned alignmentScore)
{
    ISAAC_ASSERT_MSG(UNKNOWN_ALIGNMENT_SCORE != alignmentScore, "Invalid alignmentScore");
    return std::min<unsigned>(MAX_MAPQ, alignmentScore);
}

inline unsigned char pickMapQ(
    const unsigned alignmentScore,
    const unsigned mateAlignmentScore,
    const bool properPair,
    const unsigned templateAlignmentScore)
{
    ISAAC_ASSERT_MSG(UNKNOWN_ALIGNMENT_SCORE != alignmentScore, "Invalid alignmentScore");
    ISAAC_ASSERT_MSG(UNKNOWN_ALIGNMENT_SCORE != mateAlignmentScore, "Invalid mateAlignmentScore");
    ISAAC_ASSERT_MSG(UNKNOWN_ALIGNMENT_SCORE != templateAlignmentScore, "Invalid templateAlignmentScore");

    return alignmentScoreToMapq(
        (properPair) ?
            // Rescue non-unique alignments only if both mate and pair are unique. This prevents from using unique pair score
            // when both fragments are non-unique as it usually results in accepting high score for unique pairing
            // without having seen all of the pairings.
            std::max(alignmentScore, std::min(templateAlignmentScore, mateAlignmentScore)) : alignmentScore);
}

inline unsigned char pickMapQFromMate(
    const unsigned char mateMapQ,
    const unsigned templateAlignmentScore)
{
    ISAAC_ASSERT_MSG(UNKNOWN_MAPQ != mateMapQ, "Invalid mateMapQ");
    ISAAC_ASSERT_MSG(UNKNOWN_ALIGNMENT_SCORE != templateAlignmentScore, "Invalid templateAlignmentScore");

    return std::min(alignmentScoreToMapq(templateAlignmentScore), mateMapQ);
}

inline unsigned computeAlignmentScore(
    const double restOfGenomeCorrection,
    const double alignmentProbability,
    const double otherAlignmentsProbability)
{
    return floor(-10.0 * log10((otherAlignmentsProbability + restOfGenomeCorrection) / (otherAlignmentsProbability + alignmentProbability + restOfGenomeCorrection)));
}

/**
 ** \brief Utility class providing various services related to base and sequence quality
 **/
class Quality
{
public:
    /**
     ** \brief Return the natural log of the probability of an incorrect base for a
     ** given quality.
     **
     ** This is simply 'log(perror)' where 'perror=10^(-quality/10)'
     **
     ** \param quality PHRED quality score
     **/
    static double getLogError(const unsigned int quality)
    {
        ISAAC_ASSERT_MSG(quality < logErrorLookup.size(),
                         (boost::format("Incorrect quality %u ") % quality).str().c_str());
        return logErrorLookup[quality];
    }

    /**
     ** \brief Return the natural log of the probability of a correct base for a
     ** given quality.
     **
     ** This is simply 'log(1-perror)' where 'perror=10^(-quality/10)'
     **
     ** \param quality PHRED quality score
     **/
    static double getLogCorrect(const unsigned int quality)
    {
        ISAAC_ASSERT_MSG(quality < logMatchLookup.size(),
                         (boost::format("Incorrect quality %u ") % quality).str().c_str());
        return logMatchLookup[quality];
    }

    /**
     ** \brief Return the natural log of the probability of a base that matches reference to be correct.
     **
     ** This is simply 'log(1-perror)' where 'perror=10^(-quality/10)'
     **
     ** \param quality PHRED quality score
     **/
    static double getLogMatch(const unsigned int quality)
    {
        return getLogCorrect(quality);
    }

    /**
     * \brief Same as getLogMismatchSlow but uses a pre-built lookup table
     */
    static double getLogMismatch(const unsigned int quality)
    {
        ISAAC_ASSERT_MSG(quality < logMatchLookup.size(),
                         (boost::format("Incorrect quality %u ") % quality).str().c_str());
        return logMismatchLookup[quality];
    }

    /**
     ** \brief Return the natural log of the probability of a base that mismatches the reference to be wrong.
     **
     ** This is 'log(perror/3/pcorrect)' where 'perror=10^(-quality/10)' and
     ** 'pcorrect=1-perror'. The rationale is that if there is an error, each of
     ** the three other bases has 1/3 of the chances of being the correct one.
     **
     ** \param quality PHRED quality score
     **/
    static double getLogMismatchSlow(const unsigned quality)
    {
        const double mismatch = pow(10.0, (double)quality / -10.0);
        return log(mismatch / 3.0);
    }

    /**
     ** \brief Return the 'rest of the genome' correction for uniquely aligned reads
     **
     **/
    static double restOfGenomeCorrection(const unsigned genomeLength, const unsigned readLength)
    {
        using std::log;
        return exp(log(2.0) + log((double)genomeLength) - (log(4.0) * (double)readLength));
    }

private:
    /// lookup for log of probability of a sequencing error for a given quality
    static const std::vector<double> logErrorLookup;
    /// lookup for log of probability of a match for a given quality
    static const std::vector<double> logMatchLookup;
    /// lookup for log of probability of a mismatch for a given quality
    static const std::vector<double> logMismatchLookup;
};

void trimLowQualityEnds(Cluster &cluster, const unsigned baseQualityCutoff);

template <typename FpT> bool ISAAC_LP_EQUALS(FpT left, FpT right)
{
    return 0.0000001 >= std::fabs(left - right);
}

template <typename FpT> bool ISAAC_LP_LESS(FpT left, FpT right)
{
    return !ISAAC_LP_EQUALS(left, right) && left < right;
}

} // namespace alignemnt
} // namespace isaac

#endif // #ifndef iSAAC_ALIGNMENT_QUALITY_HH


#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/calib3d/calib3d.hpp>

#include <algorithm>
#include <boost/timer.hpp>

#include"simplevo/config.h"
#include"simplevo/visual_odometry.h"

namespace simplevo{
VisualOdometry::VisualOdometry() :
    state_ ( INITIALIZING ), ref_ ( nullptr ), curr_ ( nullptr ), map_ ( new Map ), num_lost_ ( 0 ), num_inliers_ ( 0 )
{
    num_of_features_    = Config::get<int> ( "number_of_features" );
    scale_factor_       = Config::get<double> ( "scale_factor" );
    level_pyramid_      = Config::get<int> ( "level_pyramid" );
    match_ratio_        = Config::get<float> ( "match_ratio" );
    max_num_lost_       = Config::get<float> ( "max_num_lost" );
    min_inliers_        = Config::get<int> ( "min_inliers" );
    key_frame_min_rot   = Config::get<double> ( "keyframe_rotation" );
    key_frame_min_trans = Config::get<double> ( "keyframe_translation" );
    orb_ = cv::ORB::create ( num_of_features_, scale_factor_, level_pyramid_ );
}
VisualOdometry::~VisualOdometry()
{
}
// VO 对frame进行运动估计
bool VisualOdometry::addFrame ( Frame::Ptr frame )
{
    switch ( state_ )
    {
    case INITIALIZING:  // 初始化状态
    {
        state_ = OK;
        curr_ = ref_ = frame;
        map_->insertKeyFrame ( frame );
        // extract features from first frame
        extractKeyPoints();
        computeDescriptors();
        // compute the 3d position of features in ref frame
        setRef3DPoints();
        break;
    }
    case OK:
    {
        curr_ = frame; // 更新当前帧
        extractKeyPoints();
        computeDescriptors();
        featureMatching();
        poseEstimationPnP(); // pnp : 3d-2d
        if ( checkEstimatedPose() == true ) // a good estimation
        {
            cout<<ref_->T_c_w_.matrix()<<endl;
            Eigen::MatrixXd culPoseMatrix(4,4);
            culPoseMatrix = T_c_r_estimated_.matrix() * ref_->T_c_w_.matrix();
            Eigen::MatrixXd rm(3,3);
            rm<<culPoseMatrix(0,0),culPoseMatrix(0,1),culPoseMatrix(0,2),
            culPoseMatrix(1,0),culPoseMatrix(1,1),culPoseMatrix(1,2),
            culPoseMatrix(2,0), culPoseMatrix(2,1), culPoseMatrix(2,2);
            Eigen::Vector3d tv(culPoseMatrix(0,3),culPoseMatrix(1,3),culPoseMatrix(2,3));
            curr_->T_c_w_ = Sophus::SE3d(rm,tv);
            cout<<"OKsss"<<endl;
            ref_ = curr_;
            setRef3DPoints();
            num_lost_ = 0;
            if ( checkKeyFrame() == true ) // is a key-frame
            {
                addKeyFrame();
            }
        }
        else // bad estimation due to various reasons
        {
            num_lost_++;
            if ( num_lost_ > max_num_lost_ )
            {
                state_ = LOST;
            }
            return false;
        }
        break;
    }
    case LOST:
    {
        cout<<"VO has lost."<<endl;
        break;
    }
    }
    return true;
}

void VisualOdometry::extractKeyPoints()
{
    orb_->detect ( curr_->color_, keypoints_curr_ ); // keypoints_curr: h
}
void VisualOdometry::computeDescriptors()
{
    orb_->compute ( curr_->color_, keypoints_curr_, descriptors_curr_ );
}
void VisualOdometry::featureMatching()
{
    // match desp_ref and desp_curr, use OpenCV's brute force match
    vector<cv::DMatch> matches;
    cv::BFMatcher matcher ( cv::NORM_HAMMING );
    matcher.match ( descriptors_ref_, descriptors_curr_, matches );
    // select the best matches
    float min_dis = std::min_element (
                        matches.begin(), matches.end(),
                        [] ( const cv::DMatch& m1, const cv::DMatch& m2 )
    {
        return m1.distance < m2.distance;
    } )->distance;

    feature_matches_.clear();
    for ( cv::DMatch& m : matches )
    {
        if ( m.distance < max<float> ( min_dis*match_ratio_, 30.0 ) )
        {
            feature_matches_.push_back(m);
        }
    }
    cout<<"good matches: "<<feature_matches_.size()<<endl;
}
void VisualOdometry::setRef3DPoints()
{
    // select the features with depth measurements
    pts_3d_ref_.clear();
    descriptors_ref_ = Mat();
    for ( size_t i=0; i<keypoints_curr_.size(); i++ )
    {
        double d = ref_->findDepth(keypoints_curr_[i]);     // FIXME
        if ( d > 0)
        {
            Vector3d p_cam = ref_->camera_->pixel2camera(
                Vector2d(keypoints_curr_[i].pt.x, keypoints_curr_[i].pt.y), d
            );
            pts_3d_ref_.push_back( cv::Point3f( p_cam(0,0), p_cam(1,0), p_cam(2,0) ));
            descriptors_ref_.push_back(descriptors_curr_.row(i));
        }
    }
}

// Solve PNP problem
void VisualOdometry::poseEstimationPnP()
{
    // construct the 3d 2d observations
    vector<cv::Point3f> pts3d;
    vector<cv::Point2f> pts2d;

    for ( cv::DMatch m:feature_matches_ )
    {
        pts3d.push_back( pts_3d_ref_[m.queryIdx] );
        pts2d.push_back( keypoints_curr_[m.trainIdx].pt );
    }
    // Camera Matrux
    Mat K = ( cv::Mat_<double>(3,3)<<
        ref_->camera_->fx_, 0, ref_->camera_->cx_,
        0, ref_->camera_->fy_, ref_->camera_->cy_,
        0,0,1
    );
    Mat rvec, tvec, inliers;
    cout<<"Pts3d Size is:  "<<pts3d.size()<<endl;
    cout<<"Pts2d Size is:  "<<pts2d.size()<<endl;
    cout<<pts3d[0].x<<"   "<<pts3d[0].y<<"    "<<pts3d[0].z<<endl;
    cout<<pts2d[0].x<<"  "<<pts2d[0].y<<"  "<<endl;
    bool result  = cv::solvePnPRansac( pts3d, pts2d, K, Mat(), rvec, tvec, false, 100, 4.0, 0.99, inliers );
    num_inliers_ = inliers.rows;
    cout<<"pnp inliers: "<<num_inliers_<<endl;

    // process the R and the T, rvec is the rotation vector, tvec is the translation vector
     Mat matrix_rotation;
     cv::Rodrigues(rvec,matrix_rotation);  // 3 x 3 rotation matrix
      int row_mat = matrix_rotation.rows;
      int col_mat = matrix_rotation.cols;
     Eigen::MatrixXd R_eigen(row_mat,col_mat);

      R_eigen<<matrix_rotation.at<double>(0,0),matrix_rotation.at<double>(0,1),matrix_rotation.at<double>(0,2),
     matrix_rotation.at<double>(1,0),matrix_rotation.at<double>(1,1),matrix_rotation.at<double>(1,2),
     matrix_rotation.at<double>(2,0),matrix_rotation.at<double>(2,1),matrix_rotation.at<double>(2,2);

    cout<<R_eigen.matrix()<<endl;
    T_c_r_estimated_ = Sophus::SE3d(
        R_eigen,
        Vector3d( tvec.at<double>(0,0), tvec.at<double>(1,0), tvec.at<double>(2,0))
    );
    cout<<T_c_r_estimated_.matrix() <<endl;

}

bool VisualOdometry::checkEstimatedPose()
{
    // check if the estimated pose is good
    if ( num_inliers_ < min_inliers_ )
    {
        cout<<"reject because inlier is too small: "<<num_inliers_<<endl;
        return false;
    }
    // if the motion is too large, it is probably wrong
    Sophus::Vector6d d = T_c_r_estimated_.log();
    if ( d.norm() > 5.0 )
    {
        cout<<"reject because motion is too large: "<<d.norm()<<endl;
        return false;
    }
    return true;
}
bool VisualOdometry::checkKeyFrame()
{
    Sophus::Vector6d d = T_c_r_estimated_.log();
    Vector3d trans = d.head<3>();
    Vector3d rot = d.tail<3>();
    if ( rot.norm() >key_frame_min_rot || trans.norm() >key_frame_min_trans )
        return true;
    return false;
}
// 匹配比较好的点作为KeyFrame
void VisualOdometry::addKeyFrame()
{
    cout<<"adding a key-frame"<<endl;
    map_->insertKeyFrame ( curr_ );
}
}


#include <boost/geometry/strategies/spherical/distance_cross_track.hpp>


#ifndef IMPL_MESP_INNER_HPP
#define IMPL_MESP_INNER_HPP

#include <boost/dynamic_bitset.hpp>
#include <queue>
#include <stack>
#include <unordered_map>
#include <unordered_set>
#include <vector>
#include "../common/common.hpp"
#include "../common/graph.hpp"
#include "constrained_set_cover.hpp"

class mesp_inner {
public:
	int k;
	int pi_first, pi_last;
	std::shared_ptr<const graph> G;
	std::shared_ptr<const std::unordered_set<int>> C;
	path solution;

private:
	std::unordered_set<int> L;
	std::vector<int> pi;
	std::unordered_map<int, int> e;

public:
	mesp_inner(const std::shared_ptr<const graph> &G, const std::shared_ptr<const std::unordered_set<int>> &C, int k, int pi_first = -1, int pi_last = -1):
		G(G),
		C(C),
		k(k),
		pi_first(pi_first),
		pi_last(pi_last)
	{}

	bool solve()
	{
		init_L();
		do {
			init_pi();
			do {
				if (!can_pi()) continue;
				init_e();
				do {
					if (solve_inner()) return true;
				} while (next_e());
			} while (next_pi());
		} while (next_L());
		return false;
	}

private:
	bool solve_inner()
	{
		auto candidate_segments = get_segments();
		if (!candidate_segments.has_value()) return false;
		std::unordered_set<int> I(L.begin(), L.end());
		std::vector<int> h_inv((*candidate_segments).size(), -1);
		std::vector<std::vector<path>> candidates;
		for (int i = 0; i < candidate_segments->size(); i++) {
			if ((*candidate_segments)[i].size() == 1) {
				for (int u : (*candidate_segments)[i][0]) I.insert(u);
				continue;
			}
			candidates.emplace_back(std::move((*candidate_segments)[i]));
			h_inv[i] = candidates.size() - 1;
		}

		std::unordered_set<int> U;
		for (int v = 0; v < G->n; v++) {
			if (C->count(v) || I.count(v)) continue;
			if (estimate_path_dst(v) > k + 1) return false;
			if (estimate_path_dst(v) == k + 1) U.insert(v);
		}
		if (U.size() > 2 * (pi.size() - 1)) return false;

		std::vector<int> requirements;
		for (int u = 0; u < G->n; u++) {
			if (L.count(u)) continue;
			if (!C->count(u) && !U.count(u)) continue;
			int need_dst = e.count(u) ? e[u] : k;
			if (G->distance(u, I) <= need_dst) continue;
			requirements.push_back(u);
		}
		const std::function<boost::dynamic_bitset<>(const path &)> psi = [this, &requirements] (const path &segment) {
			boost::dynamic_bitset<> res(requirements.size(), 0);
			if (segment.empty()) return res;
			for (int i = 0; i < requirements.size(); i++) {
				int u = requirements[i];
				int need_dst = e.count(u) ? e[u] : k;
				if (G->distance(u, segment) <= need_dst) {
					res[i] = true;
				}
			}
			return res;
		};
		auto true_segment_id = constrained_set_cover(requirements, candidates, psi);
		if (!true_segment_id.has_value()) return false;

		solution.clear();
		for (int i = 0; i < pi.size() - 1; i++) {
			solution.push_back(pi[i]);
			path &segment = h_inv[i] == -1 ? (*candidate_segments)[i][0] : candidates[h_inv[i]][(*true_segment_id)[h_inv[i]]];
			for (int s : segment) solution.push_back(s);
		}
		solution.push_back(pi.back());
		return G->ecc(solution) <= k;
	}

	std::optional<std::vector<std::vector<path>>> get_segments() const
	{
		std::vector<std::vector<path>> candidate_segments(pi.size() - 1);
		for (int i = 0; i < pi.size() - 1; i++) {
			std::queue<int> q;
			q.push(pi[i + 1]);
			std::unordered_map<int, int> dst = {{pi[i + 1], 0}};
			while (!q.empty()) {
				int u = q.front();
				q.pop();
				if (u == pi[i]) break;
				for (int v : G->neighbors(u)) {
					if (dst.count(v)) continue;
					if (C->count(v) && v != pi[i]) continue;
					dst[v] = dst[u] + 1;
					q.push(v);
				}
			}
			if (!dst.count(pi[i])) return std::nullopt;
			if (dst[pi[i]] != G->distance(pi[i + 1], pi[i])) return std::nullopt;
			std::vector<path> Sigma;
			std::vector<int> K;
			for (int u : G->neighbors(pi[i])) {
				if (!dst.count(u) || dst[u] != dst[pi[i]] - 1) continue;
				if (u == pi[i + 1]) {
					Sigma.emplace_back();
					break;
				}
				for (int v : G->neighbors(u)) {
					if (!dst.count(v) || dst[v] != dst[u] - 1) continue;
					Sigma.push_back({u});
					int K_added = 0;
					while (v != pi[i + 1]) {
						int p = Sigma.back().back();
						if (estimate_path_dst(v) > k) {
							if (K_added++ < 2) K.push_back(v);
							if (K.size() > 4) return std::nullopt;
						}
						Sigma.back().push_back(v);
						for (int n : G->neighbors(v)) {
							if (!dst.count(n) || dst[n] != dst[v] - 1) continue;
							if (n == p) continue;
							v = n;
							break;
						}
					}
				}
			}
			for (auto &segment : Sigma) {
				std::vector<int> K_sat(K.size(), 0);
				for (int j = 0; j < K.size(); j++) {
					K_sat[j] |= G->distance(K[j], segment) <= k;
				}
				bool is_sat = true;
				for (bool s : K_sat) is_sat |= s;
				if (is_sat) {
					candidate_segments[i].emplace_back(std::move(segment));
				}
			}
		}
		return candidate_segments;
	}

	void init_L()
	{
		L.clear();
		auto C_iter = C->begin();
		if (pi_first == -1) {
			L.insert(*C_iter);
			++C_iter;
		} else {
			L.insert(pi_first);
		}
		if (pi_last == -1) {
			L.insert(*C_iter);
		} else {
			L.insert(pi_last);
		}
	}

	bool next_L()
	{
		int max_size = C->size();
		if (pi_first != -1) max_size++;
		if (pi_last != -1) max_size++;
		if (L.size() == max_size) return false;
		do {
			for (int v : *C) {
				if (L.count(v)) {
					L.erase(v);
				} else {
					L.insert(v);
					break;
				}
			}
		} while (L.size() < 2);
		return true;
	}

	void init_pi()
	{
		pi.clear();
		pi.reserve(L.size());
		if (pi_first != -1) pi.push_back(pi_first);
		for (int u : L) {
			if (u == pi_first || u == pi_last) continue;
			pi.push_back(u);
		}
		if (pi_last != -1) pi.push_back(pi_last);
		std::sort(pi.begin() + (pi_first == -1 ? 0 : 1), pi.end() - (pi_last == -1 ? 0 : 1));
	}

	bool can_pi() const
	{
		int length = 0;
		for (int i = 0; i < pi.size() - 1; i++) {
			length += G->distance(pi[i], pi[i + 1]);
		}
		return length == G->distance(pi[0], pi.back());
	}

	bool next_pi()
	{
		int first = pi_first == -1 ? 0 : 1;
		int last = (int) pi.size() - (pi_last == -1 ? 1 : 2);
		if (first >= last) return false;
		int m = last - 1;
		while (m >= first && pi[m] > pi[m + 1]) m--;
		if (m < first) return false;
		int k = last;
		while (pi[m] > pi[k]) k--;
		std::swap(pi[m], pi[k]);
		int p = m + 1;
		int q = last;
		while (p < q) {
			std::swap(pi[p], pi[q]);
			p++;
			q--;
		}
		return true;
	}

	void init_e()
	{
		e.clear();
		for (int v : *C) {
			if (L.count(v)) continue;
			e[v] = 1;
		}
	}

	bool next_e()
	{
		int cnt = 0;
		for (int v : *C) {
			if (L.count(v)) continue;
			if (e[v] < k) {
				e[v]++;
				break;
			} else {
				e[v] = 1;
				cnt++;
			}
		}
		return cnt < e.size();
	}

	int estimate_path_dst(int u) const
	{
		int res = INF;
		if (pi_first != -1) {
			res = std::min(res, G->distance(u, pi_first));
		}
		if (pi_last != -1) {
			res = std::min(res, G->distance(u, pi_last));
		}
		for (int v : *C) {
			int e_v = e.count(v) ? e.at(v) : 0;
			res = std::min(res, G->distance(u, v) + e_v);
		}
		return res;
	}

};

#endif //IMPL_MESP_INNER_HPP


/*
 Copyright (C) 2017 Quaternion Risk Management Ltd
 All rights reserved.

 This file is part of ORE, a free-software/open-source library
 for transparent pricing and risk analysis - http://opensourcerisk.org

 ORE is free software: you can redistribute it and/or modify it
 under the terms of the Modified BSD License.  You should have received a
 copy of the license along with this program.
 The license is also available online at <http://opensourcerisk.org>

 This program is distributed on the basis that it will form a useful
 contribution to risk analytics and model standardisation, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
 FITNESS FOR A PARTICULAR PURPOSE. See the license for more details.
*/

#include <boost/make_shared.hpp>
#include <boost/test/unit_test.hpp>
#include <ored/marketdata/marketimpl.hpp>
#include <ored/portfolio/builders/cms.hpp>
#include <ored/portfolio/capfloor.hpp>
#include <ored/portfolio/enginedata.hpp>
#include <ored/portfolio/envelope.hpp>
#include <ored/portfolio/legdata.hpp>
#include <ored/portfolio/schedule.hpp>
#include <ored/portfolio/swap.hpp>
#include <ored/utilities/indexparser.hpp>
#include <oret/toplevelfixture.hpp>
#include <ql/termstructures/volatility/swaption/swaptionconstantvol.hpp>
#include <ql/termstructures/yield/flatforward.hpp>
#include <ql/time/calendars/target.hpp>
#include <ql/time/daycounters/actualactual.hpp>

#include <iostream>

using namespace QuantLib;
using namespace boost::unit_test_framework;
using namespace std;
using namespace ore::data;

// CMS Swap test

namespace {

class TestMarket : public MarketImpl {
public:
    TestMarket() {
        asof_ = Date(3, Feb, 2016);

        // build discount
        yieldCurves_[make_tuple(Market::defaultConfiguration, YieldCurveType::Discount, "EUR")] = flatRateYts(0.02);

        // build swaption vols
        swaptionCurves_[make_pair(Market::defaultConfiguration, "EUR")] = flatRateSvs(0.1);

        // build ibor index
        Handle<IborIndex> hEUR(ore::data::parseIborIndex(
            "EUR-EURIBOR-6M", yieldCurves_[make_tuple(Market::defaultConfiguration, YieldCurveType::Discount, "EUR")]));
        iborIndices_[make_pair(Market::defaultConfiguration, "EUR-EURIBOR-6M")] = hEUR;

        // add swap index
        boost::shared_ptr<ore::data::Convention> swapEURConv(new ore::data::IRSwapConvention(
            "EUR-6M-SWAP-CONVENTIONS", "TARGET", "Annual", "MF", "30/360", "EUR-EURIBOR-6M"));
        conventions_.add(swapEURConv);
        boost::shared_ptr<ore::data::Convention> swapIndexEURLongConv(
            new ore::data::SwapIndexConvention("EUR-CMS-30Y", "EUR-6M-SWAP-CONVENTIONS"));
        conventions_.add(swapIndexEURLongConv);
        addSwapIndex("EUR-CMS-30Y", "EUR-EURIBOR-6M", Market::defaultConfiguration);
    }

private:
    Handle<YieldTermStructure> flatRateYts(Real forward) {
        boost::shared_ptr<YieldTermStructure> yts(new FlatForward(0, NullCalendar(), forward, ActualActual()));
        return Handle<YieldTermStructure>(yts);
    }
    Handle<SwaptionVolatilityStructure> flatRateSvs(Volatility forward) {
        boost::shared_ptr<SwaptionVolatilityStructure> Svs(
            new ConstantSwaptionVolatility(0, NullCalendar(), ModifiedFollowing, forward, ActualActual()));
        return Handle<SwaptionVolatilityStructure>(Svs);
    }
};

struct CommonVars {
    // global data
    string ccy;
    bool isPayer;
    string start;
    string end;
    string fixtenor;
    string cmstenor;
    Calendar cal;
    string calStr;
    string conv;
    string rule;
    Size days;
    string fixDC;
    Real fixedRate;
    string index;
    int fixingdays;
    bool isinarrears;
    double notional;
    string longShort;
    vector<double> notionals;
    vector<double> spread;
    vector<string> spreadDates;

    // utilities
    boost::shared_ptr<ore::data::Swap> makeSwap() {
        ScheduleData fixedSchedule(ScheduleRules(start, end, fixtenor, calStr, conv, conv, rule));
        ScheduleData cmsSchedule(ScheduleRules(start, end, cmstenor, calStr, conv, conv, rule));

        // build CMSSwap
        LegData fixedLegData(boost::make_shared<FixedLegData>(vector<double>(1, fixedRate)), !isPayer, ccy,
                             fixedSchedule, fixDC, notionals);
        LegData cmsLegData(boost::make_shared<CMSLegData>(index, fixingdays, isinarrears, spread), isPayer, ccy,
                           cmsSchedule, fixDC, notionals);

        Envelope env("CP1");
        boost::shared_ptr<ore::data::Swap> swap(new ore::data::Swap(env, fixedLegData, cmsLegData));
        return swap;
    }

    boost::shared_ptr<ore::data::Swap> makeSwap(Real fixedRate_, string fixtenor_) {
        ScheduleData fixedSchedule(ScheduleRules(start, end, fixtenor_, calStr, conv, conv, rule));
        ScheduleData cmsSchedule(ScheduleRules(start, end, cmstenor, calStr, conv, conv, rule));

        // build CMSSwap
        LegData fixedLegData(boost::make_shared<FixedLegData>(vector<double>(1, fixedRate_)), !isPayer, ccy,
                             fixedSchedule, fixDC, notionals);
        LegData cmsLegData(boost::make_shared<CMSLegData>(index, fixingdays, isinarrears, spread), isPayer, ccy,
                           cmsSchedule, fixDC, notionals);

        Envelope env("CP1");
        boost::shared_ptr<ore::data::Swap> swap(new ore::data::Swap(env, fixedLegData, cmsLegData));
        return swap;
    }

    boost::shared_ptr<ore::data::Swap> makeCmsLegSwap() {
        ScheduleData cmsSchedule(ScheduleRules(start, end, cmstenor, calStr, conv, conv, rule));

        vector<LegData> legData;
        CMSLegData cmsLegRateData;
        LegData cmsLegData(boost::make_shared<CMSLegData>(index, fixingdays, isinarrears, spread), isPayer, ccy,
                           cmsSchedule, fixDC, notionals);
        legData.push_back(cmsLegData);

        Envelope env("CP1");
        boost::shared_ptr<ore::data::Swap> swap(new ore::data::Swap(env, legData));
        return swap;
    }

    boost::shared_ptr<ore::data::Swap> makeCappedCmsLegSwap(vector<double> caps,
                                                            vector<string> capDates = vector<string>()) {
        ScheduleData cmsSchedule(ScheduleRules(start, end, cmstenor, calStr, conv, conv, rule));

        vector<LegData> legData;
        LegData cmsLegData(
            boost::make_shared<CMSLegData>(index, fixingdays, isinarrears, spread, spreadDates, caps, capDates),
            isPayer, ccy, cmsSchedule, fixDC, notionals);
        legData.push_back(cmsLegData);

        Envelope env("CP1");
        boost::shared_ptr<ore::data::Swap> swap(new ore::data::Swap(env, legData));
        return swap;
    }

    boost::shared_ptr<ore::data::CapFloor> makeCap(vector<double> caps) {
        ScheduleData cmsSchedule(ScheduleRules(start, end, cmstenor, calStr, conv, conv, rule));

        LegData cmsLegData(boost::make_shared<CMSLegData>(index, fixingdays, isinarrears, spread, spreadDates), isPayer,
                           ccy, cmsSchedule, fixDC, notionals);

        Envelope env("CP1");
        boost::shared_ptr<ore::data::CapFloor> capfloor(
            new ore::data::CapFloor(env, longShort, cmsLegData, caps, vector<double>()));
        return capfloor;
    }

    boost::shared_ptr<ore::data::Swap> makeFlooredCmsLegSwap(vector<double> floors,
                                                             vector<string> floorDates = vector<string>()) {
        ScheduleData cmsSchedule(ScheduleRules(start, end, cmstenor, calStr, conv, conv, rule));

        vector<LegData> legData;
        LegData cmsLegData(boost::make_shared<CMSLegData>(index, fixingdays, isinarrears, spread, spreadDates,
                                                          vector<double>(), vector<string>(), floors, floorDates),
                           isPayer, ccy, cmsSchedule, fixDC, notionals);
        legData.push_back(cmsLegData);

        Envelope env("CP1");
        boost::shared_ptr<ore::data::Swap> swap(new ore::data::Swap(env, legData));
        return swap;
    }

    boost::shared_ptr<ore::data::CapFloor> makeFloor(vector<double> floors) {
        ScheduleData cmsSchedule(ScheduleRules(start, end, cmstenor, calStr, conv, conv, rule));

        LegData cmsLegData(boost::make_shared<CMSLegData>(index, fixingdays, isinarrears, spread, spreadDates), isPayer,
                           ccy, cmsSchedule, fixDC, notionals);

        Envelope env("CP1");
        boost::shared_ptr<ore::data::CapFloor> capfloor(
            new ore::data::CapFloor(env, longShort, cmsLegData, vector<double>(), floors));
        return capfloor;
    }

    CommonVars() : ccy("EUR"), isPayer(false), start("20160301"), end("20360301"), fixtenor("1Y"), cmstenor("6M") {

        longShort = isPayer ? "Short" : "Long";
        cal = TARGET();
        calStr = "TARGET";
        conv = "MF";
        rule = "Forward";
        fixDC = "ACT/360";
        fixedRate = 0.0;
        index = "EUR-CMS-30Y";
        fixingdays = 2;
        isinarrears = false;
        notional = 10000000;
        notionals.push_back(10000000);
        spread.push_back(0.0);
    }
};

void outputCoupons(boost::shared_ptr<ore::data::Swap> cmsSwap) {
    Leg leg = cmsSwap->legs().at(1);
    for (auto cf : leg) {
        boost::shared_ptr<FloatingRateCoupon> frc = boost::dynamic_pointer_cast<FloatingRateCoupon>(cf);
        BOOST_TEST_MESSAGE("Coupon Date: " << frc->date() << "; Rate: " << frc->rate()
                                           << "; DayCount: " << frc->dayCounter());
    }
};
} // namespace

BOOST_FIXTURE_TEST_SUITE(OREDataTestSuite, ore::test::TopLevelFixture)

BOOST_AUTO_TEST_SUITE(CmsTests)

BOOST_AUTO_TEST_CASE(testCMSAnalyticHagan) {
    BOOST_TEST_MESSAGE("Testing CMS Analytic Hagan price...");

    // build market
    boost::shared_ptr<Market> market = boost::make_shared<TestMarket>();
    Settings::instance().evaluationDate() = market->asofDate();
    CommonVars vars;
    boost::shared_ptr<ore::data::Swap> cmsSwap = vars.makeSwap();

    // Build and price
    boost::shared_ptr<EngineData> engineData = boost::make_shared<EngineData>();
    engineData->model("CMS") = "Hagan";
    engineData->engine("CMS") = "Analytic";
    map<string, string> engineparams;
    engineparams["YieldCurveModel"] = "Standard";
    engineparams["MeanReversion"] = "0.0";
    engineData->engineParameters("CMS") = engineparams;

    engineData->model("Swap") = "DiscountedCashflows";
    engineData->engine("Swap") = "DiscountingSwapEngineOptimised";

    boost::shared_ptr<EngineFactory> engineFactory = boost::make_shared<EngineFactory>(engineData, market);
    engineFactory->registerBuilder(boost::make_shared<AnalyticHaganCmsCouponPricerBuilder>());

    cmsSwap->build(engineFactory);

    Real expectedNPV = 3440673.46;
    Real npv = cmsSwap->instrument()->NPV();

    BOOST_TEST_MESSAGE("Hagan Analytic price is " << npv);
    outputCoupons(cmsSwap);

    BOOST_CHECK_CLOSE(npv, expectedNPV, 1.0);
}

BOOST_AUTO_TEST_CASE(testCMSNumericalHagan) {
    BOOST_TEST_MESSAGE("Testing CMS Numerical Hagan price...");

    // build market
    boost::shared_ptr<Market> market = boost::make_shared<TestMarket>();
    Settings::instance().evaluationDate() = market->asofDate();

    CommonVars vars;
    boost::shared_ptr<ore::data::Swap> cmsSwap = vars.makeSwap();

    // Build and price
    boost::shared_ptr<EngineData> engineData = boost::make_shared<EngineData>();
    engineData->model("CMS") = "Hagan";
    engineData->engine("CMS") = "Numerical";
    map<string, string> engineparams;
    engineparams["YieldCurveModel"] = "Standard";
    engineparams["MeanReversion"] = "0.0";
    engineparams["UpperLimit"] = "0.0";
    engineparams["LowerLimit"] = "1.0";
    engineparams["Precision"] = "0.000001";
    engineData->engineParameters("CMS") = engineparams;

    engineData->model("Swap") = "DiscountedCashflows";
    engineData->engine("Swap") = "DiscountingSwapEngineOptimised";

    boost::shared_ptr<EngineFactory> engineFactory = boost::make_shared<EngineFactory>(engineData, market);
    engineFactory->registerBuilder(boost::make_shared<NumericalHaganCmsCouponPricerBuilder>());

    cmsSwap->build(engineFactory);

    Real expectedNPV = 3440673.46;
    Real npv = cmsSwap->instrument()->NPV();

    BOOST_TEST_MESSAGE("Hagan Numerical price is " << npv);
    outputCoupons(cmsSwap);

    BOOST_CHECK_CLOSE(npv, expectedNPV, 1.0);
}

BOOST_AUTO_TEST_CASE(testCMSLinearTsr) {
    BOOST_TEST_MESSAGE("Testing CMS Linear TSR price...");

    // build market
    boost::shared_ptr<Market> market = boost::make_shared<TestMarket>();
    Settings::instance().evaluationDate() = market->asofDate();

    CommonVars vars;
    boost::shared_ptr<ore::data::Swap> cmsSwap = vars.makeSwap();

    // Build and price
    boost::shared_ptr<EngineData> engineData = boost::make_shared<EngineData>();
    engineData->model("CMS") = "LinearTSR";
    engineData->engine("CMS") = "LinearTSRPricer";
    map<string, string> engineparams;
    engineparams["MeanReversion"] = "0.0";
    engineparams["Policy"] = "RateBound";
    engineparams["LowerRateBoundLogNormal"] = "0.0001";
    engineparams["UpperRateBoundLogNormal"] = "2.0000";
    engineData->engineParameters("CMS") = engineparams;

    engineData->model("Swap") = "DiscountedCashflows";
    engineData->engine("Swap") = "DiscountingSwapEngineOptimised";

    boost::shared_ptr<EngineFactory> engineFactory = boost::make_shared<EngineFactory>(engineData, market);
    engineFactory->registerBuilder(boost::make_shared<LinearTSRCmsCouponPricerBuilder>());

    cmsSwap->build(engineFactory);

    Real expectedNPV = 3440673.46;
    Real npv = cmsSwap->instrument()->NPV();

    BOOST_TEST_MESSAGE("Linear TSR price is " << npv);
    outputCoupons(cmsSwap);

    BOOST_CHECK_CLOSE(npv, expectedNPV, 1.0);
}

BOOST_AUTO_TEST_CASE(cmsCapFloor) {
    BOOST_TEST_MESSAGE("Testing CMS CapFloor price...");

    // build market
    boost::shared_ptr<Market> market = boost::make_shared<TestMarket>();
    Settings::instance().evaluationDate() = market->asofDate();

    CommonVars vars;
    boost::shared_ptr<EngineData> engineData = boost::make_shared<EngineData>();
    engineData->model("CMS") = "Hagan";
    engineData->engine("CMS") = "Analytic";
    map<string, string> engineparams;
    engineparams["YieldCurveModel"] = "Standard";
    engineparams["MeanReversion"] = "0.0";
    engineData->engineParameters("CMS") = engineparams;

    engineData->model("Swap") = "DiscountedCashflows";
    engineData->engine("Swap") = "DiscountingSwapEngineOptimised";

    boost::shared_ptr<EngineFactory> engineFactory = boost::make_shared<EngineFactory>(engineData, market);
    engineFactory->registerBuilder(boost::make_shared<AnalyticHaganCmsCouponPricerBuilder>());

    BOOST_TEST_MESSAGE(
        "Comparing CMS Cap price to replication by a Single Legged CMS Swap and a Single Leg Capped CMS Swap...");
    vector<double> capRate(1, 0.021);
    boost::shared_ptr<ore::data::Swap> cmsLegSwap = vars.makeCmsLegSwap();
    boost::shared_ptr<ore::data::Swap> cappedCmsLegSwap = vars.makeCappedCmsLegSwap(capRate);
    boost::shared_ptr<ore::data::CapFloor> cap = vars.makeCap(capRate);

    cmsLegSwap->build(engineFactory);
    cappedCmsLegSwap->build(engineFactory);
    cap->build(engineFactory);

    Real cmsLegNpv = cmsLegSwap->instrument()->NPV();
    Real cappedCmsLegNpv = cappedCmsLegSwap->instrument()->NPV();
    Real capNpv = cap->instrument()->NPV();

    Real capBySwaps = cmsLegNpv - cappedCmsLegNpv;

    BOOST_TEST_MESSAGE("CMS Leg swap NPV is " << cmsLegNpv);
    BOOST_TEST_MESSAGE("CMS Capped Leg swap NPV is " << cappedCmsLegNpv);
    BOOST_TEST_MESSAGE("CMS Cap NPV is " << capNpv);
    BOOST_TEST_MESSAGE("CMS Cap NPV from Swap replication is " << capBySwaps);
    BOOST_CHECK_CLOSE(capNpv, capBySwaps, 1.0);

    BOOST_TEST_MESSAGE("Checking CMS Cap with high Cap is zero...");
    vector<double> capHigh(1, 1.0);

    cap = vars.makeCap(capHigh);
    cap->build(engineFactory);
    capNpv = cap->instrument()->NPV();
    BOOST_TEST_MESSAGE("CMS Cap (Cap of 100%) NPV is " << capNpv);
    BOOST_CHECK_SMALL(capNpv, 0.01);

    BOOST_TEST_MESSAGE("Checking CMS Cap with low Cap is equal to single leg swap...");
    vector<double> capLow(1, -1.0);

    cap = vars.makeCap(capLow);
    cap->build(engineFactory);
    capNpv = cap->instrument()->NPV();
    BOOST_TEST_MESSAGE("CMS Cap (Cap of -100%) NPV is " << capNpv);
    BOOST_CHECK_CLOSE(capNpv, cmsLegNpv, 1.0);

    BOOST_TEST_MESSAGE("Checking CMS Floor with low Cap is equal to zero...");
    vector<double> floorLow(1, -1.0);

    boost::shared_ptr<ore::data::CapFloor> floor = vars.makeFloor(floorLow);
    floor->build(engineFactory);
    Real floorNpv = floor->instrument()->NPV();
    BOOST_TEST_MESSAGE("CMS Floor (Flooe of -100%) NPV is " << floorNpv);
    BOOST_CHECK_SMALL(floorNpv, 0.01);

    BOOST_TEST_MESSAGE("Checking CMS Cap + CMS Floor = Swap...");
    vector<double> floorRate(1, 0.021);

    cap = vars.makeCap(capRate);
    floor = vars.makeFloor(floorRate);
    boost::shared_ptr<ore::data::Swap> swap = vars.makeSwap(0.021, "6M");
    cap->build(engineFactory);
    floor->build(engineFactory);
    swap->build(engineFactory);
    capNpv = cap->instrument()->NPV();
    floorNpv = floor->instrument()->NPV();
    Real swapNpv = swap->instrument()->NPV();
    Real capFloorNpv = capNpv - floorNpv;
    BOOST_TEST_MESSAGE("CMS Cap NPV is " << capNpv);
    BOOST_TEST_MESSAGE("CMS Floor NPV is " << floorNpv);
    BOOST_TEST_MESSAGE("CMS Cap + Floor NPV is " << capFloorNpv);
    BOOST_TEST_MESSAGE("CMS Swap NPV is " << swapNpv);
    BOOST_CHECK_CLOSE(capFloorNpv, swapNpv, 1.0);
}

BOOST_AUTO_TEST_SUITE_END()

BOOST_AUTO_TEST_SUITE_END()


#include <boost/function_output_iterator.hpp>
#include <boost/geometry/geometry.hpp>
#include <chrono>
#include <iostream>
#include <random>
#include <vector>

// number of dimensions (must be 2 or 3)
const int D = 2;

// default parameters (documented below)
const double DefaultParticleSpacing = 1;
const double DefaultAttractionDistance = 3;
const double DefaultMinMoveDistance = 1;
const int DefaultStubbornness = 0;
const double DefaultStickiness = 1;

// boost is used for its spatial index
using BoostPoint = boost::geometry::model::point<
    double, D, boost::geometry::cs::cartesian>;

using IndexValue = std::pair<BoostPoint, int>;

using Index = boost::geometry::index::rtree<
    IndexValue, boost::geometry::index::linear<4>>;

// Vector represents a point or a vector
class Vector {
public:
    Vector() :
        m_X(0), m_Y(0), m_Z(0) {}

    Vector(double x, double y) :
        m_X(x), m_Y(y), m_Z(0) {}

    Vector(double x, double y, double z) :
        m_X(x), m_Y(y), m_Z(z) {}

    double X() const {
        return m_X;
    }

    double Y() const {
        return m_Y;
    }

    double Z() const {
        return m_Z;
    }

    BoostPoint ToBoost() const {
        return BoostPoint(m_X, m_Y, m_Z);
    }

    double Length() const {
        return std::sqrt(m_X * m_X + m_Y * m_Y + m_Z * m_Z);
    }

    double LengthSquared() const {
        return m_X * m_X + m_Y * m_Y + m_Z * m_Z;
    }

    double Distance(const Vector &v) const {
        const double dx = m_X - v.m_X;
        const double dy = m_Y - v.m_Y;
        const double dz = m_Z - v.m_Z;
        return std::sqrt(dx * dx + dy * dy + dz * dz);
    }

    Vector Normalized() const {
        const double m = 1 / Length();
        return Vector(m_X * m, m_Y * m, m_Z * m);
    }

    Vector operator+(const Vector &v) const {
        return Vector(m_X + v.m_X, m_Y + v.m_Y, m_Z + v.m_Z);
    }

    Vector operator-(const Vector &v) const {
        return Vector(m_X - v.m_X, m_Y - v.m_Y, m_Z - v.m_Z);
    }

    Vector operator*(const double a) const {
        return Vector(m_X * a, m_Y * a, m_Z * a);
    }

    Vector &operator+=(const Vector &v) {
        m_X += v.m_X; m_Y += v.m_Y; m_Z += v.m_Z;
        return *this;
    }

private:
    double m_X;
    double m_Y;
    double m_Z;
};

// Lerp linearly interpolates from a to b by distance.
Vector Lerp(const Vector &a, const Vector &b, const double d) {
    return a + (b - a).Normalized() * d;
}

// Random returns a uniformly distributed random number between lo and hi
double Random(const double lo = 0, const double hi = 1) {
    static thread_local std::mt19937 gen(
        std::chrono::high_resolution_clock::now().time_since_epoch().count());
    std::uniform_real_distribution<double> dist(lo, hi);
    return dist(gen);
}

// RandomInUnitSphere returns a random, uniformly distributed point inside the
// unit sphere (radius = 1)
Vector RandomInUnitSphere() {
    while (true) {
        const Vector p = Vector(
            Random(-1, 1),
            Random(-1, 1),
            D == 2 ? 0 : Random(-1, 1));
        if (p.LengthSquared() < 1) {
            return p;
        }
    }
}

// Model holds all of the particles and defines their behavior.
class Model {
public:
    Model() :
        m_ParticleSpacing(DefaultParticleSpacing),
        m_AttractionDistance(DefaultAttractionDistance),
        m_MinMoveDistance(DefaultMinMoveDistance),
        m_Stubbornness(DefaultStubbornness),
        m_Stickiness(DefaultStickiness),
        m_BoundingRadius(0) {}

    void SetParticleSpacing(const double a) {
        m_ParticleSpacing = a;
    }

    void SetAttractionDistance(const double a) {
        m_AttractionDistance = a;
    }

    void SetMinMoveDistance(const double a) {
        m_MinMoveDistance = a;
    }

    void SetStubbornness(const int a) {
        m_Stubbornness = a;
    }

    void SetStickiness(const double a) {
        m_Stickiness = a;
    }

    // Add adds a new particle with the specified parent particle
    void Add(const Vector &p, const int parent = -1) {
        const int id = m_Points.size();
        m_Index.insert(std::make_pair(p.ToBoost(), id));
        m_Points.push_back(p);
        m_JoinAttempts.push_back(0);
        m_BoundingRadius = std::max(
            m_BoundingRadius, p.Length() + m_AttractionDistance);
        std::cout
            << id << "," << parent << ","
            << p.X() << "," << p.Y() << "," << p.Z() << std::endl;
    }

    // Nearest returns the index of the particle nearest the specified point
    int Nearest(const Vector &point) const {
        int result = -1;
        m_Index.query(
            boost::geometry::index::nearest(point.ToBoost(), 1),
            boost::make_function_output_iterator([&result](const auto &value) {
                result = value.second;
            }));
        return result;
    }

    // RandomStartingPosition returns a random point to start a new particle
    Vector RandomStartingPosition() const {
        const double d = m_BoundingRadius;
        return RandomInUnitSphere().Normalized() * d;
    }

    // ShouldReset returns true if the particle has gone too far away and
    // should be reset to a new random starting position
    bool ShouldReset(const Vector &p) const {
        return p.Length() > m_BoundingRadius * 2;
    }

    // ShouldJoin returns true if the point should attach to the specified
    // parent particle. This is only called when the point is already within
    // the required attraction distance.
    bool ShouldJoin(const Vector &p, const int parent) {
        m_JoinAttempts[parent]++;
        if (m_JoinAttempts[parent] < m_Stubbornness) {
            return false;
        }
        return Random() <= m_Stickiness;
    }

    // PlaceParticle computes the final placement of the particle.
    Vector PlaceParticle(const Vector &p, const int parent) const {
        return Lerp(m_Points[parent], p, m_ParticleSpacing);
    }

    // MotionVector returns a vector specifying the direction that the
    // particle should move for one iteration. The distance that it will move
    // is determined by the algorithm.
    Vector MotionVector(const Vector &p) const {
        return RandomInUnitSphere();
    }

    // AddParticle diffuses one new particle and adds it to the model
    void AddParticle() {
        // compute particle starting location
        Vector p = RandomStartingPosition();

        // do the random walk
        while (true) {
            // get distance to nearest other particle
            const int parent = Nearest(p);
            const double d = p.Distance(m_Points[parent]);

            // check if close enough to join
            if (d < m_AttractionDistance) {
                if (!ShouldJoin(p, parent)) {
                    // push particle away a bit
                    p = Lerp(m_Points[parent], p,
                        m_AttractionDistance + m_MinMoveDistance);
                    continue;
                }

                // adjust particle position in relation to its parent
                p = PlaceParticle(p, parent);

                // add the point
                Add(p, parent);
                return;
            }

            // move randomly
            const double m = std::max(
                m_MinMoveDistance, d - m_AttractionDistance);
            p += MotionVector(p).Normalized() * m;

            // check if particle is too far away, reset if so
            if (ShouldReset(p)) {
                p = RandomStartingPosition();
            }
        }
    }

private:
    // m_ParticleSpacing defines the distance between particles that are
    // joined together
    double m_ParticleSpacing;

    // m_AttractionDistance defines how close together particles must be in
    // order to join together
    double m_AttractionDistance;

    // m_MinMoveDistance defines the minimum distance that a particle will move
    // during its random walk
    double m_MinMoveDistance;

    // m_Stubbornness defines how many interactions must occur before a
    // particle will allow another particle to join to it.
    int m_Stubbornness;

    // m_Stickiness defines the probability that a particle will allow another
    // particle to join to it.
    double m_Stickiness;

    // m_BoundingRadius defines the radius of the bounding sphere that bounds
    // all of the particles
    double m_BoundingRadius;

    // m_Points stores the final particle positions
    std::vector<Vector> m_Points;

    // m_JoinAttempts tracks how many times other particles have attempted to
    // join with each finalized particle
    std::vector<int> m_JoinAttempts;

    // m_Index is the spatial index used to accelerate nearest neighbor queries
    Index m_Index;
};

int main() {
    // create the model
    Model model;

    // add seed point(s)
    model.Add(Vector());

    // {
    //     const int n = 3600;
    //     const double r = 1000;
    //     for (int i = 0; i < n; i++) {
    //         const double t = (double)i / n;
    //         const double a = t * 2 * M_PI;
    //         const double x = std::cos(a) * r;
    //         const double y = std::sin(a) * r;
    //         model.Add(Vector(x, y, 0));
    //     }
    // }

    // run diffusion-limited aggregation
    for (int i = 0; i < 100000; i++) {
        model.AddParticle();
    }

    return 0;
}