/*
* Copyright (C) 2017 Incognito (Edited by ProMetheus)
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
*     http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/

#include <streamer/config.hpp>
#include "../natives.h"

#include "../core.h"
#include "../main.h"
#include "../utility.h"

#include <streamer/areas.hpp>

#include <boost/geometry.hpp>
#include <boost/geometry/geometries/geometries.hpp>
#include <boost/intrusive_ptr.hpp>
#include <boost/scoped_ptr.hpp>
#include <boost/tuple/tuple.hpp>
#include <boost/unordered_map.hpp>
#include <boost/unordered_set.hpp>
#include <boost/variant.hpp>

#include <Eigen/Core>
#include <vector>

STREAMER_BEGIN_NS

int CreateDynamicCircle(float x, float y, float size, int worldid, int interiorid, int playerid, int priority) {
	if (core->getData()->getGlobalMaxItems(STREAMER_TYPE_AREA) == core->getData()->areas.size()) {
		return 0;
	}

	int areaId = Item::Area::identifier.get();
	Item::SharedArea area(new Item::Area);
	//area->amx = amx;
	area->areaId = areaId;
	area->type = STREAMER_AREA_TYPE_CIRCLE;
	area->position = Eigen::Vector2f(x, y);
	area->comparableSize = size * size;
	area->size = size;

	Utility::addToContainer(area->worlds, worldid);
	Utility::addToContainer(area->interiors, interiorid);
	Utility::addToContainer(area->players, playerid);
	area->priority = priority;
	core->getGrid()->addArea(area);
	core->getData()->areas.insert(std::make_pair(areaId, area));
	return areaId;
}

int CreateDynamicCylinder(float x, float y, float minz, float maxz, float size, int worldid, int interiorid, int playerid, int priority) {
	if (core->getData()->getGlobalMaxItems(STREAMER_TYPE_AREA) == core->getData()->areas.size()) {
		return 0;
	}

	int areaId = Item::Area::identifier.get();
	Item::SharedArea area(new Item::Area);
	//area->amx = amx;
	area->areaId = areaId;
	area->type = STREAMER_AREA_TYPE_CYLINDER;
	area->position = Eigen::Vector2f(x, y);
	area->height = Eigen::Vector2f(minz, maxz);
	area->comparableSize = size * size;
	area->size = size;

	Utility::addToContainer(area->worlds, worldid);
	Utility::addToContainer(area->interiors, interiorid);
	Utility::addToContainer(area->players,  playerid);
	area->priority = priority;
	core->getGrid()->addArea(area);
	core->getData()->areas.insert(std::make_pair(areaId, area));
	return areaId;
}

int CreateDynamicSphere(float x, float y, float z, float size, int worldid, int interiorid, int playerid, int priority) {
	if (core->getData()->getGlobalMaxItems(STREAMER_TYPE_AREA) == core->getData()->areas.size()) {
		return 0;
	}

	int areaId = Item::Area::identifier.get();
	Item::SharedArea area(new Item::Area);
	//area->amx = amx;
	area->areaId = areaId;
	area->spectateMode = true;
	area->type = STREAMER_AREA_TYPE_SPHERE;
	area->position = Eigen::Vector3f(x, y, z);
	area->comparableSize = size * size;
	area->size = size;

	Utility::addToContainer(area->worlds, worldid);
	Utility::addToContainer(area->interiors, interiorid);
	Utility::addToContainer(area->players, playerid);
	area->priority = priority;
	core->getGrid()->addArea(area);
	core->getData()->areas.insert(std::make_pair(areaId, area));
	return areaId;
}

int CreateDynamicRectangle(float minx, float miny, float maxx, float maxy, int worldid, int interiorid, int playerid, int priority) {
	if (core->getData()->getGlobalMaxItems(STREAMER_TYPE_AREA) == core->getData()->areas.size()) {
		return 0;
	}

	int areaId = Item::Area::identifier.get();
	Item::SharedArea area(new Item::Area);
	//area->amx = amx;
	area->areaId = areaId;
	area->spectateMode = true;
	area->type = STREAMER_AREA_TYPE_RECTANGLE;
	area->position = Box2d(Eigen::Vector2f(minx, miny), Eigen::Vector2f(maxx, maxy));
	boost::geometry::correct(boost::get<Box2d>(area->position));
	area->comparableSize = static_cast<float>(boost::geometry::comparable_distance(boost::get<Box2d>(area->position).min_corner(), boost::get<Box2d>(area->position).max_corner()));
	area->size = static_cast<float>(boost::geometry::distance(boost::get<Box2d>(area->position).min_corner(), boost::get<Box2d>(area->position).max_corner()));

	Utility::addToContainer(area->worlds, worldid);
	Utility::addToContainer(area->interiors, interiorid);
	Utility::addToContainer(area->players, playerid);
	area->priority = priority;
	core->getGrid()->addArea(area);
	core->getData()->areas.insert(std::make_pair(areaId, area));
	return areaId;
}

int CreateDynamicCuboid(float minx, float miny, float minz, float maxx, float maxy, float maxz, int worldid, int interiorid, int playerid, int priority) {
	if (core->getData()->getGlobalMaxItems(STREAMER_TYPE_AREA) == core->getData()->areas.size()) {
		return 0;
	}

	int areaId = Item::Area::identifier.get();
	Item::SharedArea area(new Item::Area);
	//area->amx = amx;
	area->areaId = areaId;
	area->spectateMode = true;
	area->type = STREAMER_AREA_TYPE_CUBOID;
	area->position = Box3d(Eigen::Vector3f(minx, miny, minz), Eigen::Vector3f(maxx, maxy, maxz));
	boost::geometry::correct(boost::get<Box3d>(area->position));
	area->comparableSize = static_cast<float>(boost::geometry::comparable_distance(Eigen::Vector2f(boost::get<Box3d>(area->position).min_corner()[0], boost::get<Box3d>(area->position).min_corner()[1]), Eigen::Vector2f(boost::get<Box3d>(area->position).max_corner()[0], boost::get<Box3d>(area->position).max_corner()[1])));
	area->size = static_cast<float>(boost::geometry::distance(Eigen::Vector2f(boost::get<Box3d>(area->position).min_corner()[0], boost::get<Box3d>(area->position).min_corner()[1]), Eigen::Vector2f(boost::get<Box3d>(area->position).max_corner()[0], boost::get<Box3d>(area->position).max_corner()[1])));

	Utility::addToContainer(area->worlds, worldid);
	Utility::addToContainer(area->interiors, interiorid);
	Utility::addToContainer(area->players, playerid);
	area->priority = priority;
	core->getGrid()->addArea(area);
	core->getData()->areas.insert(std::make_pair(areaId, area));
	return areaId;
}

int CreateDynamicPolygon(std::vector<Eigen::Vector2f> points, float minz, float maxz, int maxpoints, int worldid, int interiorid, int playerid, int priority) {
	if (core->getData()->getGlobalMaxItems(STREAMER_TYPE_AREA) == core->getData()->areas.size()) {
		return 0;
	}

	if (static_cast<int>(maxpoints >= 2 && static_cast<int>(maxpoints) % 2)) {
		Utility::logError("CreateDynamicPolygon: Number of points must be divisible by two");
		return 0;
	}

	int areaId = Item::Area::identifier.get();
	Item::SharedArea area(new Item::Area);
	//area->amx = amx;
	area->areaId = areaId;
	area->spectateMode = true;
	area->type = STREAMER_AREA_TYPE_POLYGON;

	Polygon2d pol = boost::get<Polygon2d>(area->position);
	boost::geometry::assign_points(pol, points);
	boost::geometry::correct(pol);

	area->height = Eigen::Vector2f(minz, maxz);
	Box2d box = boost::geometry::return_envelope<Box2d>(boost::get<Polygon2d>(area->position));
	area->comparableSize = static_cast<float>(boost::geometry::comparable_distance(box.min_corner(), box.max_corner()));
	area->size = static_cast<float>(boost::geometry::distance(box.min_corner(), box.max_corner()));
	Utility::addToContainer(area->worlds, worldid);
	Utility::addToContainer(area->interiors, interiorid);
	Utility::addToContainer(area->players, playerid);
	area->priority = priority;
	core->getGrid()->addArea(area);
	core->getData()->areas.insert(std::make_pair(areaId, area));
	return areaId;
}

int DestroyDynamicArea(int id)
{
	Utility::executeFinalAreaCallbacks(id);
	boost::unordered_map<int, Item::SharedArea>::iterator a = core->getData()->areas.find(id);
	if (a != core->getData()->areas.end()) {
		Utility::destroyArea(a);
		return 1;
	}
	return 0;
}

int IsValidDynamicArea(int id) {
	boost::unordered_map<int, Item::SharedArea>::iterator a = core->getData()->areas.find(id);
	if (a != core->getData()->areas.end()) {
		return 1;
	}
	return 0;
}

// TODO: <sphinx> Check this out
Polygon2d GetDynamicPolygonPoints(int id, std::vector<Eigen::Vector2f> points, int maxpoints) {
	boost::unordered_map<int, Item::SharedArea>::iterator a = core->getData()->areas.find(id);
	if (a != core->getData()->areas.end()) {
		return boost::get<Polygon2d>(a->second->position);
	}
	return Polygon2d();
}

int GetDynamicPolygonNumberPoints(int id) {
	boost::unordered_map<int, Item::SharedArea>::iterator a = core->getData()->areas.find(id);
	if (a != core->getData()->areas.end()) {
		return boost::get<Polygon2d>(a->second->position).outer().size();
	}
	return 0;
}

int IsPlayerInDynamicArea(int playerId, int areaId, bool recheck) {
	boost::unordered_map<int, Player>::iterator p = core->getData()->players.find(playerId);
	if (p != core->getData()->players.end()) {
		if (!recheck) {
			boost::unordered_set<int>::iterator i = p->second.internalAreas.find(areaId);
			if (i != p->second.internalAreas.end()) {
				return 1;
			}
		} else {
			boost::unordered_map<int, Item::SharedArea>::iterator a = core->getData()->areas.find(areaId);
			if (a != core->getData()->areas.end()) {
				return Utility::isPointInArea(p->second.position, a->second);
			}
		}
	}
	return 0;
}

int IsPlayerInAnyDynamicArea(int id, bool recheck) {
	boost::unordered_map<int, Player>::iterator p = core->getData()->players.find(id);
	if (p != core->getData()->players.end()) {
		if (!recheck) {
			if (!p->second.internalAreas.empty()) {
				return 1;
			}
		} else {
			for (boost::unordered_map<int, Item::SharedArea>::const_iterator a = core->getData()->areas.begin(); a != core->getData()->areas.end(); ++a) {
				if (Utility::isPointInArea(p->second.position, a->second)) {
					return 1;
				}
			}
		}
	}
	return 0;
}

int IsAnyPlayerInDynamicArea(int id, bool recheck) {
	for (boost::unordered_map<int, Player>::iterator p = core->getData()->players.begin(); p != core->getData()->players.end(); ++p) {
		if (!recheck) {
			boost::unordered_set<int>::iterator i = p->second.internalAreas.find(id);
			if (i != p->second.internalAreas.end())
			{
				return 1;
			}
		} else {
			boost::unordered_map<int, Item::SharedArea>::iterator a = core->getData()->areas.find(id);
			if (a != core->getData()->areas.end()) {
				return Utility::isPointInArea(p->second.position, a->second);
			}
		}
	}
	return 0;
}

int IsAnyPlayerInAnyDynamicArea(bool recheck) {
	for (boost::unordered_map<int, Player>::iterator p = core->getData()->players.begin(); p != core->getData()->players.end(); ++p) {
		if (!recheck) {
			if (!p->second.internalAreas.empty())
			{
				return 1;
			}
		} else {
			for (boost::unordered_map<int, Item::SharedArea>::const_iterator a = core->getData()->areas.begin(); a != core->getData()->areas.end(); ++a) {
				if (Utility::isPointInArea(p->second.position, a->second)) {
					return 1;
				}
			}
		}
	}
	return 0;
}

int IsPointInDynamicArea(int id, float x, float y, float z) {
	boost::unordered_map<int, Item::SharedArea>::iterator a = core->getData()->areas.find(id);
	if (a != core->getData()->areas.end()) {
		return Utility::isPointInArea(Eigen::Vector3f(x, y, z), a->second);
	}
	return 0;
}

int IsPointInAnyDynamicArea(float x, float y, float z) {
	for (boost::unordered_map<int, Item::SharedArea>::const_iterator a = core->getData()->areas.begin(); a != core->getData()->areas.end(); ++a) {
		if (Utility::isPointInArea(Eigen::Vector3f(x, y, z), a->second)) {
			return 1;
		}
	}
	return 0;
}

int IsLineInDynamicArea(int id, float x, float y, float z, float x2, float y2, float z2) {
	boost::unordered_map<int, Item::SharedArea>::iterator a = core->getData()->areas.find(id);
	if (a != core->getData()->areas.end()) {
		return Utility::doesLineSegmentIntersectArea(Eigen::Vector3f(x, y, z), Eigen::Vector3f(x2, y2, z2), a->second);
	}
	return 0;
}

int IsLineInAnyDynamicArea(float x, float y, float z, float x2, float y2, float z2) {
	for (boost::unordered_map<int, Item::SharedArea>::const_iterator a = core->getData()->areas.begin(); a != core->getData()->areas.end(); ++a) {
		if (Utility::doesLineSegmentIntersectArea(Eigen::Vector3f(x, y, z), Eigen::Vector3f(x2, y2, z2), a->second)) {
			return 1;
		}
	}
	return 0;
}

std::vector<int> GetPlayerDynamicAreas(int id) {
	boost::unordered_map<int, Player>::iterator p = core->getData()->players.find(id);
	std::vector<int> finalAreas;
	if (p != core->getData()->players.end()) {
		std::multimap<float, int> orderedAreas;
		for (boost::unordered_set<int>::iterator i = p->second.internalAreas.begin(); i != p->second.internalAreas.end(); ++i) {
			boost::unordered_map<int, Item::SharedArea>::iterator a = core->getData()->areas.find(*i);
			if (a != core->getData()->areas.end()) {

				boost::variant<Polygon2d, Box2d, Box3d, Eigen::Vector2f, Eigen::Vector3f> position;
				if (a->second->attach) {
					position = a->second->position;
				} else {
					position = a->second->position;
				}

				float distance = 0.0f;
				switch (a->second->type)
				{
				case STREAMER_AREA_TYPE_CIRCLE:
				case STREAMER_AREA_TYPE_CYLINDER:
				{
					distance = static_cast<float>(boost::geometry::comparable_distance(Eigen::Vector2f(p->second.position[0], p->second.position[1]), boost::get<Eigen::Vector2f>(position)));
					break;
				}
				case STREAMER_AREA_TYPE_SPHERE:
				{
					distance = static_cast<float>(boost::geometry::comparable_distance(p->second.position, boost::get<Eigen::Vector3f>(position)));
					break;
				}
				case STREAMER_AREA_TYPE_RECTANGLE:
				{
					Eigen::Vector2f centroid = boost::geometry::return_centroid<Eigen::Vector2f>(boost::get<Box2d>(position));
					distance = static_cast<float>(boost::geometry::comparable_distance(Eigen::Vector2f(p->second.position[0], p->second.position[1]), centroid));
					break;
				}
				case STREAMER_AREA_TYPE_CUBOID:
				{
					Eigen::Vector3f centroid = boost::geometry::return_centroid<Eigen::Vector3f>(boost::get<Box3d>(position));
					distance = static_cast<float>(boost::geometry::comparable_distance(p->second.position, centroid));
					break;

				}
				case STREAMER_AREA_TYPE_POLYGON:
				{
					Eigen::Vector2f centroid = boost::geometry::return_centroid<Eigen::Vector2f>(boost::get<Polygon2d>(position));
					distance = static_cast<float>(boost::geometry::comparable_distance(Eigen::Vector2f(p->second.position[0], p->second.position[1]), centroid));
					break;
				}
				}
				orderedAreas.insert(std::pair<float, int>(distance, a->first));
			}
		}
		for (std::map<float, int>::iterator i = orderedAreas.begin(); i != orderedAreas.end(); ++i) {
			finalAreas.push_back(i->second);
		}
		return finalAreas;
	}
	return finalAreas;
}

int GetPlayerNumberDynamicAreas(int id) {
	boost::unordered_map<int, Player>::iterator p = core->getData()->players.find(id);
	if (p != core->getData()->players.end()) {
		return static_cast<cell>(p->second.internalAreas.size());
	}
	return 0;
}

std::vector<int> GetDynamicAreasForPoint(float x, float y, float z) {
	std::multimap<float, int> orderedAreas;
	std::vector<SharedCell> pointCells;
	std::vector<int> finalAreas;
	core->getGrid()->findMinimalCellsForPoint(Eigen::Vector2f(x, y), pointCells);
	for (std::vector<SharedCell>::const_iterator p = pointCells.begin(); p != pointCells.end(); ++p) {
		for (boost::unordered_map<int, Item::SharedArea>::const_iterator a = (*p)->areas.begin(); a != (*p)->areas.end(); ++a) {
			if (Utility::isPointInArea(Eigen::Vector3f(x, y, z), a->second)) {

				boost::variant<Polygon2d, Box2d, Box3d, Eigen::Vector2f, Eigen::Vector3f> position;
				if (a->second->attach) {
					position = a->second->position;
				} else {
					position = a->second->position;
				}

				float distance = 0.0f;
				switch (a->second->type)
				{
				case STREAMER_AREA_TYPE_CIRCLE:
				case STREAMER_AREA_TYPE_CYLINDER:
				{
					distance = static_cast<float>(boost::geometry::comparable_distance(Eigen::Vector2f(x, y), boost::get<Eigen::Vector2f>(position)));
					break;
				}
				case STREAMER_AREA_TYPE_SPHERE:
				{
					distance = static_cast<float>(boost::geometry::comparable_distance(Eigen::Vector3f(x, y, z), boost::get<Eigen::Vector3f>(position)));
					break;
				}
				case STREAMER_AREA_TYPE_RECTANGLE:
				{
					Eigen::Vector2f centroid = boost::geometry::return_centroid<Eigen::Vector2f>(boost::get<Box2d>(position));
					distance = static_cast<float>(boost::geometry::comparable_distance(Eigen::Vector2f(x, y), centroid));
					break;
				}
				case STREAMER_AREA_TYPE_CUBOID:
				{
					Eigen::Vector3f centroid = boost::geometry::return_centroid<Eigen::Vector3f>(boost::get<Box3d>(position));
					distance = static_cast<float>(boost::geometry::comparable_distance(Eigen::Vector3f(x, y, z), centroid));
					break;

				}
				case STREAMER_AREA_TYPE_POLYGON:
				{
					Eigen::Vector2f centroid = boost::geometry::return_centroid<Eigen::Vector2f>(boost::get<Polygon2d>(position));
					distance = static_cast<float>(boost::geometry::comparable_distance(Eigen::Vector2f(x, y), centroid));
					break;
				}
				}
				orderedAreas.insert(std::pair<float, int>(distance, a->first));
			}
		}
	}
	for (std::map<float, int>::iterator i = orderedAreas.begin(); i != orderedAreas.end(); ++i) {
		finalAreas.push_back(i->second);
	}
	return finalAreas;
}

int GetNumberDynamicAreasForPoint(float x, float y, float z) {
	int areaCount = 0;
	std::vector<SharedCell> pointCells;
	core->getGrid()->findMinimalCellsForPoint(Eigen::Vector2f(x, y), pointCells);
	for (std::vector<SharedCell>::const_iterator p = pointCells.begin(); p != pointCells.end(); ++p) {
		for (boost::unordered_map<int, Item::SharedArea>::const_iterator a = (*p)->areas.begin(); a != (*p)->areas.end(); ++a) {
			if (Utility::isPointInArea(Eigen::Vector3f(x, y, z), a->second)) {
				++areaCount;
			}
		}
	}
	return areaCount;
}

std::vector<int> GetDynamicAreasForLine(float x, float y, float z, float x2, float y2, float z2) {
	std::multimap<float, int> orderedAreas;
	for (boost::unordered_map<int, Item::SharedArea>::const_iterator a = core->getData()->areas.begin(); a != core->getData()->areas.end(); ++a) {
		if (Utility::doesLineSegmentIntersectArea(Eigen::Vector3f(x, y, z), Eigen::Vector3f(x2, y2, z2), a->second)) {

			boost::variant<Polygon2d, Box2d, Box3d, Eigen::Vector2f, Eigen::Vector3f> position;
			if (a->second->attach) {
				position = a->second->position;
			} else {
				position = a->second->position;
			}

			float distance = 0.0f;
			switch (a->second->type)
			{
			case STREAMER_AREA_TYPE_CIRCLE:
			case STREAMER_AREA_TYPE_CYLINDER:
			{
				distance = static_cast<float>(boost::geometry::comparable_distance(Eigen::Vector2f(x, y), boost::get<Eigen::Vector2f>(position)));
				break;
			}
			case STREAMER_AREA_TYPE_SPHERE:
			{
				distance = static_cast<float>(boost::geometry::comparable_distance(Eigen::Vector3f(x, y, z), boost::get<Eigen::Vector3f>(position)));
				break;
			}
			case STREAMER_AREA_TYPE_RECTANGLE:
			{
				Eigen::Vector2f centroid = boost::geometry::return_centroid<Eigen::Vector2f>(boost::get<Box2d>(position));
				distance = static_cast<float>(boost::geometry::comparable_distance(Eigen::Vector2f(x, y), centroid));
				break;
			}
			case STREAMER_AREA_TYPE_CUBOID:
			{
				Eigen::Vector3f centroid = boost::geometry::return_centroid<Eigen::Vector3f>(boost::get<Box3d>(position));
				distance = static_cast<float>(boost::geometry::comparable_distance(Eigen::Vector3f(x, y, z), centroid));
				break;

			}
			case STREAMER_AREA_TYPE_POLYGON:
			{
				Eigen::Vector2f centroid = boost::geometry::return_centroid<Eigen::Vector2f>(boost::get<Polygon2d>(position));
				distance = static_cast<float>(boost::geometry::comparable_distance(Eigen::Vector2f(x, y), centroid));
				break;
			}
			}
			orderedAreas.insert(std::pair<float, int>(distance, a->first));
		}
	}
	std::vector<int> finalAreas;
	for (std::map<float, int>::iterator i = orderedAreas.begin(); i != orderedAreas.end(); ++i) {
		finalAreas.push_back(i->second);
	}
	return finalAreas;
}

int GetNumberDynamicAreasForLine(float x, float y, float z, float x2, float y2, float z2) {
	int areaCount = 0;
	for (boost::unordered_map<int, Item::SharedArea>::const_iterator a = core->getData()->areas.begin(); a != core->getData()->areas.end(); ++a) {
		if (Utility::doesLineSegmentIntersectArea(Eigen::Vector3f(x, y, z), Eigen::Vector3f(x2, y2, z2), a->second)) {
			++areaCount;
		}
	}
	return static_cast<cell>(areaCount);
}

int AttachDynamicAreaToObject(int id, int objectid, int type, int playerid, float x, float y, float z) {
	boost::unordered_map<int, Item::SharedArea>::iterator a = core->getData()->areas.find(id);
	if (a != core->getData()->areas.end()) {
		/*if (a->second->type != STREAMER_AREA_TYPE_CIRCLE && a->second->type != STREAMER_AREA_TYPE_SPHERE) {
			Utility::logError("AttachDynamicAreaToObject: Only circles and spheres may be attached to objects");
			return 0;
		}*/

		if ((objectid != INVALID_OBJECT_ID && type != STREAMER_OBJECT_TYPE_DYNAMIC) || (objectid != INVALID_STREAMER_ID && type == STREAMER_OBJECT_TYPE_DYNAMIC)) {
			a->second->attach = boost::intrusive_ptr<Item::Area::Attach>(new Item::Area::Attach);
			a->second->attach->player = INVALID_OBJECT_ID;
			a->second->attach->vehicle = INVALID_OBJECT_ID;
			a->second->attach->position = a->second->position;
			a->second->attach->object = boost::make_tuple(objectid, type, playerid);
			a->second->attach->positionOffset = Eigen::Vector3f(x, y, z);
			core->getStreamer()->attachedAreas.insert(a->second);
		} else {
			if (a->second->attach) {
				if ((a->second->attach->object.get<0>() != INVALID_OBJECT_ID && a->second->attach->object.get<1>() != STREAMER_OBJECT_TYPE_DYNAMIC) || (a->second->attach->object.get<0>() != INVALID_STREAMER_ID && a->second->attach->object.get<1>() == STREAMER_OBJECT_TYPE_DYNAMIC)) {
					a->second->attach.reset();
					core->getStreamer()->attachedAreas.erase(a->second);
					core->getGrid()->removeArea(a->second, true);
				}
			}
		}
		return 1;
	}
	return 0;
}

int AttachDynamicAreaToPlayer(int id, int playerrid, float x, float y, float z) {
	boost::unordered_map<int, Item::SharedArea>::iterator a = core->getData()->areas.find(id);
	if (a != core->getData()->areas.end()) {
		/*if (a->second->type != STREAMER_AREA_TYPE_CIRCLE && a->second->type != STREAMER_AREA_TYPE_SPHERE) {
			Utility::logError("AttachDynamicAreaToPlayer: Only circles and spheres may be attached to players");
			return 0;
		}*/

		if (playerrid != INVALID_OBJECT_ID) {
			a->second->attach = boost::intrusive_ptr<Item::Area::Attach>(new Item::Area::Attach);
			a->second->attach->object = boost::make_tuple(INVALID_STREAMER_ID, STREAMER_OBJECT_TYPE_DYNAMIC, INVALID_PLAYER_ID);
			a->second->attach->vehicle = INVALID_OBJECT_ID;
			a->second->attach->position = a->second->position;
			a->second->attach->player = playerrid;
			a->second->attach->positionOffset = Eigen::Vector3f(x, y, z);
			core->getStreamer()->attachedAreas.insert(a->second);
		} else {
			if (a->second->attach) {
				if (a->second->attach->player != INVALID_OBJECT_ID) {
					a->second->attach.reset();
					core->getStreamer()->attachedAreas.erase(a->second);
					core->getGrid()->removeArea(a->second, true);
				}
			}
		}
		return 1;
	}
	return 0;
}

int AttachDynamicAreaToVehicle(int id, int vehicleid, float x, float y, float z) {
	boost::unordered_map<int, Item::SharedArea>::iterator a = core->getData()->areas.find(id);
	if (a != core->getData()->areas.end()) {
		/*if (a->second->type != STREAMER_AREA_TYPE_CIRCLE && a->second->type != STREAMER_AREA_TYPE_SPHERE) {
			Utility::logError("AttachDynamicAreaToVehicle: Only circles and spheres may be attached to vehicles");
			return 0;
		}*/

		if (vehicleid != INVALID_OBJECT_ID) {
			a->second->attach = boost::intrusive_ptr<Item::Area::Attach>(new Item::Area::Attach);
			a->second->attach->object = boost::make_tuple(INVALID_STREAMER_ID, STREAMER_OBJECT_TYPE_DYNAMIC, INVALID_PLAYER_ID);
			a->second->attach->player = INVALID_OBJECT_ID;
			a->second->attach->position = a->second->position;
			a->second->attach->vehicle = vehicleid;
			a->second->attach->positionOffset = a->second->attach->positionOffset = Eigen::Vector3f(x, y, z);
			core->getStreamer()->attachedAreas.insert(a->second);
		} else {
			if (a->second->attach) {
				if (a->second->attach->vehicle != INVALID_OBJECT_ID) {
					a->second->attach.reset();
					core->getStreamer()->attachedAreas.erase(a->second);
					core->getGrid()->removeArea(a->second, true);
				}
			}
		}
		return 1;
	}
	return 0;
}

int ToggleDynAreaSpectateMode(int areaid, int spectator) {
	boost::unordered_map<int, Item::SharedArea>::iterator a = core->getData()->areas.find(areaid);
	if (a != core->getData()->areas.end()) {
		a->second->spectateMode = spectator != 0;
		return 1;
	}
	return 0;
}

int IsToggleDynAreaSpectateMode(int areaid) {
	boost::unordered_map<int, Item::SharedArea>::iterator a = core->getData()->areas.find(areaid);
	if (a != core->getData()->areas.end()) {
		return static_cast<cell>(a->second->spectateMode != 0);
	}
	return 0;
}

STREAMER_END_NS


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

// Copyright (c) 2007-2012 Barend Gehrels, 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_GEOMETRY_POLICIES_RELATE_INTERSECTION_POINTS_HPP
#define BOOST_GEOMETRY_GEOMETRY_POLICIES_RELATE_INTERSECTION_POINTS_HPP

#include <algorithm>
#include <string>

#include <boost/concept_check.hpp>
#include <boost/numeric/conversion/cast.hpp>

#include <boost/geometry/algorithms/detail/assign_indexed_point.hpp>
#include <boost/geometry/core/access.hpp>
#include <boost/geometry/core/assert.hpp>
#include <boost/geometry/strategies/side_info.hpp>
#include <boost/geometry/util/promote_integral.hpp>
#include <boost/geometry/util/select_calculation_type.hpp>
#include <boost/geometry/util/select_most_precise.hpp>
#include <boost/geometry/util/math.hpp>

namespace geofeatures_boost {} namespace boost = geofeatures_boost; namespace geofeatures_boost { namespace geometry
{

namespace policies { namespace relate
{

/*!
\brief Policy calculating the intersection points themselves
 */
template
<
    typename ReturnType
>
struct segments_intersection_points
{
    typedef ReturnType return_type;

    template
    <
        typename Point,
        typename Segment,
        typename SegmentRatio,
        typename T
    >
    static inline void assign(Point& point,
                Segment const& segment,
                SegmentRatio const& ratio,
                T const& dx, T const& dy)
    {
        typedef typename geometry::coordinate_type<Point>::type coordinate_type;

        // Calculate the intersection point based on segment_ratio
        // Up to now, division was postponed. Here we divide using numerator/
        // denominator. In case of integer this results in an integer
        // division.
        BOOST_GEOMETRY_ASSERT(ratio.denominator() != 0);

        typedef typename promote_integral<coordinate_type>::type promoted_type;

        promoted_type const numerator
            = geofeatures_boost::numeric_cast<promoted_type>(ratio.numerator());
        promoted_type const denominator
            = geofeatures_boost::numeric_cast<promoted_type>(ratio.denominator());
        promoted_type const dx_promoted = geofeatures_boost::numeric_cast<promoted_type>(dx);
        promoted_type const dy_promoted = geofeatures_boost::numeric_cast<promoted_type>(dy);

        set<0>(point, get<0, 0>(segment) + geofeatures_boost::numeric_cast
            <
                coordinate_type
            >(numerator * dx_promoted / denominator));
        set<1>(point, get<0, 1>(segment) + geofeatures_boost::numeric_cast
            <
                coordinate_type
            >(numerator * dy_promoted / denominator));
    }

    template
    <
        typename Segment1,
        typename Segment2,
        typename SegmentIntersectionInfo
    >
    static inline return_type segments_crosses(side_info const&,
                    SegmentIntersectionInfo const& sinfo,
                    Segment1 const& s1, Segment2 const& s2)
    {
        return_type result;
        result.count = 1;

        bool use_a = true;

        // Prefer one segment if one is on or near an endpoint
        bool const a_near_end = sinfo.robust_ra.near_end();
        bool const b_near_end = sinfo.robust_rb.near_end();
        if (a_near_end && ! b_near_end)
        {
            use_a = true;
        }
        else if (b_near_end && ! a_near_end)
        {
            use_a = false;
        }
        else
        {
            // Prefer shorter segment
            typedef typename SegmentIntersectionInfo::promoted_type ptype;
            ptype const len_a = sinfo.dx_a * sinfo.dx_a + sinfo.dy_a * sinfo.dy_a;
            ptype const len_b = sinfo.dx_b * sinfo.dx_b + sinfo.dy_b * sinfo.dy_b;
            if (len_b < len_a)
            {
                use_a = false;
            }
            // else use_a is true but was already assigned like that
        }

        if (use_a)
        {
            assign(result.intersections[0], s1, sinfo.robust_ra,
                sinfo.dx_a, sinfo.dy_a);
        }
        else
        {
            assign(result.intersections[0], s2, sinfo.robust_rb,
                sinfo.dx_b, sinfo.dy_b);
        }

        result.fractions[0].assign(sinfo);

        return result;
    }

    template <typename Segment1, typename Segment2, typename Ratio>
    static inline return_type segments_collinear(
        Segment1 const& a, Segment2 const& b, bool /*opposite*/,
        int a1_wrt_b, int a2_wrt_b, int b1_wrt_a, int b2_wrt_a,
        Ratio const& ra_from_wrt_b, Ratio const& ra_to_wrt_b,
        Ratio const& rb_from_wrt_a, Ratio const& rb_to_wrt_a)
    {
        return_type result;
        unsigned int index = 0, count_a = 0, count_b = 0;
        Ratio on_a[2];

        // The conditions "index < 2" are necessary for non-robust handling,
        // if index would be 2 this indicate an (currently uncatched) error

        // IMPORTANT: the order of conditions is different as in direction.hpp
        if (a1_wrt_b >= 1 && a1_wrt_b <= 3 // ra_from_wrt_b.on_segment()
            && index < 2)
        {
            //     a1--------->a2
            // b1----->b2
            //
            // ra1 (relative to b) is between 0/1:
            // -> First point of A is intersection point
            detail::assign_point_from_index<0>(a, result.intersections[index]);
            result.fractions[index].assign(Ratio::zero(), ra_from_wrt_b);
            on_a[index] = Ratio::zero();
            index++;
            count_a++;
        }
        if (b1_wrt_a == 2 //rb_from_wrt_a.in_segment()
            && index < 2)
        {
            // We take the first intersection point of B
            // a1--------->a2
            //         b1----->b2
            // But only if it is not located on A
            // a1--------->a2
            // b1----->b2      rb_from_wrt_a == 0/1 -> a already taken

            detail::assign_point_from_index<0>(b, result.intersections[index]);
            result.fractions[index].assign(rb_from_wrt_a, Ratio::zero());
            on_a[index] = rb_from_wrt_a;
            index++;
            count_b++;
        }

        if (a2_wrt_b >= 1 && a2_wrt_b <= 3 //ra_to_wrt_b.on_segment()
            && index < 2)
        {
            // Similarly, second IP (here a2)
            // a1--------->a2
            //         b1----->b2
            detail::assign_point_from_index<1>(a, result.intersections[index]);
            result.fractions[index].assign(Ratio::one(), ra_to_wrt_b);
            on_a[index] = Ratio::one();
            index++;
            count_a++;
        }
        if (b2_wrt_a == 2 // rb_to_wrt_a.in_segment()
            && index < 2)
        {
            detail::assign_point_from_index<1>(b, result.intersections[index]);
            result.fractions[index].assign(rb_to_wrt_a, Ratio::one());
            on_a[index] = rb_to_wrt_a;
            index++;
            count_b++;
        }

        // TEMPORARY
        // If both are from b, and b is reversed w.r.t. a, we swap IP's
        // to align them w.r.t. a
        // get_turn_info still relies on some order (in some collinear cases)
        if (index == 2 && on_a[1] < on_a[0])
        {
            std::swap(result.fractions[0], result.fractions[1]);
            std::swap(result.intersections[0], result.intersections[1]);
        }

        result.count = index;

        return result;
    }

    static inline return_type disjoint()
    {
        return return_type();
    }
    static inline return_type error(std::string const&)
    {
        return return_type();
    }

    // Both degenerate
    template <typename Segment>
    static inline return_type degenerate(Segment const& segment, bool)
    {
        return_type result;
        result.count = 1;
        set<0>(result.intersections[0], get<0, 0>(segment));
        set<1>(result.intersections[0], get<0, 1>(segment));
        return result;
    }

    // One degenerate
    template <typename Segment, typename Ratio>
    static inline return_type one_degenerate(Segment const& degenerate_segment,
            Ratio const& ratio, bool a_degenerate)
    {
        return_type result;
        result.count = 1;
        set<0>(result.intersections[0], get<0, 0>(degenerate_segment));
        set<1>(result.intersections[0], get<0, 1>(degenerate_segment));
        if (a_degenerate)
        {
            // IP lies on ratio w.r.t. segment b
            result.fractions[0].assign(Ratio::zero(), ratio);
        }
        else
        {
            result.fractions[0].assign(ratio, Ratio::zero());
        }
        return result;
    }
};

}} // namespace policies::relate

}} // namespace geofeatures_boost::geometry

#endif // BOOST_GEOMETRY_GEOMETRY_POLICIES_RELATE_INTERSECTION_POINTS_HPP


// GMTL is (C) Copyright 2001-2010 by Allen Bierbaum
// Distributed under the GNU Lesser General Public License 2.1 with an
// addendum covering inlined code. (See accompanying files LICENSE and
// LICENSE.addendum or http://www.gnu.org/copyleft/lesser.txt)

// Includes ====================================================================
#include <boost/python.hpp>
#include <gmtl-Xforms.h>

// Using =======================================================================
using namespace boost::python;

// Module ======================================================================
void _Export_gmtl_Xforms_h()
{
    def("xform", (gmtl::LineSeg<float> & (*)(gmtl::LineSeg<float> &, const gmtl::Matrix<float,4,4> &, const gmtl::LineSeg<float> &))&gmtl::xform, return_internal_reference< 1 >());
    def("xform", (gmtl::LineSeg<double> & (*)(gmtl::LineSeg<double> &, const gmtl::Matrix<double,4,4> &, const gmtl::LineSeg<double> &))&gmtl::xform, return_internal_reference< 1 >());
    def("xform", (gmtl::Ray<float> & (*)(gmtl::Ray<float> &, const gmtl::Matrix<float,4,4> &, const gmtl::Ray<float> &))&gmtl::xform, return_internal_reference< 1 >());
    def("xform", (gmtl::Ray<double> & (*)(gmtl::Ray<double> &, const gmtl::Matrix<double,4,4> &, const gmtl::Ray<double> &))&gmtl::xform, return_internal_reference< 1 >());
    def("xform", (gmtl::Point<float,4> & (*)(gmtl::Point<float,4> &, const gmtl::Matrix<float,4,4> &, const gmtl::Point<float,4> &))&gmtl::xform, return_internal_reference< 1 >());
    def("xform", (gmtl::Point<float,3> & (*)(gmtl::Point<float,3> &, const gmtl::Matrix<float,4,4> &, const gmtl::Point<float,3> &))&gmtl::xform, return_internal_reference< 1 >());
    def("xform", (gmtl::Point<double,4> & (*)(gmtl::Point<double,4> &, const gmtl::Matrix<double,4,4> &, const gmtl::Point<double,4> &))&gmtl::xform, return_internal_reference< 1 >());
    def("xform", (gmtl::Point<float,4> & (*)(gmtl::Point<float,4> &, const gmtl::Matrix<float,4,4> &, const gmtl::Point<float,4> &))&gmtl::xform, return_internal_reference< 1 >());
    def("xform", (gmtl::Vec<double,4> & (*)(gmtl::Vec<double,4> &, const gmtl::Matrix<double,4,4> &, const gmtl::Vec<double,4> &))&gmtl::xform, return_internal_reference< 1 >());
    def("xform", (gmtl::Vec<double,3> & (*)(gmtl::Vec<double,3> &, const gmtl::Matrix<double,4,4> &, const gmtl::Vec<double,3> &))&gmtl::xform, return_internal_reference< 1 >());
    def("xform", (gmtl::Vec<float,4> & (*)(gmtl::Vec<float,4> &, const gmtl::Matrix<float,4,4> &, const gmtl::Vec<float,4> &))&gmtl::xform, return_internal_reference< 1 >());
    def("xform", (gmtl::Vec<float,3> & (*)(gmtl::Vec<float,3> &, const gmtl::Matrix<float,4,4> &, const gmtl::Vec<float,3> &))&gmtl::xform, return_internal_reference< 1 >());
    def("xform", (gmtl::Vec<double,4> & (*)(gmtl::Vec<double,4> &, const gmtl::Matrix<double,4,4> &, const gmtl::Vec<double,4> &))&gmtl::xform, return_internal_reference< 1 >());
    def("xform", (gmtl::Vec<float,4> & (*)(gmtl::Vec<float,4> &, const gmtl::Matrix<float,4,4> &, const gmtl::Vec<float,4> &))&gmtl::xform, return_internal_reference< 1 >());
    def("xform", (gmtl::Vec<double,3> & (*)(gmtl::Vec<double,3> &, const gmtl::Matrix<double,3,3> &, const gmtl::Vec<double,3> &))&gmtl::xform, return_internal_reference< 1 >());
    def("xform", (gmtl::Vec<float,3> & (*)(gmtl::Vec<float,3> &, const gmtl::Matrix<float,3,3> &, const gmtl::Vec<float,3> &))&gmtl::xform, return_internal_reference< 1 >());
    def("xform", (gmtl::Vec<double,2> & (*)(gmtl::Vec<double,2> &, const gmtl::Matrix<double,3,3> &, const gmtl::Vec<double,2> &))&gmtl::xform, return_internal_reference< 1 >());
    def("xform", (gmtl::Vec<float,2> & (*)(gmtl::Vec<float,2> &, const gmtl::Matrix<float,3,3> &, const gmtl::Vec<float,2> &))&gmtl::xform, return_internal_reference< 1 >());
    def("xform", (gmtl::VecBase<double,3> & (*)(gmtl::VecBase<double,3> &, const gmtl::Quat<double> &, const gmtl::VecBase<double,3> &))&gmtl::xform, return_internal_reference< 1 >());
    def("xform", (gmtl::Point<double,4> & (*)(gmtl::Point<double,4> &, const gmtl::Matrix<double,4,4> &, const gmtl::Point<double,4> &))&gmtl::xform, return_internal_reference< 1 >());
    def("xform", (gmtl::Point<double,3> & (*)(gmtl::Point<double,3> &, const gmtl::Matrix<double,4,4> &, const gmtl::Point<double,3> &))&gmtl::xform, return_internal_reference< 1 >());
    def("xform", (gmtl::Point<double,3> & (*)(gmtl::Point<double,3> &, const gmtl::Matrix<double,3,3> &, const gmtl::Point<double,3> &))&gmtl::xform, return_internal_reference< 1 >());
    def("xform", (gmtl::Point<float,3> & (*)(gmtl::Point<float,3> &, const gmtl::Matrix<float,3,3> &, const gmtl::Point<float,3> &))&gmtl::xform, return_internal_reference< 1 >());
    def("xform", (gmtl::VecBase<float,3> & (*)(gmtl::VecBase<float,3> &, const gmtl::Quat<float> &, const gmtl::VecBase<float,3> &))&gmtl::xform, return_internal_reference< 1 >());
    def("xform", (gmtl::Point<double,2> & (*)(gmtl::Point<double,2> &, const gmtl::Matrix<double,3,3> &, const gmtl::Point<double,2> &))&gmtl::xform, return_internal_reference< 1 >());
    def("xform", (gmtl::Point<float,2> & (*)(gmtl::Point<float,2> &, const gmtl::Matrix<float,3,3> &, const gmtl::Point<float,2> &))&gmtl::xform, return_internal_reference< 1 >());
}


// Copyright 2017 Apex.AI, Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#ifndef UTILITIES__CPU_USAGE_TRACKER_HPP_
#define UTILITIES__CPU_USAGE_TRACKER_HPP_

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

#if defined(QNX)
#include <sys/neutrino.h>
#include <sys/syspage.h>
#include <sys/types.h>
#include <chrono>
#endif  // defined(QNX)

#ifdef PERFORMANCE_TEST_ODB_FOR_SQL_ENABLED
#include <odb/core.hxx>
#endif

namespace performance_test
{

struct CpuInfo
{
  CpuInfo(uint32_t cpu_cores, float cpu_usage)
  : m_cpu_cores(cpu_cores), m_cpu_usage(cpu_usage) {}

#ifdef PERFORMANCE_TEST_ODB_FOR_SQL_ENABLED
  CpuInfo() {}
#endif

  uint32_t cpu_cores() const
  {
    return m_cpu_cores;
  }

  float cpu_usage() const
  {
    return m_cpu_usage;
  }

private:
#ifdef PERFORMANCE_TEST_ODB_FOR_SQL_ENABLED
  friend class odb::access;
#endif
#pragma db default(0)
  uint32_t m_cpu_cores;
#pragma db default(0.0)
  float m_cpu_usage;
};

///  Calculate the CPU usage for the running experiment in the performance test
class CPUsageTracker
{
protected:
  boost::timer::cpu_timer m_cpu_timer;

public:
  CPUsageTracker()
  : m_cpu_timer()
  {
#if defined(QNX)
    m_tot_cpu_cores = _syspage_ptr->num_cpu;
#endif
  }

  /**
    * \brief Computes the CPU usage % as (process_active_time/total_cpu_time)*100
    *
    */
  CpuInfo get_cpu_usage()
  {
#if defined(QNX)
    struct timespec cur_usage_st;
    float_t cpu_usage_local{};

    int64_t cur_time_ms =
      std::chrono::time_point_cast<std::chrono::milliseconds>(std::chrono::steady_clock::now()).
      time_since_epoch().count();

    if (-1 != clock_gettime(CLOCK_PROCESS_CPUTIME_ID, &cur_usage_st)) {
      int64_t cur_active_time =
        std::chrono::duration_cast<std::chrono::milliseconds>(
        std::chrono::seconds{cur_usage_st.tv_sec} +
        std::chrono::nanoseconds{cur_usage_st.tv_nsec}).count();

      int64_t active_time_ms = (cur_active_time - m_prev_active_time);

      int64_t total_time_ms = ((cur_time_ms - m_prev_total_time) *
        static_cast<int64_t>(m_tot_cpu_cores));

      cpu_usage_local = (static_cast<float_t>(active_time_ms) /
        static_cast<float_t>(total_time_ms)) * 100.0F;

      m_prev_active_time = cur_active_time;
      m_prev_total_time = cur_time_ms;

    } else {
      throw std::runtime_error("get_cpu_usage: Error getting process CPU usage time");
    }

    CpuInfo cpu_info_local{m_tot_cpu_cores, cpu_usage_local};
    return cpu_info_local;
#else
    auto times = m_cpu_timer.elapsed();
    m_cpu_timer.start();

    auto cpu_cores = std::thread::hardware_concurrency();
    return CpuInfo(
      cpu_cores,
      100.0F * static_cast<float>(times.user + times.system) /
      static_cast<float>(times.wall * cpu_cores)
    );
#endif  // defined(QNX)
  }
#if defined(QNX)

private:
  int64_t m_prev_active_time;
  int64_t m_prev_total_time;
  uint32_t m_tot_cpu_cores;
#endif  // defined(QNX)
};
}  // namespace performance_test

#endif  // UTILITIES__CPU_USAGE_TRACKER_HPP_


// c++
#include <cmath>

// boost
#include <boost/make_shared.hpp>

// ros
#include <angles/angles.h>
#include <visp_bridge/camera.h>

// visp
#include <visp/vpFeatureBuilder.h>

#include "bebop_vservo/bebop_vservo_ctrl.h"

namespace bebop_vservo
{

BebopVServoCtrl::BebopVServoCtrl(ros::NodeHandle &nh)
  : enabled_(false),
    servo_inited_(false),
    caminfo_recv_(false),
    force_reinit_(false),
    nh_(nh),
    nh_priv_("~"),
    sub_caminfo_(nh.subscribe("bebop/camera_info", 1, &BebopVServoCtrl::CameraCallback, this)),
    sub_cam_orientation_(nh_.subscribe("bebop/states/ARDrone3/CameraState/Orientation", 10
                                      , &BebopVServoCtrl::CameraOrientationCallback, this)),
    sub_bebop_att_(nh_.subscribe("bebop/states/ARDrone3/PilotingState/AttitudeChanged", 10,
                                 &BebopVServoCtrl::BebopAttitudeCallback, this)),
    sub_roi_(nh_.subscribe("target", 1, &BebopVServoCtrl::TargetCallback, this)),
    sub_enable_(nh_.subscribe("enable", 1, &BebopVServoCtrl::EnableCallback, this)),
    pub_cmd_vel_(nh_.advertise<geometry_msgs::Twist>("cmd_vel", 10)),
    pub_debug_(nh_.advertise<bebop_vservo::Debug>("debug", 30)),
    pub_land_(nh_.advertise<std_msgs::Empty>("land", 10)),
    target_recv_time_(ros::Time(0)),
    bebop_recv_time_(ros::Time(0)),
    fov_x_(0.0),
    fov_y_(0.0),
    cam_tilt_rad_(0.0),
    bebop_yaw_rad_(0.0),
    vp_task_ptr_()  // set to null
{
  UpdateParams();
  util::ResetCmdVel(msg_cmd_vel_);
}

void BebopVServoCtrl::UpdateParams()
{
  util::GetParam(nh_priv_, "start_enabled", enabled_, false);
  util::GetParam(nh_priv_, "servo_gain", vp_gain_, 0.4);
  util::GetParam(nh_priv_, "update_freq", param_update_freq_, 30.0);
  util::GetParam(nh_priv_, "cam_tilt", cam_tilt_rad_, 0.0);
  util::GetParam(nh_priv_, "land_on_small_error", param_land_on_small_error_, false);
  util::GetParam(nh_priv_, "error_v_threshold", param_error_v_threshold_, 0.1);
  util::GetParam(nh_priv_, "error_angle_threshold", param_error_angle_threshold_, 10.0);
  param_error_angle_threshold_ = angles::from_degrees(param_error_angle_threshold_);

  cam_tilt_rad_ = angles::from_degrees(cam_tilt_rad_);
}

void BebopVServoCtrl::EnableCallback(const std_msgs::BoolConstPtr &enable_msg_ptr)
{
  enabled_ = enable_msg_ptr->data;
  ROS_INFO_STREAM("[VSER] request: " << (enabled_ ? "enable" : "disable"));
}

void BebopVServoCtrl::TargetCallback(const TargetConstPtr target_msg_ptr)
{
  target_cptr_ = target_msg_ptr;
  force_reinit_ = static_cast<bool>(target_msg_ptr->reinit);
  target_recv_time_ = ros::Time::now();
}

void BebopVServoCtrl::CameraOrientationCallback(const bebop_msgs::Ardrone3CameraStateOrientationConstPtr& cam_ori_ptr)
{
  const double& new_tilt_angle_rad = -angles::from_degrees(cam_ori_ptr->tilt);
  if (fabs(cam_tilt_rad_ - new_tilt_angle_rad) > 0.01)
  {
    // TEMP
    ROS_DEBUG_STREAM("[VSER] Bebop camera's new tilt: " << -cam_ori_ptr->tilt);
  }
  cam_tilt_rad_ = new_tilt_angle_rad;
}

void BebopVServoCtrl::BebopAttitudeCallback(const bebop_msgs::Ardrone3PilotingStateAttitudeChangedConstPtr &att_ptr)
{
  bebop_yaw_rad_ = -att_ptr->yaw;
  bebop_recv_time_ = ros::Time::now();
}

void BebopVServoCtrl::CameraCallback(const sensor_msgs::CameraInfoConstPtr& cinfo_msg_ptr)
{
  if (!caminfo_recv_)
  {
    cam_model_.fromCameraInfo(cinfo_msg_ptr);
    fov_x_ = 2.0 * atan2(cam_model_.fullResolution().width / 2.0, cam_model_.fx());
    fov_y_ = 2.0 * atan2(cam_model_.fullResolution().height/ 2.0, cam_model_.fy());
    ROS_INFO_STREAM_ONCE("[VSER] FOV " << angles::to_degrees(fov_x_) << " " << angles::to_degrees(fov_y_));

    vp_cam_ = visp_bridge::toVispCameraParameters(*cinfo_msg_ptr);
    vp_cam_.printParameters();

    caminfo_recv_ = true;
    // Might be an overkill
    ROS_WARN("[VSER] Camera info received, shutting down the subscriber.");
    sub_caminfo_.shutdown();
  }

}

void BebopVServoCtrl::Reset()
{
  util::ResetCmdVel(msg_cmd_vel_);
  pub_cmd_vel_.publish(msg_cmd_vel_);
}

void BebopVServoCtrl::Spin()
{
  ROS_INFO("[VSER] Control loop started ...");

  ros::Rate loop_rate(param_update_freq_);
  while (ros::ok())
  {
    try
    {
      ros::spinOnce();
      if (!loop_rate.sleep())
      {
        throw std::runtime_error("[VSER] Missed target frequency");
      }

      if (!enabled_)
      {
        ROS_WARN_THROTTLE(10, "[VSER] Not enabled ...");
        continue;
      }
      if (!Update()) Reset();
    }
    catch (const vpException& e)
    {
      ROS_ERROR_STREAM("[VSER] Visp error " << e.what());
      Reset();
    }
    catch (const ros::Exception& e)
    {
      ROS_ERROR_STREAM("[VSER] ROS error " << e.what());
      Reset();
    }
    catch (const std::runtime_error& e)
    {
      ROS_ERROR_STREAM("[VSER] Runtime error " << e.what());
      Reset();
    }
  }
}

bool BebopVServoCtrl::Update()
{
  if (!caminfo_recv_)
  {
    ROS_WARN("[VSER] The camera info has not been received yet!");
    return false;
  }

  //bool is_valid_roi = (ros::Time::now() - target_recv_time_).toSec() < (5.0 / param_update_freq_);
  bool is_valid_roi = (ros::Time::now() - target_recv_time_).toSec() < 2.0;

  sensor_msgs::RegionOfInterest roi;
  if (is_valid_roi && target_cptr_) roi = target_cptr_->roi;
  if (roi.width < 5 || roi.height < 5 ||
      (roi.x_offset + roi.width >= cam_model_.fullResolution().width) ||
      (roi.y_offset + roi.height >= cam_model_.fullResolution().height))
  {
    ROS_WARN_STREAM_THROTTLE(1, "[VSER] roi " << roi.x_offset << " " << roi.y_offset << " " << roi.width << " " << roi.height);
    is_valid_roi = false;
  }

  // Experimental
  // Our base pattern is a square, rectangles may make the system unstable
  if (roi.width != roi.height)
  {
    int32_t roi_wh = std::max(roi.width, roi.height);
    roi.x_offset -= (roi_wh - roi.width) / 2;
    roi.y_offset -= (roi_wh - roi.height) / 2;
    roi.width = roi_wh;
    roi.height = roi_wh;

    roi.x_offset = util::clamp<int32_t>(roi.x_offset, 0, cam_model_.fullResolution().width - 1);
    roi.y_offset = util::clamp<int32_t>(roi.y_offset, 0, cam_model_.fullResolution().height - 1);
    const int32_t x2 = util::clamp<int32_t>(roi.x_offset + roi.width, 0, cam_model_.fullResolution().width - 1);
    const int32_t y2 = util::clamp<int32_t>(roi.y_offset + roi.height, 0, cam_model_.fullResolution().height - 1);
    roi.width = x2 - roi.x_offset;
    roi.height = y2 - roi.y_offset;
  }

  if (!is_valid_roi)
  {
    ROS_WARN_THROTTLE(1, "[VSER] ROI is too old or not valid. Disabling visual servo!");
    ROS_WARN_STREAM_THROTTLE(1, "[VSER] Latency " << (ros::Time::now() - target_recv_time_).toSec());

    // Experimental
    enabled_ = false;
    servo_inited_ = false;
    return false;
  }

  if (is_valid_roi && (!servo_inited_ || force_reinit_))
  {
    // This will re-set all internal params of the task
    vp_task_ptr_ = boost::make_shared<vpServo>();
    fp_.clear();
    fpd_.clear();
    fp_.resize(4);
    fpd_.resize(4);

    servo_inited_ = false;

    servo_desired_depth_ = target_cptr_->desired_depth;
    servo_desired_yaw_rad_ = target_cptr_->desired_yaw_rad;
    servo_target_height_ = target_cptr_->target_height_m;
    servo_target_width_ = target_cptr_->target_width_m;
    servo_target_dist_ground_ = target_cptr_->target_distance_ground;

    ROS_WARN_STREAM("[VSER] (re-)init " <<
                    " is_valid_roi: " << (is_valid_roi ? "yes" : "no") <<
                    " re-init request: " << (force_reinit_ ? "yes" : "no") <<
                    " Desired depth: " << servo_desired_depth_ <<
                    " Desired yaw: " << angles::to_degrees(servo_desired_yaw_rad_) <<
                    " Target height: " << servo_target_height_ <<
                    " Target width: " << servo_target_width_ <<
                    " Target d2 ground: " << servo_target_dist_ground_
                    );

    ROS_WARN_STREAM("[VSER] Target ROI [x,y,w,h]: " << roi.x_offset << " " << roi.y_offset << " " << roi.width << " " << roi.height);

    //lambda_adapt.initStandard(4.0, 0.4, 40.0);

    ROS_ASSERT(fp_.size() == 4);
    vpFeatureBuilder::create(fp_[0], vp_cam_, vpImagePoint(roi.y_offset, roi.x_offset));
    vpFeatureBuilder::create(fp_[1], vp_cam_, vpImagePoint(roi.y_offset, roi.x_offset + roi.width));
    vpFeatureBuilder::create(fp_[2], vp_cam_, vpImagePoint(roi.y_offset + roi.height, roi.x_offset + roi.width));
    vpFeatureBuilder::create(fp_[3], vp_cam_, vpImagePoint(roi.y_offset + roi.height, roi.x_offset));

    vpPoint point[4];
    const double h2 = servo_target_height_ / 2.0;
    const double w2 = servo_target_width_ / 2.0;
    point[0].setWorldCoordinates(-w2, -h2, 0);
    point[1].setWorldCoordinates( w2, -h2, 0);
    point[2].setWorldCoordinates( w2,  h2, 0);
    point[3].setWorldCoordinates(-w2,  h2, 0);

//    point[0].setWorldCoordinates(-h2, -w2, 0);
//    point[1].setWorldCoordinates( h2, -w2, 0);
//    point[2].setWorldCoordinates( h2,  w2, 0);
//    point[3].setWorldCoordinates(-h2,  w2, 0);

    vpHomogeneousMatrix cMo;
    vpTranslationVector cto(0, 0, servo_desired_depth_);
    vpRxyzVector cro(vpMath::rad(0.0), vpMath::rad(0.0), vpMath::rad(0.0));
    vpRotationMatrix cRo(cro);
    cMo.buildFrom(cto, cRo);

    for (uint32_t i = 0; i < 4; i++)
    {
      vpColVector cP, p_img;
      point[i].changeFrame(cMo, cP);
      point[i].projection(cP, p_img);

      fpd_[i].set_xyZ(p_img[0], p_img[1], cP[2]);
    }

    ROS_ASSERT(vp_task_ptr_);
    ROS_ASSERT((fp_.size() == 4) && (fpd_.size() == 4));

    for (uint32_t i = 0; i < 4; i++)
    {
      vp_task_ptr_->addFeature(fp_[i], fpd_[i]);
    }

    vp_task_ptr_->setServo(vpServo::EYEINHAND_CAMERA);
    vp_task_ptr_->setInteractionMatrixType(vpServo::MEAN, vpServo::PSEUDO_INVERSE);
    vp_task_ptr_->setLambda(vp_gain_);
    vp_task_ptr_->print();
    servo_inited_ = true;
    force_reinit_ = false;
  }

  if (!servo_inited_ || !vp_task_ptr_)
  {
    ROS_WARN_THROTTLE(1, "[VSER] Servo task has not been initialized yet.");
    return false;
  }

  const double im_width_px = static_cast<double>(cam_model_.fullResolution().width);
  const double im_height_px = static_cast<double>(cam_model_.fullResolution().height);
  double sigma_1 = static_cast<double>(roi.y_offset) / im_height_px * fov_y_;
  double sigma_2 = (im_height_px - static_cast<double>(roi.y_offset + roi.height)) / im_height_px * fov_y_;
  double roi_height_px = roi.height;
  double beta_rad = (roi_height_px / im_height_px) * fov_y_;
  double d1_m = servo_target_height_ * sin(M_PI_2 - fov_y_/2.0 + sigma_2 - cam_tilt_rad_) / sin(beta_rad);
  double z1_m = d1_m * sin(M_PI_2 + fov_y_/2.0  - cam_tilt_rad_ - sigma_1);

  ROS_ASSERT(fp_.size() == 4);

  vpFeatureBuilder::create(fp_[0], vp_cam_, vpImagePoint(roi.y_offset, roi.x_offset));
  vpFeatureBuilder::create(fp_[1], vp_cam_, vpImagePoint(roi.y_offset, roi.x_offset + roi.width));
  vpFeatureBuilder::create(fp_[2], vp_cam_, vpImagePoint(roi.y_offset + roi.height, roi.x_offset + roi.width));
  vpFeatureBuilder::create(fp_[3], vp_cam_, vpImagePoint(roi.y_offset + roi.height, roi.x_offset));

  for (uint32_t i = 0; i < 4; i++)
  {
    fp_[i].set_Z(z1_m);
  }
  vp_v_ = vp_task_ptr_->computeControlLaw();

  //vp_task_.print();
  ROS_DEBUG_STREAM("V_SERVO: " << vp_v_.transpose());

  /* EXPERIMENTAL */
//  ROS_ERROR_STREAM("[VSER] delay: " << (ros::Time::now() - bebop_recv_time_).toSec());
//  ROS_ERROR_STREAM("[VSER] current yaw err (deg): " << angles::to_degrees(angles::normalize_angle(servo_desired_yaw_rad_ - bebop_yaw_rad_)));

  const double yaw_image_err_raw =
      ((cam_model_.fullResolution().width/2.0 - (roi.x_offset + roi.width / 2.0)) / (double) cam_model_.fullResolution().width) * fov_x_;

  vp_v_[0] = 0.0;
  servo_desired_yaw_rad_ = vp_gain_ * yaw_image_err_raw;
  /* END */

  // Here we should convert from Camera (Visp) coordinates to Bebop Coordinates
  // TODO: Consider camera tilt
  // Servo:           z: forward, x: right, y: down
  // Maps to bebop:   z: x_b    , x: -y_b , y: -z_b

  util::ResetCmdVel(msg_cmd_vel_);

  // No clamp/filter here
  // Map to Bebop's frame from camera's frame

  const double& vx_cam = vp_v_[2];
  const double& vz_cam = -vp_v_[1];

  const bool do_land = param_land_on_small_error_ &&
      (fabs(vx_cam) < param_error_v_threshold_) &&
      (fabs(vz_cam) < param_error_v_threshold_) &&
      (fabs(yaw_image_err_raw) < param_error_angle_threshold_);

  // TODO: Check the freshness of tilt!
  msg_cmd_vel_.linear.x =  vx_cam * cos(cam_tilt_rad_) + vz_cam * sin(cam_tilt_rad_);
  msg_cmd_vel_.linear.z = -vx_cam * sin(cam_tilt_rad_) + vz_cam * cos(cam_tilt_rad_);
  msg_cmd_vel_.linear.y = -vp_v_[0];

//  msg_cmd_vel_.linear.x = vp_v_[2];
//  msg_cmd_vel_.linear.y = -vp_v_[0];
//  msg_cmd_vel_.linear.z = -vp_v_[1];

  // We can't control all four DOF of the vehicle
  msg_cmd_vel_.angular.z = servo_desired_yaw_rad_;

  if (do_land)
  {
    ROS_WARN("[VSER] land on small error has been activated");
    std_msgs::Empty empty_msg;
    util::ResetCmdVel(msg_cmd_vel_);
    pub_land_.publish(empty_msg);
  }

  pub_cmd_vel_.publish(msg_cmd_vel_);

  msg_debug_.header.stamp = ros::Time::now();
  msg_debug_.header.frame_id = "";
  msg_debug_.bb_height = roi_height_px;
  //msg_debug_.d_groundtruth = d_truth;
  msg_debug_.cam_tilt = angles::to_degrees(cam_tilt_rad_);
  msg_debug_.beta = beta_rad;
  msg_debug_.sigma_1 = sigma_1;
  msg_debug_.sigma_2 = sigma_2;
  msg_debug_.d_raw_bb = z1_m;
  msg_debug_.target_height = servo_target_height_;
  msg_debug_.target_grounddist = servo_target_dist_ground_;
  for (uint32_t i = 0; i < 6; ++i)
    msg_debug_.v_img[i] = vp_v_.data[i];

  msg_debug_.desired_depth = servo_desired_depth_;
  msg_debug_.desired_yaw_rad = servo_desired_yaw_rad_;
  msg_debug_.target_roi = roi;

  pub_debug_.publish(msg_debug_);
  return true;
}

}  // namespace bebop_servo


#ifndef MJOLNIR_TEST_OMP_CHECK_CONSISTENCY_HPP
#define MJOLNIR_TEST_OMP_CHECK_CONSISTENCY_HPP
#include <mjolnir/math/Vector.hpp>
#include <mjolnir/core/System.hpp>

#include <test/util/clear_system.hpp>

#ifdef BOOST_TEST_DYN_LINK
#include <boost/test/unit_test.hpp>
#else
#include <boost/test/included/unit_test.hpp>
#endif

namespace mjolnir
{
namespace test
{

template<typename SequentialTrait, typename SequentialInteraction,
         typename ParallelTrait,   typename ParallelInteraction>
void check_force_consistency(System<SequentialTrait>      seq_sys,
                             const SequentialInteraction& seq_interaction,
                             System<ParallelTrait>        par_sys,
                             const ParallelInteraction    par_interaction,
                             const typename SequentialTrait::real_type tol)
{
    BOOST_TEST_REQUIRE(seq_sys.size() == par_sys.size());

    clear_force(seq_sys);
    seq_sys.preprocess_forces();
    seq_interaction.calc_force(seq_sys);
    seq_sys.postprocess_forces();

    clear_force(par_sys);
    par_sys.preprocess_forces();
    par_interaction.calc_force(par_sys);
    par_sys.postprocess_forces();

    // check the values are the same
    for(std::size_t i=0; i<seq_sys.size(); ++i)
    {
        BOOST_TEST(mjolnir::math::X(seq_sys.force(i)) == mjolnir::math::X(par_sys.force(i)),
                   boost::test_tools::tolerance(tol));
        BOOST_TEST(mjolnir::math::Y(seq_sys.force(i)) == mjolnir::math::Y(par_sys.force(i)),
                   boost::test_tools::tolerance(tol));
        BOOST_TEST(mjolnir::math::Z(seq_sys.force(i)) == mjolnir::math::Z(par_sys.force(i)),
                   boost::test_tools::tolerance(tol));
    }
    // check the virials are the same
    for(std::size_t i=0; i<9; ++i)
    {
        BOOST_TEST(seq_sys.virial()[i] == par_sys.virial()[i], boost::test_tools::tolerance(tol));
    }

    return;
}

template<typename SequentialTrait, typename SequentialInteraction,
         typename ParallelTrait,   typename ParallelInteraction>
void check_force_and_virial_consistency(
        System<SequentialTrait>      seq_sys,
        const SequentialInteraction& seq_interaction,
        System<ParallelTrait>        par_sys,
        const ParallelInteraction    par_interaction,
        const typename SequentialTrait::real_type tol)
{
    BOOST_TEST_REQUIRE(seq_sys.size() == par_sys.size());

    clear_force(seq_sys);
    seq_sys.preprocess_forces();
    seq_interaction.calc_force_and_virial(seq_sys);
    seq_sys.postprocess_forces();

    clear_force(par_sys);
    par_sys.preprocess_forces();
    par_interaction.calc_force_and_virial(par_sys);
    par_sys.postprocess_forces();

    // check the values are the same
    for(std::size_t i=0; i<seq_sys.size(); ++i)
    {
        BOOST_TEST(mjolnir::math::X(seq_sys.force(i)) == mjolnir::math::X(par_sys.force(i)),
                   boost::test_tools::tolerance(tol));
        BOOST_TEST(mjolnir::math::Y(seq_sys.force(i)) == mjolnir::math::Y(par_sys.force(i)),
                   boost::test_tools::tolerance(tol));
        BOOST_TEST(mjolnir::math::Z(seq_sys.force(i)) == mjolnir::math::Z(par_sys.force(i)),
                   boost::test_tools::tolerance(tol));
    }
    // check the virials are the same
    for(std::size_t i=0; i<9; ++i)
    {
        BOOST_TEST(seq_sys.virial()[i] == par_sys.virial()[i], boost::test_tools::tolerance(tol));
    }

    return;
}

template<typename SequentialTrait, typename SequentialInteraction,
         typename ParallelTrait,   typename ParallelInteraction>
void check_force_and_energy_consistency(
        System<SequentialTrait>      seq_sys,
        const SequentialInteraction& seq_interaction,
        System<ParallelTrait>        par_sys,
        const ParallelInteraction    par_interaction,
        const typename SequentialTrait::real_type tol)
{
    BOOST_TEST_REQUIRE(seq_sys.size() == par_sys.size());

    clear_force(seq_sys);
    seq_sys.preprocess_forces();
    const auto seq_ene = seq_interaction.calc_force_and_energy(seq_sys);
    seq_sys.postprocess_forces();

    clear_force(par_sys);
    par_sys.preprocess_forces();
    const auto par_ene = par_interaction.calc_force_and_energy(par_sys);
    par_sys.postprocess_forces();

    BOOST_TEST(seq_ene == par_ene, boost::test_tools::tolerance(tol));

    // check the values are the same
    for(std::size_t i=0; i<seq_sys.size(); ++i)
    {
        BOOST_TEST(mjolnir::math::X(seq_sys.force(i)) == mjolnir::math::X(par_sys.force(i)),
                   boost::test_tools::tolerance(tol));
        BOOST_TEST(mjolnir::math::Y(seq_sys.force(i)) == mjolnir::math::Y(par_sys.force(i)),
                   boost::test_tools::tolerance(tol));
        BOOST_TEST(mjolnir::math::Z(seq_sys.force(i)) == mjolnir::math::Z(par_sys.force(i)),
                   boost::test_tools::tolerance(tol));
    }
    // check the virials are the same
    for(std::size_t i=0; i<9; ++i)
    {
        BOOST_TEST(seq_sys.virial()[i] == par_sys.virial()[i], boost::test_tools::tolerance(tol));
    }

    return;
}

template<typename SequentialTrait, typename SequentialInteraction,
         typename ParallelTrait,   typename ParallelInteraction>
void check_force_energy_virial_consistency(
        System<SequentialTrait>      seq_sys,
        const SequentialInteraction& seq_interaction,
        System<ParallelTrait>        par_sys,
        const ParallelInteraction    par_interaction,
        const typename SequentialTrait::real_type tol)
{
    clear_force(seq_sys);
    seq_sys.preprocess_forces();
    const auto seq_ene = seq_interaction.calc_force_virial_energy(seq_sys);
    seq_sys.postprocess_forces();

    clear_force(par_sys);
    par_sys.preprocess_forces();
    const auto par_ene = par_interaction.calc_force_virial_energy(par_sys);
    par_sys.postprocess_forces();

    BOOST_TEST(seq_ene == par_ene, boost::test_tools::tolerance(tol));

    // check the values are the same
    for(std::size_t i=0; i<seq_sys.size(); ++i)
    {
        BOOST_TEST(mjolnir::math::X(seq_sys.force(i)) == mjolnir::math::X(par_sys.force(i)),
                   boost::test_tools::tolerance(tol));
        BOOST_TEST(mjolnir::math::Y(seq_sys.force(i)) == mjolnir::math::Y(par_sys.force(i)),
                   boost::test_tools::tolerance(tol));
        BOOST_TEST(mjolnir::math::Z(seq_sys.force(i)) == mjolnir::math::Z(par_sys.force(i)),
                   boost::test_tools::tolerance(tol));
    }
    // check the virials are the same
    for(std::size_t i=0; i<9; ++i)
    {
        BOOST_TEST(seq_sys.virial()[i] == par_sys.virial()[i], boost::test_tools::tolerance(tol));
    }

    return;
}

template<typename SequentialTrait, typename SequentialInteraction,
         typename ParallelTrait,   typename ParallelInteraction>
void check_energy_consistency(
        const System<SequentialTrait>& seq_sys,
        const SequentialInteraction&   seq_interaction,
        const System<ParallelTrait>&   par_sys,
        const ParallelInteraction      par_interaction,
        const typename SequentialTrait::real_type tol)
{
    const auto seq_ene = seq_interaction.calc_energy(seq_sys);
    const auto par_ene = par_interaction.calc_energy(par_sys);

    BOOST_TEST(seq_ene == par_ene, boost::test_tools::tolerance(tol));

    return;
}

} // test
} // mjolnir
#endif


//
// Created by lukas on 10.07.18.
//

#include "ebilp.hpp"

#include "../instance/instance.hpp" // IWYU pragma: keep
#include "../instance/laggraph.hpp" // IWYU pragma: keep
#include "../util/solverconfig.hpp" // IWYU pragma: keep
#include "algorithms/graphalgos.hpp"
#include "contrib/ilpabstraction/src/common.hpp" // for VariableType, Varia...
#include "datastructures/maybe.hpp"              // for Maybe
#include "ilp/ilp.hpp"                           // for ILPBase
#include "instance/job.hpp"                      // for Job

#include <boost/log/core/record.hpp>            // for record
#include <boost/log/sources/record_ostream.hpp> // for BOOST_LOG
#include <iostream>                             // for operator<<, cout
#include <math.h>                               // for round
#include <stddef.h>                             // for size_t
#include <unordered_map>

class AdditionalResultStorage;
class Instance;
namespace ilpabstraction {
class GurobiInterface;
}

template <class SolverT>
EBILP<SolverT>::EBILP(const Instance & instance_in,
                      AdditionalResultStorage & additional_in,
                      const SolverConfig & sconf_in)
    : ILPBase<SolverT>(instance_in, additional_in, sconf_in), l("EBILP"),
      start_point_mode(false), enforce_end_after_start_via_sum(true)
{
	if (this->sconf.has_config("use_start_points")) {
		this->start_point_mode = (bool)this->sconf["use_start_points"];
	}
	if (this->sconf.has_config("enforce_order_by_sum")) {
		this->enforce_end_after_start_via_sum =
		    (bool)this->sconf["enforce_order_by_sum"];
	}
}

template <class SolverT>
std::string
EBILP<SolverT>::get_id()
{
	return "EBILP v0.1 (" + std::string(SolverT::NAME) + ")";
}

template <class SolverT>
void
EBILP<SolverT>::compute_skip_numbers() noexcept
{
	NecessaryOrderComputer noc(this->instance);
	auto predecessors = noc.get_predecessor_count();
	auto successors = noc.get_successor_count();

	this->max_deadline = 0;

	this->skip_numbers.resize(this->instance.job_count());
	this->event_counts.resize(this->instance.job_count());
	for (unsigned int jid = 0; jid < this->instance.job_count(); ++jid) {
		// Every predecessor must have start and end strictly before us,
		// vice versa for successors.
		this->skip_numbers[jid] = {2 * predecessors[jid], 2 * successors[jid]};
		this->event_counts[jid] = 2 * this->instance.job_count() -
		                          2 * predecessors[jid] - 2 * successors[jid];
		const auto & job = this->instance.get_job(jid);
		this->max_deadline = std::max(this->max_deadline, job.get_deadline());
	}
}

template <class SolverT>
void
EBILP<SolverT>::prepare_variables()
{
	if (this->start_point_mode) {
		this->generate_vars_start_points();
	}

	size_t event_count = 2 * this->instance.job_count();

	this->job_start_events.resize(this->instance.job_count());
	this->job_end_events.resize(this->instance.job_count());
	for (unsigned int jid = 0; jid < this->instance.job_count(); ++jid) {
		size_t necessary_events = this->event_counts[jid];
		for (size_t eid = 0; eid < necessary_events; ++eid) {
			assert(this->skip_numbers[jid].first + eid < event_count);
			this->job_start_events[jid].push_back(this->model.add_var(
			    ilpabstraction::VariableType::BINARY, 0, 1,
			    std::to_string(jid) + std::string("_starts_at_") +
			        std::to_string(eid + this->skip_numbers[jid].first)));
			this->job_end_events[jid].push_back(this->model.add_var(
			    ilpabstraction::VariableType::BINARY, 0, 1,
			    std::to_string(jid) + std::string("_ends_at_") +
			        std::to_string(eid + this->skip_numbers[jid].first)));
		}
	}

	for (size_t eid = 0; eid < event_count; ++eid) {
		this->event_times.push_back(this->model.add_var(
		    ilpabstraction::VariableType::CONTINUOUS, 0, SolverT::INFTY,
		    std::string("event_") + std::to_string(eid) + "_time"));
	}

	/*
	        for (unsigned int rid = 0 ; rid < this->instance.resource_count() ;
	   ++rid) { this->usages_after_event.emplace_back(); for (unsigned int eid =
	   0; eid < event_count; ++eid) { this->usages_after_event[rid].push_back(
	                                                        this->model.add_var(ilpabstraction::VariableType::CONTINUOUS,
	   0, SolverT::INFTY, std::string("usage_rid_") + std::to_string(rid)
	                                                                               + std::string("_after_") + std::to_string(eid)));
	                }
	        }
	 */
}

template <class SolverT>
void
EBILP<SolverT>::prepare_time_constraints()
{
	/*
	for (unsigned int jid = 0 ; jid < this->instance.job_count() ; ++jid) {
	        const Job & job = this->instance.get_job(jid);

	        this->model.add_constraint(job.get_duration(),
	                                  this->end_points[jid] -
	this->start_points[jid], SolverT::INFTY,
	std::string("event_separation_for_jid_")
	                                  + std::to_string(jid));
	}
	 */

	for (unsigned int jid = 0; jid < this->instance.job_count(); ++jid) {
		const Job & job = this->instance.get_job(jid);

		if (this->enforce_end_after_start_via_sum) {
			Expression start_sum;
			Expression end_sum;
			for (unsigned int ev = 0; ev < this->event_counts[jid]; ++ev) {
				start_sum += (ev * this->job_start_events[jid][ev]);
				end_sum += (ev * this->job_end_events[jid][ev]);
			}

			this->model.add_constraint(start_sum + 1, end_sum, SolverT::INFTY,
			                           "enforce_start_end_order_" +
			                               std::to_string(jid));
		}

		for (unsigned int start_ev = 0; start_ev < this->event_counts[jid];
		     ++start_ev) {
			for (unsigned int end_ev = 0; end_ev < this->event_counts[jid];
			     ++end_ev) {
				if (end_ev <= start_ev) {
					// This may never happen
					// If we didn't use the sum trick above, we have to manually forbid
					// this
					if (!this->enforce_end_after_start_via_sum) {
						this->model.add_constraint(
						    0,
						    this->job_start_events[jid][start_ev] +
						        this->job_end_events[jid][end_ev],
						    1,
						    "forbid_jid_" + std::to_string(jid) + std::string("_end_at_") +
						        std::to_string(end_ev + this->skip_numbers[jid].first) +
						        std::string("_before_") +
						        std::to_string(start_ev + this->skip_numbers[jid].first));
					}
				} else {
					// If both are one, the duration between both events must be long
					// enough
					this->model.add_constraint(
					    ((this->job_start_events[jid][start_ev] +
					      this->job_end_events[jid][end_ev] - 1) *
					     job.get_duration()),
					    this->event_times[end_ev + this->skip_numbers[jid].first] -
					        this->event_times[start_ev + this->skip_numbers[jid].first],
					    SolverT::INFTY,
					    std::string("duration_") + std::to_string(jid) +
					        std::string("_between_") +
					        std::to_string(start_ev + this->skip_numbers[jid].first) +
					        std::string("_and_") +
					        std::to_string(end_ev + this->skip_numbers[jid].first));
				}
			}
		}
	}
}

template <class SolverT>
void
EBILP<SolverT>::prepare_start_points_constraints()
{
	for (unsigned int jid = 0; jid < this->instance.job_count(); ++jid) {
		for (unsigned int eid = 0; eid < this->event_counts[jid]; ++eid) {
			this->model.add_constraint(
			    this->event_times[eid + this->skip_numbers[jid].first] -
			        ((1 - this->job_start_events[jid][eid]) * this->latest_deadline),
			    this->start_points[jid],
			    this->event_times[eid + this->skip_numbers[jid].first] +
			        ((1 - this->job_start_events[jid][eid]) * this->latest_deadline));
		}
	}
}

template <class SolverT>
void
EBILP<SolverT>::prepare_dependency_constraints()
{
	for (auto & edge : this->instance.get_laggraph().edges()) {
		const Job & s_job = this->instance.get_job(edge.s);
		const Job & t_job = this->instance.get_job(edge.t);

		for (unsigned int start_ev = 0; start_ev < this->event_counts[edge.s];
		     ++start_ev) {
			for (unsigned int end_ev = 0; end_ev < this->event_counts[edge.t];
			     ++end_ev) {

				// this->latest_deadline is being used as big-M here
				this->model.add_constraint(
				    (((this->job_start_events[s_job.get_jid()][start_ev] +
				       this->job_start_events[t_job.get_jid()][end_ev] - 1) *
				      (edge.lag + static_cast<int>(this->latest_deadline))) -
				     static_cast<int>(this->latest_deadline)),
				    this->event_times[end_ev + this->skip_numbers[edge.t].first] -
				        this->event_times[start_ev + this->skip_numbers[edge.s].first],
				    SolverT::INFTY,
				    std::string("dependency_from_") + std::to_string(s_job.get_jid()) +
				        std::string("_to_") + std::to_string(t_job.get_jid()) +
				        std::string("_between_") +
				        std::to_string(start_ev + this->skip_numbers[edge.s].first) +
				        std::string("_and_") +
				        std::to_string(end_ev + this->skip_numbers[edge.t].first));
			}
		}
	}
}

template <class SolverT>
void
EBILP<SolverT>::create_solution()
{
	this->computed_solution_start_times = std::vector<Maybe<unsigned int>>(
	    this->instance.job_count(), Maybe<unsigned int>());

	if (!this->model.has_feasible()) {
		BOOST_LOG(l.w()) << "ILP has found no feasible solution.";
		return;
	}

	for (unsigned int jid = 0; jid < this->instance.job_count(); ++jid) {
		for (unsigned int eid = 0; eid < this->event_counts[jid]; ++eid) {
			if (this->model.get_variable_assignment(
			        this->job_start_events[jid][eid]) >= 0.8) {
				/*				std::cout << "Job " << jid << " starts at event "
				          << eid + this->skip_numbers[jid].first << " at time "
				          << this->model.get_variable_assignment(
				                 this->event_times[eid + this->skip_numbers[jid].first])
				                 << "\n"; */
				this->computed_solution_start_times[jid] =
				    (unsigned int)std::round(this->model.get_variable_assignment(
				        this->event_times[eid + this->skip_numbers[jid].first]));
			}
		}
	}
}

template <class SolverT>
void
EBILP<SolverT>::prepare_release_deadline_constraints()
{
	for (unsigned int jid = 0; jid < this->instance.job_count(); ++jid) {
		const Job & job = this->instance.get_job(jid);

		for (unsigned int eid = 0; eid < this->event_counts[jid]; ++eid) {
			this->model.add_constraint(
			    SolverT::NEGATIVE_INFTY,
			    this->event_times[eid + this->skip_numbers[jid].first] -
			        ((1 - this->job_end_events[jid][eid]) * this->latest_deadline),
			    job.get_deadline(),
			    std::string("release_") + std::to_string(jid) + std::string("_at_") +
			        std::to_string(eid + this->skip_numbers[jid].first));
			this->model.add_constraint(
			    job.get_release(),
			    this->event_times[eid + this->skip_numbers[jid].first] +
			        ((1 - this->job_start_events[jid][eid]) * this->latest_deadline),
			    SolverT::INFTY,
			    std::string("deadline_") + std::to_string(jid) + std::string("_at_") +
			        std::to_string(eid + this->skip_numbers[jid].first));
		}
	}
}

template <class SolverT>
void
EBILP<SolverT>::prepare_duration_constraint()
{
	for (unsigned int jid = 0; jid < this->instance.job_count(); ++jid) {
		const Job & job = this->instance.get_job(jid);

		this->model.add_constraint(
		    job.get_duration(), this->duration_variables[jid], job.get_duration(),
		    std::string("job_") + std::to_string(jid) + std::string("_duration"));
	}
}

template <class SolverT>
void
EBILP<SolverT>::prepare_event_constraints()
{
	size_t event_count = 2 * this->instance.job_count();

	for (unsigned int eid = 0; eid < event_count - 1; ++eid) {
		this->model.add_constraint(this->event_times[eid],
		                           this->event_times[eid + 1], SolverT::INFTY);
	}
}

template <class SolverT>
void
EBILP<SolverT>::prepare_usage_expressions()
{
	size_t event_count = 2 * this->instance.job_count();
	/*
	        for (unsigned int eid = 0 ; eid < event_count ; ++eid) {
	                for (unsigned int rid = 0 ; rid <
	   this->instance.resource_count() ; ++rid) { Expression usage =
	   this->env.create_expression(); if (eid > 0) { usage +=
	   this->usages_after_event[rid][eid - 1];
	                        }

	                        for (unsigned int jid = 0 ; jid <
	   this->instance.job_count() ; ++jid) { const Job & job =
	   this->instance.get_job(jid); usage += this->job_start_events[jid][eid]
	                                                                *
	   job.get_resource_usage(rid); usage -= this->job_end_events[jid][eid]
	                                                                *
	   job.get_resource_usage(rid);
	                        }

	                        this->model.add_constraint(usage,
	   this->usages_after_event[rid][eid], SolverT::INFTY);
	                        this->model.add_constraint(usage,
	   this->max_usage_variables[rid], SolverT::INFTY);
	                }
	        }
	*/

	this->event_usages.resize(this->instance.resource_count(),
	                          std::vector<Expression>(event_count));

	for (unsigned int eid = 0; eid < event_count; ++eid) {
		for (unsigned int rid = 0; rid < this->instance.resource_count(); ++rid) {
			Expression last_event_expression;
			if (eid > 0) {
				last_event_expression = this->event_usages[rid][eid - 1];
			} else {
				last_event_expression = this->env.create_expression();
			}

			this->event_usages[rid][eid] =
			    this->env.create_expression() + last_event_expression;
			for (unsigned int jid = 0; jid < this->instance.job_count(); ++jid) {
				const Job & job = this->instance.get_job(jid);
				if ((eid < this->skip_numbers[jid].first) ||
				    (eid >= event_count - this->skip_numbers[jid].second)) {
					continue;
				}

				this->event_usages[rid][eid] +=
				    this->job_start_events[jid][eid - this->skip_numbers[jid].first] *
				    job.get_resource_usage(rid);
				this->event_usages[rid][eid] -=
				    this->job_end_events[jid][eid - this->skip_numbers[jid].first] *
				    job.get_resource_usage(rid);
			}

			this->model.add_constraint(
			    this->event_usages[rid][eid], this->max_usage_variables[rid],
			    SolverT::INFTY,
			    std::string("usage_res_") + std::to_string(rid) +
			        std::string("_after_ev_") + std::to_string(eid));
		}
	}
}

template <class SolverT>
void
EBILP<SolverT>::prepare_job_event_constraints()
{

	/*
	std::vector<Expression> event_expressions(event_count);
	for (size_t i = 0; i < event_count; ++i) {
	  event_expressions[i] = this->env.create_expression();
	}
	*/

	for (unsigned int jid = 0; jid < this->instance.job_count(); ++jid) {
		Expression start_expr = this->env.create_expression();
		Expression end_expr = this->env.create_expression();

		for (unsigned int eid = 0; eid < this->event_counts[jid]; ++eid) {
			start_expr += this->job_start_events[jid][eid];
			end_expr += this->job_end_events[jid][eid];
			/*
			event_expressions[eid] += this->job_start_events[jid][eid];
			event_expressions[eid] += this->job_end_events[jid][eid];
			*/
		}

		this->model.add_constraint(1, start_expr, 1,
		                           std::string("start_") + std::to_string(jid) +
		                               std::string("_once"));
		this->model.add_constraint(1, end_expr, 1,
		                           std::string("end_") + std::to_string(jid) +
		                               std::string("_once"));
	}

	/*
	for (size_t i = 0; i < event_count; ++i) {
	        this->model.add_constraint(1, event_expressions[i], 1,
	std::string("event_unique_") + std::to_string(i));
	}
	 */
}

template <class SolverT>
void
EBILP<SolverT>::prepare()
{
	this->prepare_pre();

	BOOST_LOG(l.d()) << "Preparing variables...";
	this->compute_skip_numbers();
	this->prepare_variables();
	BOOST_LOG(l.d()) << "Preparing duration constraints...";
	this->prepare_duration_constraint();
	BOOST_LOG(l.d()) << "Preparing event order constraints...";
	this->prepare_event_constraints();
	BOOST_LOG(l.d()) << "Preparing job / event linking constraints...";
	this->prepare_job_event_constraints();
	BOOST_LOG(l.d()) << "Preparing time between event constraints...";
	this->prepare_time_constraints();
	BOOST_LOG(l.d()) << "Preparing usage constraints...";
	this->prepare_usage_expressions();

	if (this->start_point_mode) {
		BOOST_LOG(l.d()) << "Preparing start time constraints...";
		this->prepare_start_points_constraints();
	} else {
		BOOST_LOG(l.d()) << "Preparing release / deadline constraints...";
		this->prepare_release_deadline_constraints();
		BOOST_LOG(l.d()) << "Preparing dependency constraints...";
		this->prepare_dependency_constraints();
	}

	this->prepare_post();
}

template <class SolverT>
void
EBILP<SolverT>::run()
{
	this->prepare();
	this->base_run();

	if (!this->start_point_mode) {
		this->create_solution();
	}
}

// explicit instantiation

#if defined(GUROBI_FOUND)
template class EBILP<ilpabstraction::GurobiInterface>;
#endif

#if defined(CPLEX_FOUND)
template class EBILP<ilpabstraction::CPLEXInterface>;
#endif


/*
 * msf eval adapted from aslam_eval by stefan leutenegger
 *
 */

#include <boost/thread.hpp>
#include <ros/ros.h>
#include <image_transport/image_transport.h>
#include "sensor_msgs/Imu.h"
#include "geometry_msgs/PoseWithCovarianceStamped.h"
#include "geometry_msgs/PoseStamped.h"
#include <opencv2/opencv.hpp>
#include <iostream>
#include <fstream>
#include <stdlib.h>
#include "rosbag/bag.h"
#include "rosbag/chunked_file.h"
#include "rosbag/view.h"
#include <sys/stat.h>
#include <sys/types.h>
#include <set>
#include <boost/assign/list_of.hpp>
#include <boost/foreach.hpp>
#include <nav_msgs/Odometry.h>
#include <geometry_msgs/TransformStamped.h>
#include <sm/kinematics/transformations.hpp>
#include <sm/kinematics/Transformation.hpp>
#include <sm/kinematics/quaternion_algebra.hpp>
#include <ros/package.h>

Eigen::Matrix<double, 3, 1> getPosition(geometry_msgs::TransformStampedConstPtr tf){
  Eigen::Matrix<double, 3, 1> pos;
  pos << tf->transform.translation.x, tf->transform.translation.y, tf->transform.translation.z;
  return pos;
}

Eigen::Matrix<double, 3, 1> getPosition(geometry_msgs::PoseWithCovarianceStampedConstPtr ps){
  Eigen::Matrix<double, 3, 1> pos;
  pos << ps->pose.pose.position.x, ps->pose.pose.position.y, ps->pose.pose.position.z;
  return pos;
}

Eigen::Quaterniond getOrientation(geometry_msgs::TransformStampedConstPtr tf){
  Eigen::Quaterniond orientation;
  orientation.w() = tf->transform.rotation.w;
  orientation.x() = tf->transform.rotation.x;
  orientation.y() = tf->transform.rotation.y;
  orientation.z() = tf->transform.rotation.z;
  return orientation;
}

Eigen::Quaterniond getOrientation(geometry_msgs::PoseWithCovarianceStampedConstPtr ps){
  Eigen::Quaterniond orientation;
  orientation.w() = ps->pose.pose.orientation.w;
  orientation.x() = ps->pose.pose.orientation.x;
  orientation.y() = ps->pose.pose.orientation.y;
  orientation.z() = ps->pose.pose.orientation.z;
  return orientation;
}

Eigen::Matrix<double, 6, 6> getCovariance(geometry_msgs::PoseWithCovarianceStampedConstPtr ps){
  return Eigen::Matrix<double, 6, 6>(&ps->pose.covariance[0]);
}

int main(int argc, char **argv)
{
  ros::init(argc, argv, "msf_eval");
  ros::Time::init();

  enum argIndices{
    bagfile = 1,
    EVAL_topic = 2,
    GT_topic = 3,
    singleRunOnly = 4
  };

  //calibration of sensor to vicon
  Eigen::Matrix4d T_BaBg_mat;

  //this is the Vicon one - SLAM sensor V0
  ///////////////////////////////////////////////////////////////////////////////////////////
  T_BaBg_mat << 0.999706627053000, -0.022330158354000, 0.005123243528000, -0.060614697387000, 0.022650462142000, 0.997389634278000, -0.068267398302000, 0.035557942651000, -0.003589706237000, 0.068397960288000, 0.997617159323000, -0.042589657349000, 0, 0, 0, 1.000000000000000;
  ////////////////////////////////////////////////////////////////////////////////////////////

  ros::Duration dt(0.0039);
  const double timeSyncThreshold = 0.005;
  const double timeDiscretization = 10.0; //discretization step for different starting points into the dataset
  const double trajectoryTimeDiscretization = 0.049; //discretization of evaluation points (vision framerate for fair comparison)
  const double startTimeOffset = 10.0;

  if (argc < 4)
  {
    MSF_ERROR_STREAM("usage: ./"<<argv[0]<<" bagfile EVAL_topic GT_topic [singleRunOnly]");
    return -1;
  }
  bool singleRun = false;
  if (argc == 5)
  {
    singleRun = atoi(argv[singleRunOnly]);
  }

  if(singleRun){
    MSF_WARN_STREAM("Doing only a single run.");
  }else{
    MSF_WARN_STREAM("Will process the dataset from different starting points.");
  }

  typedef geometry_msgs::TransformStamped GT_TYPE;
  typedef geometry_msgs::PoseWithCovarianceStamped EVAL_TYPE;

  // output file
  std::stringstream matlab_fname;

  std::string path = ros::package::getPath("msf_eval");
  matlab_fname << path << "/Matlab/matlab_data" << ros::Time::now().sec << ".m";

  std::ofstream outfile(matlab_fname.str().c_str());

  std::stringstream poses_EVAL;
  std::stringstream poses_GT;

  assert(outfile.good());

  outfile << "data=[" << std::endl;

  ros::Duration startOffset(startTimeOffset);

  sm::kinematics::Transformation T_BaBg(T_BaBg_mat); //body aslam to body Ground Truth

  // open for reading
  rosbag::Bag bag(argv[bagfile], rosbag::bagmode::Read);

  // views on topics
  rosbag::View view_EVAL(bag, rosbag::TopicQuery(argv[EVAL_topic]));
  rosbag::View view_GT(bag, rosbag::TopicQuery(argv[GT_topic]));

  //check topics
  if (view_EVAL.size() == 0)
  {
    MSF_ERROR_STREAM("The bag you provided does not contain messages for topic "<<argv[EVAL_topic]);
    return -1;
  }
  if (view_GT.size() == 0)
  {
    MSF_ERROR_STREAM("The bag you provided does not contain messages for topic "<<argv[GT_topic]);
    return -1;
  }

  //litter console with number of messages
  MSF_INFO_STREAM("Reading from "<<argv[bagfile]);
  MSF_INFO_STREAM("Topic "<<argv[EVAL_topic]<<", size: "<<view_EVAL.size());
  MSF_INFO_STREAM("Topic "<<argv[GT_topic]<<", size: "<<view_GT.size());

  //get times of first messages
  GT_TYPE::ConstPtr GT_begin = view_GT.begin()->instantiate<GT_TYPE>();
  assert(GT_begin);
  EVAL_TYPE::ConstPtr POSE_begin = view_EVAL.begin()->instantiate<EVAL_TYPE>();
  assert(POSE_begin);

  MSF_INFO_STREAM("First GT data at "<<GT_begin->header.stamp);
  MSF_INFO_STREAM("First EVAL data at "<<POSE_begin->header.stamp);

  while (true) // start eval from different starting points
  {

    rosbag::View::const_iterator it_EVAL = view_EVAL.begin();
    rosbag::View::const_iterator it_GT = view_GT.begin();

    // read ground truth
    ros::Time start;

    // Find start time alignment: set the GT iterater to point to a time larger than the aslam time
    while (true)
    {
      GT_TYPE::ConstPtr trafo = it_GT->instantiate<GT_TYPE>();
      assert(trafo);
      ros::Time time_GT;
      time_GT = trafo->header.stamp + dt;

      EVAL_TYPE::ConstPtr pose = it_EVAL->instantiate<EVAL_TYPE>();
      assert(pose);

      ros::Time time_EKF;
      time_EKF = pose->header.stamp;

      if (time_EKF >= time_GT)
      {
        it_GT++;
        if (it_GT == view_GT.end())
        {
          MSF_ERROR_STREAM("Time synchronization failed");
          return false;
        }
      }
      else
      {
        start = time_GT;
        MSF_INFO_STREAM("Time synced! GT start: "<<start <<" EVAL start: "<<time_EKF);
        break;
      }
    }

    // world frame alignment
    sm::kinematics::Transformation T_WaBa;
    sm::kinematics::Transformation T_WgBg;
    sm::kinematics::Transformation T_WgBg_last;
    sm::kinematics::Transformation T_WaWg;

    // now find the GT/EKF pairings
    int ctr = 0; //how many meas did we add this run?
    double ds = 0.0; // distance travelled
    ros::Time lastTime(0.0);

    MSF_INFO_STREAM("Processing measurements... Current start point: "<<startOffset<<"s into the bag.");

    for (; it_GT != view_GT.end(); ++it_GT)
    {
      GT_TYPE::ConstPtr trafo = it_GT->instantiate<GT_TYPE>();
      assert(trafo);

      // find closest timestamp
      ros::Time time_GT = trafo->header.stamp + dt;

      EVAL_TYPE::ConstPtr pose = it_EVAL->instantiate<EVAL_TYPE>();
      assert(pose);

      ros::Time time_EKF = pose->header.stamp;

      bool terminate = false;
      //get the measurement close to this GT value
      while (time_GT > time_EKF)
      {
        it_EVAL++;
        if (it_EVAL == view_EVAL.end())
        {
          terminate = true;
          MSF_INFO_STREAM("done. All EKF meas processed!");
          break;
        }
        pose = it_EVAL->instantiate<EVAL_TYPE>();
        assert(pose);
        time_EKF = pose->header.stamp;
      }
      if (terminate){
        break;
      }

      // add comparison value
      if (time_GT - start >= startOffset)
      {

        T_WaBa = sm::kinematics::Transformation(
            Eigen::Vector4d(-pose->pose.pose.orientation.x, -pose->pose.pose.orientation.y,
                            -pose->pose.pose.orientation.z, pose->pose.pose.orientation.w),
                            Eigen::Vector3d(pose->pose.pose.position.x, pose->pose.pose.position.y, pose->pose.pose.position.z));

        T_WgBg = sm::kinematics::Transformation(
            Eigen::Vector4d(-trafo->transform.rotation.x, -trafo->transform.rotation.y, -trafo->transform.rotation.z,
                            trafo->transform.rotation.w),
                            Eigen::Vector3d(trafo->transform.translation.x, trafo->transform.translation.y,
                                            trafo->transform.translation.z));

        // initial alignment
        if (ctr == 0)
        {
          T_WaWg = (T_WaBa) * T_BaBg * T_WgBg.inverse();
          T_WgBg_last = T_WgBg;
        }

        sm::kinematics::Transformation dT = T_WaBa * (T_WaWg * T_WgBg * T_BaBg.inverse()).inverse();
        sm::kinematics::Transformation T_WaBa_gt = (T_WaWg * T_WgBg * T_BaBg.inverse());

        T_WaBa_gt = sm::kinematics::Transformation(T_WaBa_gt.q().normalized(), T_WaBa_gt.t());
        dT = sm::kinematics::Transformation(dT.q().normalized(), dT.t());

        // update integral
        if (trafo)
        {
          ds += (T_WgBg.t() - T_WgBg_last.t()).norm();
          // store last GT transformation
          T_WgBg_last = T_WgBg;
        }
        else
        {
          // too noisy
          if ((T_WgBg * T_WgBg_last.inverse()).t().norm() > .1)
          {
            ds += (T_WgBg.t() - T_WgBg_last.t()).norm();
            //MSF_INFO_STREAM((T_WgBg*T_WgBg_last.inverse()).t().norm());
            // store last GT transformation
            T_WgBg_last = T_WgBg;
          }
        }
        Eigen::Vector3d dalpha;
        if (dT.q().head<3>().norm() > 1e-12)
        {
          dalpha = (asin(dT.q().head<3>().norm()) * 2 * dT.q().head<3>().normalized());
        }
        else
        {
          dalpha = 2 * dT.q().head<3>();
        }
        // g-vector alignment
        Eigen::Vector3d e_z_Wg(0, 0, 1);
        Eigen::Vector3d e_z_Wa = dT.C() * e_z_Wg;
        double dalpha_e_z = acos(std::min(1.0, e_z_Wg.dot(e_z_Wa)));

        if (fabs((time_GT - time_EKF).toSec()) < timeSyncThreshold
            && (time_EKF - lastTime).toSec() > trajectoryTimeDiscretization)
        {
          if (startOffset.toSec() == startTimeOffset)
          {
            poses_EVAL << T_WaBa.t().transpose() << ";" << std::endl;
            poses_GT << T_WgBg.t().transpose() << ";" << std::endl;
          }
          Eigen::Matrix<double, 6, 6> cov = getCovariance(pose);
          outfile << (time_GT - start).toSec() << " "
              << ds << " " << (T_WaBa.t() - T_WaBa_gt.t()).norm() << " " //translation
              << 2 * acos(std::min(1.0, fabs(dT.q()[3]))) << " " << dalpha_e_z << " " //orientation
              <<cov(0, 0)<<" "<<cov(1, 1)<<" "<<cov(2, 2)<<" " //position covariances
              <<cov(3, 3)<<" "<<cov(4, 4)<<" "<<cov(5, 5)<<";" //orientation covariances
              << std::endl;

          lastTime = time_EKF; // remember
        }

        // count comparisons
        ctr++;

      }
    }

    MSF_INFO_STREAM("Added "<<ctr<<" measurement edges.");

    //where in the bag should the next eval start
    startOffset += ros::Duration(timeDiscretization);

    if (ctr == 0 || singleRun) //any new measurements this run?
    {
      break;
    }
  }

  // cleanup
  bag.close();

  outfile << "];";
  outfile << "poses = [" << poses_EVAL.str() << "];" << std::endl;
  outfile << "poses_GT = [" << poses_GT.str() << "];" << std::endl;
  outfile.close();

  return 0;
}


#include <Data/DATA.h>
#include <Data/RIGID_STRUCTURE_DATA.h>
#include <Driver/SIMULATION.h>
#include <Evolution/EVOLUTION.h>
#include <Force/FORCE.h>
#include <Force/FORCE_TYPE.h>
#include <Utilities/CEREAL_HELPERS.h>
#include <Utilities/RANDOM.h>
#include <fstream>
#include <Eigen/Geometry>
#include <sys/stat.h>
using namespace Mechanics;
///////////////////////////////////////////////////////////////////////
template<class TV> SIMULATION<TV>::
SIMULATION()
    :data(*new DATA<TV>()),evolution(*new EVOLUTION<TV>()),force(*new FORCE<TV>()),
    current_frame(0),restart_frame(0),output_number(0),dt(.1),time(0),restart(false),substeps(false),write(true),
    output_directory("."),title("")
{
}
///////////////////////////////////////////////////////////////////////
template<class TV> SIMULATION<TV>::
~SIMULATION()
{
    delete &data;
    delete &evolution;
    delete &force;
}
///////////////////////////////////////////////////////////////////////
template<class TV> void SIMULATION<TV>::
Write(const std::string& frame_title)
{
    if(write){
        title=frame_title;
        std::ostringstream stringStream;
        mkdir(output_directory.c_str(),0777);
        stringStream<<output_directory<<"/frame."<<output_number++;
        std::ofstream output(stringStream.str().c_str(),std::ios::out);
        //cereal::BinaryOutputArchive archive(output);
        cereal::JSONOutputArchive archive(output);
        archive(time,title,data,data.random,force);
    }
}
///////////////////////////////////////////////////////////////////////
template<class TV> bool SIMULATION<TV>::
Read(const int frame)
{
    std::ostringstream stringStream;
    stringStream<<output_directory<<"/frame."<<frame;
    std::ifstream input(stringStream.str().c_str(),std::ios::in);
    if(!input.is_open()){return false;}
    //cereal::BinaryInputArchive archive(input);
    cereal::JSONInputArchive archive(input);
    archive(time,title,data,data.random,force);
    return true;
}
///////////////////////////////////////////////////////////////////////
GENERIC_TYPE_DEFINITION(SIMULATION)


// Copyright (c) 2005 - 2014 Marc de Kamps
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met:
//
//    * Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer.
//    * Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation
//      and/or other materials provided with the distribution.
//    * Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from this software
//      without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY
// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF
// USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
//
//      If you use this software in work leading to a scientific publication, you should include a reference there to
//      the 'currently valid reference', which can be found at http://miind.sourceforge.net
#define BOOST_TEST_DYN_LINK
#include <boost/test/unit_test.hpp>
#include <boost/test/execution_monitor.hpp>
#include <MPILib/include/MPINetworkCode.hpp>
#include <MPILib/include/NodeType.hpp>
#include <MPILib/include/DelayAssemblyAlgorithmCode.hpp>
#include <MPILib/include/report/handler/RootReportHandler.hpp>
#include <MPILib/include/RateFunctorCode.hpp>
#include <GeomLib.hpp>

typedef MPILib::MPINetwork<MPILib::DelayedConnection, MPILib::utilities::CircularDistribution> Network;
typedef GeomLib::GeomAlgorithm<MPILib::DelayedConnection> GeomDelayAlg;

using MPILib::RateFunctor;
using MPILib::DelayAssemblyParameter;
using MPILib::DelayAssemblyAlgorithm;
using MPILib::Time;

BOOST_AUTO_TEST_CASE(DelayAssemblyConstructionTest ) {

  MPILib::DelayAssemblyParameter par_ass;
  MPILib::DelayAssemblyAlgorithm<MPILib::DelayedConnection> alg_ass(par_ass);
}

Rate SwitchOnAndOff(Time t){

  if (t > 0.1 && t < 0.15)
    return 100.0;

  if (t > 0.7 && t < 0.75 )
    return -100.0;

  return 0.0;
}

BOOST_AUTO_TEST_CASE(DelayAssemblyNetworkTest) {
  Network network;

  DelayAssemblyParameter
    par_ass
    (
     1e10, // A large membrane time constant; the population keeps its activation
     30.,  // If the population is switched on, it fires initially at 30 Hz
     10.,  // If the total weighted input comes above this value, delay is triggered
     -10.  // If the total weighted input comes below this value, delay is switched off
    );

  DelayAssemblyAlgorithm<MPILib::DelayedConnection> alg(par_ass);
  NodeId id_ass = network.addNode(alg,EXCITATORY_DIRECT);

  RateFunctor<MPILib::DelayedConnection> func(SwitchOnAndOff);
  NodeId id_switch = network.addNode(func,NEUTRAL);

  network.makeFirstInputOfSecond(id_switch,id_ass,MPILib::DelayedConnection(1.0,1.0,0.0));

  MPILib::report::handler::RootReportHandler handler("delayass.root", false, false);
  SimulationRunParameter par_run(handler,1000000,0.,1.,1e-2,1e-4,"delayass.log");
  network.configureSimulation(par_run);

  network.evolve();
}