#include <vector>
#include <array>
#include <iostream>
#include <math.h>

/*
	Requires C++ Boost Library for 1.65s for windows
	https://dl.bintray.com/boostorg/release/1.65.1/binaries/
	(boost_1_65_1-msvc-10.0-32.exe)

	include directory: C:\local\boost_1_65_1
*/
#include <boost\polygon\voronoi.hpp>
#include "FastNoise.h"
#include <SFML\Graphics.hpp>

#include "DiamondSquare.h"
#include "QuadTree.h"
#include "MinimumSpanningTree.h"
#include "RoadNode.h"
#include "Node.h"
#include "MstNode.h"
#include "V2.h"
#include "Edge.h"
#include "Road.h"
#include "Pathfinding.h"
#include "WaterData.h"
#include "BuildingLot.h"
#include "UtilRandom.h"
#include "Utility.h"
#include "Line.h"

#include "FloorPlan.h"
#include "BoundingBox.h"
#include "jbShape.h"
#include "shapeLine.h"

//! Voronoi
using namespace std;
using namespace boost::polygon;
typedef int coordinate_type;
typedef point_data<coordinate_type> point_type;
typedef voronoi_diagram<double> VD;

const int winSize = 512;
int offsetForRoadNodes = 5;
float maxMinorRoadDist = 100;

template <typename Iter>
Iter ReturnNextIter(Iter iter)
{
	return ++iter;
}

void makeAllFastNoiseValuesPositive(std::vector<std::vector<float>> &popMap, float &highestVal)
{
	// Find the lowest and highest values
	float highest = INT_MIN;
	float lowest = INT_MAX;
	for (auto vec : popMap)
	{
		for (auto val : vec)
		{
			if (val > highest) highest = val;
			if (val < lowest) lowest = val;
		}
	}

	highestVal = highest;

	// Add the absolute of the lowest onto every value
	// so it ranges from 0-highest+abs(lowest) and not -1 - 1
	float toAdd = fabsf(lowest);
	highestVal += toAdd;
	for (auto &vec : popMap)
	{
		for (auto &val : vec)
		{
			val += toAdd;
		}
	}
}

void seedNoise(FastNoise &fn)
{
	srand(time(NULL));

	fn.SetSeed(rand() % INT_MAX);
}

void fillNoise(FastNoise &fn, std::vector<std::vector<float>> &popMap, int size)
{
	for (int y = 0; y < size; y++)
	{
		popMap.push_back(std::vector<float>());
		for (int x = 0; x < size; x++)
		{
			popMap[y].push_back(fn.GetNoise(x, y));
		}
	}
}

void LoadWaterDataFromMap(WaterData &wd)
{
	sf::Image waterMap;
	waterMap.loadFromFile("watermap.bmp");

	int wid = waterMap.getSize().x;
	int hei = waterMap.getSize().y;

	for (int y = 0; y < hei; y++)
	{
		for (int x = 0; x < wid; x++)
		{
			waterMap.getPixel(x, y) == sf::Color::White ? wd.SetPixelWater(x, y, true) : wd.SetPixelWater(x, y, false);
		}
	}
}

void LoadWaterDataFromTerrain(WaterData &wd, DiamondSquare terrain, float percentage)
{
	// First, make all of the values positive
	float lowest = terrain.Lowest();
	float highest = terrain.Highest();
	float toAdd = fabsf(lowest);
	for (auto &row : terrain.Points())
	{
		for (auto &point : row)
		{
			point->z += toAdd;
		}
	}

	// Find the new highest and lowest values
	float newLowest = lowest + toAdd;
	float newHighest = highest + toAdd;

	// Find the difference
	float difference = newHighest - newLowest;

	// The bottom 20% shal be considered water
	float cutoffPoint = difference * (percentage / 100);
	std::cout << "Highest terrain point: " << newHighest << std::endl;
	std::cout << "Cut off point: " << cutoffPoint << std::endl;

	// Get the size
	int wid, hei;
	wid = terrain.getDivisions();
	hei = terrain.getDivisions();

	// Loop through, assign water or terrain
	auto points = terrain.Points();
	for (int y = 0; y < hei; y++)
	{
		for (int x = 0; x < wid; x++)
		{
			// If the Z point is lower than or equal to the cut off point, set it as water. Otherwise, don't
			points[y][x]->z <= cutoffPoint ? wd.SetPixelWater(x, y, true) : wd.SetPixelWater(x, y, false);
		}
	}
}

int main()
{
	/*************************
		Population noise
	**************************/
	std::cout << "Generation population noise." << std::endl;
	FastNoise fn;
	std::vector<std::vector<float>> popMap;
	seedNoise(fn);
	fn.SetNoiseType(FastNoise::ValueFractal);
	fn.SetFrequency(0.01); // Default 0.01
	fn.SetFractalOctaves(6); // Default 3
	fn.SetInterp(FastNoise::Interp::Quintic); // Default Quintic
	fillNoise(fn, popMap, winSize);

	/* Make the values positive */
	float highestVal;
	makeAllFastNoiseValuesPositive(popMap, highestVal);

	/************************
		Height/terrain data
	*************************/
	std::cout << "Generating DiamondSquare terrain..." << std::endl;
	int divisions = winSize;
	int size = 5;
	int height = 64;
	DiamondSquare terrain = DiamondSquare(divisions, size, height);
	terrain.Generate();
	terrain.CreatePoints();
	terrain.CalcuateBoundaryPoints();
	// ds.Print();

	/************************
		Water-boundary map
	*************************/
	std::cout << "Loading water-boundary map." << std::endl;
	WaterData waterData = WaterData(winSize);
	// LoadWaterDataFromMap(waterData);
	LoadWaterDataFromTerrain(waterData, terrain, 10);

	// To display
	//sf::Texture waterMapTexture;
	//waterMapTexture.loadFromFile("watermap.bmp");
	//waterMapTexture.setSmooth(true);
	//sf::Sprite waterMapSprite;
	//waterMapSprite.setTexture(waterMapTexture);

	/***************************
			Quad tree
	**************************/
	std::cout << "Creating quad-tree." << std::endl;
	QuadTree* qt = new QuadTree(0, 0, winSize, winSize, nullptr, popMap, waterData, winSize, highestVal);

	/****************************
		Voronoi diagram
		to create minor roads
	*****************************/

	std::cout << "Creating voronoi minor roads." << std::endl;

	// Create voronoi points
	vector<point_type> voronoiPoints;
	int bufferSpace = 5;

	// For each quad in the quadtree
	for (auto quad : qt->GetTreeChildren())
	{
		// Get the boundaries of this quad
		int minX = quad->xOrigin;
		int maxX = quad->xOrigin + quad->width;
		int minY = quad->yOrigin;
		int maxY = quad->yOrigin + quad->height;

		// Do not place a point in a quad that contains water..

		// Find all valid land positions in this quad
		std::vector<V2> waterPositions;
		for (int y = minY; y < maxY; y++)
		{
			for (int x = minX; x < maxX; x++)
			{
				if (waterData.IsWater(x, y))
				{
					waterPositions.push_back(V2(x, y));
				}
			}
		}

		// If none exist, continue without placing a point
		if (waterPositions.size() > 0) { continue; }

		// Otherwise, place a point
		double px = UtilRandom::Instance()->RandomFloat(minX + bufferSpace, maxX - bufferSpace);
		double py = UtilRandom::Instance()->RandomFloat(minY + bufferSpace, maxY - bufferSpace);

		voronoiPoints.push_back(point_type(px, py));
	}

	// Construct the voronoi diagram
	VD vd;
	construct_voronoi(voronoiPoints.begin(), voronoiPoints.end(), &vd);

	// Create major roads from voronoi edges
	std::vector<Road*> minorRoads;
	for (auto const &edge : vd.edges())
	{
		if (edge.vertex0() != NULL && edge.vertex1() != NULL)
		{
			int sX = edge.vertex0()->x();
			int sY = edge.vertex0()->y();

			int eX = edge.vertex1()->x();
			int eY = edge.vertex1()->y();

			Road* road = new Road(sX, sY, eX, eY);

			bool exists = false;
			for (auto r : minorRoads)
			{
				if (*road->start == *r->end && *road->end == *r->start)
				{
					exists = true;
					break;
				}
			}
			if (!exists)
			{
				minorRoads.push_back(road);
			}
		}
	}

	/************************
				MST
	*************************/
	std::cout << "Creating MST..." << std::endl;

	MinimumSpanningTree mst = MinimumSpanningTree();
	for (auto quad : qt->GetTreeChildren())
	{
		// Only spawn quads in the big-enough quads
		if (quad->indexer > 2)
		{
			mst.SpawnPoint(waterData, offsetForRoadNodes, quad->xOrigin, quad->yOrigin, (quad->xOrigin + quad->width), (quad->yOrigin + quad->height));
		}
	}
	float maximumDistanceBetweenNeighbours = winSize / 4;
	mst.AssignNighbours(maximumDistanceBetweenNeighbours); // Assign neighbours
	mst.CreateAllEdges(); // Create edges
	mst.Sort(); // Sort the MST

	/****************************************
		Creating roadnodes from terrain data
	*****************************************/
	std::vector<std::vector<RoadNode*>> roadNodes;
	roadNodes = terrain.CreatePointsAndPassBackRoadNodes(offsetForRoadNodes, waterData);

	/***************************************************
		Pathfind roads using the mst edges
		and roadnodes generated from the diamondsquare
		terrain
	****************************************************/

	std::vector<Road*> majorRoads;
	std::cout << "Pathfinding " << mst.GetTreeEdges().size() << " roads..." << std::endl;
	int roadCounter = 0;
	for (Edge edge : mst.GetTreeEdges())
	{
		if (edge.start->position->x == edge.end->position->x && edge.start->position->y == edge.end->position->y)
		{
			continue;
		}

		// Create the road
		Road* road = new Road();

		// Pathfind to get the nodes
		road->nodes = Pathfinding::PathFind(roadNodes,
			edge.start->position->x / offsetForRoadNodes,
			edge.start->position->y / offsetForRoadNodes,
			edge.end->position->x / offsetForRoadNodes,
			edge.end->position->y / offsetForRoadNodes,
			offsetForRoadNodes);

		// Split the road into segments
		for (std::vector<RoadNode*>::iterator iter = road->nodes.begin(); iter != road->nodes.end(); iter++)
		{
			if (ReturnNextIter(iter) != road->nodes.end())
			{
				road->segments.push_back(
					new Road((*iter)->position, (*ReturnNextIter(iter))->position));
			}
		}

		majorRoads.push_back(road);

		std::cout << "\r" << roadCounter++ << " roads complete.\t\t";
	}
	std::cout << std::endl;

	/*******************************************************
			Remove any voronoi minor roads that
			overlap main mst roads and reconnect the
			voronoi to the main roads
	********************************************************/
	std::vector<V2*> expandPoints;

	std::cout << "Checking for minor roads to prune..." << std::endl;

	// For each minor road
	for (auto minor : minorRoads)
	{
		// Check each major road
		for (auto &major : majorRoads)
		{
			// Check each segment
			for (auto &seg : major->segments)
			{
				if (Utility::GetIntersectionPointWithFiniteLines(minor->start, minor->end, seg->start, seg->end) != nullptr)
				{
					// Mark this road for deletion
					minor->markedForDeletion = true;

					// Tell its neighbours to expand and find a major road to connect to
					expandPoints.push_back(new V2(minor->start->x, minor->start->y));
					expandPoints.push_back(new V2(minor->end->x, minor->end->y));
				}
			}
		}
	}

	std::cout << "Deleting roads." << std::endl;

	// Remove and minor roads that have been marked for deletion
	for (std::vector<Road*>::iterator iter = minorRoads.begin(); iter != minorRoads.end(); /**/)
	{
		if ((*iter)->markedForDeletion)
		{
			// Tell its neighbours to expand and find a major road to connect to
			iter = minorRoads.erase(iter);
		}
		else
		{
			iter++;
		}
	}

	// Time to reconnect the voronoi to the main roads

	std::cout << "Reconnecting minor roads to highways" << std::endl;

	// For each point
	for (V2* point : expandPoints)
	{
		// Find the closest roadNode to it
		RoadNode* closest = nullptr;

		// Check each node
		for (Road* road : majorRoads)
		{
			for (RoadNode* node : road->nodes)
			{
				// The first will be closest by default
				if (closest == nullptr) { closest = node; }

				// If node is closer than closest, closest becomes node
				if (V2::DistanceBetween(*node->position, *point) < V2::DistanceBetween(*closest->position, *point))
				{
					closest = node;
				}
			}
		}

		// After finding the closest, create this new road
		minorRoads.push_back(
			new Road(point->x,
				point->y,
				closest->position->x,
				closest->position->y));
	}

	// Now that we've built new minor roads, we need to check for collisions between any minor roads

	std::cout << "Checking for new collisions." << std::endl;

	for (Road* minorRoad : minorRoads)
	{
		for (Road* other : minorRoads)
		{
			// Skip itself
			if (*minorRoad == *other) { continue; }

			// if Other is already marked for deletion, skip
			// We don't want both roads to get deleted which would cause there to be
			// 0 connections instead of 2 (we want there to be 1)
			if (other->markedForDeletion) { continue; }

			// Get the intersection point
			V2* iPoint = Utility::GetIntersectionPointWithFiniteLines(minorRoad->start, minorRoad->end, other->start, other->end);

			// If it's not null
			if (iPoint != nullptr)
			{
				// Make sure its not where the roads naturally connect
				if (*iPoint == *minorRoad->start
					|| *iPoint == *minorRoad->end
					|| *iPoint == *other->start
					|| *iPoint == *other->end)
				{
					continue;
				}
			}
			else { continue; }

			// If not, continue
			minorRoad->markedForDeletion = true;
		}
	}

	// We can also detect here to delete and roads that have an end which sits inside of a body of water
	for (Road* minorRoad : minorRoads)
	{
		if (waterData.IsWater(minorRoad->start->x, minorRoad->start->y)
			||
			waterData.IsWater(minorRoad->end->x, minorRoad->end->y))
		{
			minorRoad->markedForDeletion = true;
		}
	}

	// Next we delete any minor roads that are above a certain length threshold
	// this is to remove the long outwards lines left over from the voronoi
	for (Road* minorRoad : minorRoads)
	{
		if (V2::DistanceBetween(*minorRoad->start, *minorRoad->end) > maxMinorRoadDist)
		{
			minorRoad->markedForDeletion = true;
		}
	}

	// Remove any new minor roads that have been marked for deletion
	for (std::vector<Road*>::iterator iter = minorRoads.begin(); iter != minorRoads.end(); /**/)
	{
		if ((*iter)->markedForDeletion)
		{
			// Erase
			iter = minorRoads.erase(iter);
		}
		else
		{
			iter++;
		}
	}

	std::cout << "Neighbouring minor roads " << std::endl;

	// Now that we've deleted all of these roads, neighbour the remaining roads
	for (Road* minorRoad : minorRoads)
	{
		// Against every other road
		for (Road* otherRoad : minorRoads)
		{
			// Skip itself
			if (*minorRoad == *otherRoad) { continue; }

			// Check for neighbours

			// If otherRoad has minorRoad.Start
			if (*minorRoad->start == *otherRoad->start
				||
				*minorRoad->start == *otherRoad->end)
			{
				minorRoad->startChildren.push_back(minorRoad);
			}

			// If otherRoad has minorRoad.End
			if (*minorRoad->end == *otherRoad->start
				||
				*minorRoad->end == *otherRoad->end)
			{
				minorRoad->endChildren.push_back(minorRoad);
			}
		}
	}

	std::cout << "Removing minor roads with zero neighbours" << std::endl;

	// Remove any minor roads that have 0 neighbours (and thus are sitting along)
	for (std::vector<Road*>::iterator iter = minorRoads.begin(); iter != minorRoads.end(); /**/)
	{
		// no neighbours on BOTH sides, it has to have 0 end AND 0 start
		if ((*iter)->endChildren.size() == 0 && (*iter)->startChildren.size() == 0)
		{
			iter = minorRoads.erase(iter);
		}
		else
		{
			iter++;
		}
	}

	/***********************************************
		Now that the minor roads are set up,
		generate building lots in each minor
		road that is still in the road network
	************************************************/
	std::cout << "Generating building lots on minor roads" << std::endl;
	int lotCounter = 0;
	for (Road* road : minorRoads)
	{
		std::cout << "\rGenerating lot of road: " << lotCounter++ << "\t\t";
		road->GenerateBuildingLots();
	}
	std::cout << std::endl;

	/*******************************************************
		Check for and remove any building lot collisions
	********************************************************/

	std::cout << "Checking for building lot collisions..." << std::endl;

	/* MINOR ROADS */

	/* First, we prioritive collisions against minor roads and major roads */

	std::cout << "Between minor roads..." << std::endl;
	// For every building lot
	for (Road* minorRoad : minorRoads)
	{
		for (BuildingLot* lot : minorRoad->lots)
		{
			// Check every minor road
			for (Road* otherRoad : minorRoads)
			{
				// Skip itself
				if (*minorRoad == *otherRoad) { continue; }

				// Check if each line of the lot intersects with the otherRoad
				for (Line* line : lot->lines)
				{
					if (Utility::GetIntersectionPointWithFiniteLines(line->start, line->end, otherRoad->start, otherRoad->end) != nullptr)
					{
						lot->markForDeletion = true;
					}
				}
			}
		}
	}

	std::cout << "Between major roads..." << std::endl;
	int majorRoadsChecked = 0;
	// For every building lot
	for (Road* minorRoad : minorRoads)
	{
		for (BuildingLot* lot : minorRoad->lots)
		{
			// For every major road
			for (Road* majorRoad : majorRoads)
			{
				// Check each segment of the major road
				for (auto seg : majorRoad->segments)
				{
					// For each line of the buildnig lot
					for (auto line : lot->lines)
					{
						if (Utility::GetIntersectionPointWithFiniteLines(line->start, line->end, seg->start, seg->end) != nullptr)
						{
							lot->markForDeletion = true;
						}
					}
				}
			}
		}
	}

	int minorLotDeletionCounter = 0;
	for (Road* road : minorRoads)
	{
		std::vector<BuildingLot*> lots = road->lots;

		for (std::vector<BuildingLot*>::iterator it = lots.begin(); it != lots.end(); /**/)
		{
			if ((*it)->markForDeletion)
			{
				it = lots.erase(it);
				minorLotDeletionCounter++;
			}
			else
				++it;
		}
		road->lots = lots;
	}

	/* Next, we check for collisions against other building lots */

	std::cout << "Between other building lots..." << std::endl;
	// Check every road against every other road
	for (Road* minorRoad : minorRoads)
	{
		for (Road* otherRoad : minorRoads)
		{
			// Do not check the road against itself
			if (*minorRoad == *otherRoad) { continue; }

			// Check each lot of each road
			for (BuildingLot* lot : minorRoad->lots)
			{
				for (BuildingLot* otherLot : otherRoad->lots)
				{
					// If the lots are far away we do not need to check for interceptions
					if (V2::DistanceBetween(*lot->bottomLeft, *otherLot->bottomLeft) > 15)
					{
						continue;
					}

					if (lot->IsLotWithin(otherLot))
					{
						lot->markForDeletion = true;
						otherLot->markForDeletion = true;
					}
				}
			}
		}
	}

	for (Road* road : minorRoads)
	{
		std::vector<BuildingLot*> lots = road->lots;

		for (std::vector<BuildingLot*>::iterator it = lots.begin();  it != lots.end(); /**/)
		{
			if ((*it)->markForDeletion)
			{
				it = lots.erase(it);
				minorLotDeletionCounter++;
			}
			else
				++it;
		}
		road->lots = lots;
	}

	std::cout << minorLotDeletionCounter << " lots pruned." << std::endl;

	/* MAJOR ROADS */

	//std::cout << "Checking for lot interceptions" << std::endl;

	//int lotInterceptionCounter = 0;
	//// For every lot
	//for (auto &road : majorRoads) {
	//	std::cout << "\rChecking road " << lotInterceptionCounter++ << "/" << majorRoads.size() << "\t\t";
	//	for (auto &lot : road->lots) {

	//		// For every other lot
	//		for (auto &otherRoad : majorRoads) {
	//			for (auto &otherLot : road->lots) {

	//				// Don't check a lot against itself
	//				if (*lot == *otherLot) { continue; }

	//				// We don't need to check lots that have already been checked
	//				if (otherLot->markForDeletion) { continue; }

	//				// We don't need to check lots that aren't within the minimum range
	//				if (V2::DistanceBetween(*lot->bottomLeft, *otherLot->bottomLeft) > 10) { continue; }

	//				if (lot->IsLotWithin(otherLot))
	//				{
	//					std::cout << "\nSetting a lot to be marked." << std::endl;
	//					otherLot->markForDeletion = true;
	//				}
	//			}
	//		}
	//	}
	//}
	//std::cout << std::endl;

	//int deletionCounter = 0;
	//int counter = 0;

	//for (auto &road : majorRoads)
	//{
	//	std::vector<BuildingLot*> lots = road->lots;

	//	for (std::vector<BuildingLot*>::iterator it = lots.begin();  it != lots.end(); /**/)
	//	{
	//		counter++;
	//		if ((*it)->markForDeletion)
	//		{
	//			it = lots.erase(it);
	//			deletionCounter++;
	//		}
	//		else
	//			++it;
	//	}
	//	road->lots = lots;
	//}

	//std::cout << counter << " building lots." << std::endl;
	//std::cout << deletionCounter << " deleted." << std::endl;

	/*******************************************************
		Generate buildings inside of each building lot
	********************************************************/

	std::vector<V2*> lotCenter;
	std::vector<V2*> buildingCenter;

	for (auto road : minorRoads)
	{
		for (auto lot : road->lots)
		{
			// Give the lot a new FloorPlan object
			lot->fp = new FloorPlan();

			// Create the lots bounding box for building generation
			lot->fp->SetBoundingBox(lot->minX, lot->minY, lot->maxX - lot->minX, lot->maxY - lot->minY);

			// Generate shapes
			int numberOfShapes = UtilRandom::Instance()->RandomInt(2, 4);
			lot->fp->GenerateShapes(numberOfShapes);

			// Calculate building perimeter
			lot->fp->GeneratePerimeter();

			// Find the center point of the generated building

			// First we need to find the minimum and maximum boundaries
			float buildingMinX = FLT_MAX, buildingMaxX = -FLT_MAX, buildingMinY = FLT_MAX, buildingMaxY = -FLT_MAX;
			for (shapeLine* line : lot->fp->perimeterLines)
			{
				if (line->start->x < buildingMinX) buildingMinX = line->start->x;
				if (line->start->x > buildingMaxX) buildingMaxX = line->start->x;

				if (line->start->y < buildingMinY) buildingMinY = line->start->y;
				if (line->start->y > buildingMaxY) buildingMaxY = line->start->y;

				if (line->end->x < buildingMinX) buildingMinX = line->start->x;
				if (line->end->x > buildingMaxX) buildingMaxX = line->start->x;

				if (line->end->y < buildingMinY) buildingMinY = line->start->y;
				if (line->end->y > buildingMaxY) buildingMaxY = line->start->y;
			}

			// Then we find the center
			float buildingDiffX = buildingMaxX - buildingMinX;
			float buildingDiffY = buildingMaxY - buildingMinY;

			float buildingMidX = buildingMaxX - (buildingDiffX / 2);
			float buildingMidY = buildingMaxY - (buildingDiffY / 2);

			/*
			Find the angle to rotate by
			*/

			// Get the road the lot belongs to
			Road* lotParent = lot->parent;

			// Get parent start and end
			V2* parentStart = lotParent->nodes[0]->position;
			V2* parentEnd = lotParent->nodes[1]->position;

			// Find the parents facing angle
			V2* parentDir = new V2(parentEnd->x - parentStart->x, parentEnd->y - parentStart->y);
			float parentAngle = V2::VectorToAngle(parentDir);

			float angleToRotate = 90 - parentAngle - 45;

			// Rotate the points around the found center point
			for (shapeLine* line : lot->fp->perimeterLines)
			{
				line->start = Utility::RotateAroundPoint(line->start, new V2(buildingMidX, buildingMidY), angleToRotate);
				line->end = Utility::RotateAroundPoint(line->end, new V2(buildingMidX, buildingMidY), angleToRotate);
			}

			/*
			Move the building so the center point of the building is the center point of the shape
			*/

			// Find the center of the lot
			float lotDiffX = lot->maxX - lot->minX;
			float lotDiffY = lot->maxY - lot->minY;

			float lotMidX = lot->maxX - (lotDiffX / 2);
			float lotMidY = lot->maxY - (lotDiffY / 2);

			// Find out the offset
			float offsetX = lotMidX - buildingMidX;
			float offsetY = lotMidY - buildingMidY;

			buildingCenter.push_back(new V2(buildingMidX, buildingMidY));
			lotCenter.push_back(new V2(lotMidX, lotMidY));

			// Move the lines by this much
			for (shapeLine* line : lot->fp->perimeterLines)
			{
				line->start->x += offsetX;
				line->start->y += offsetY;

				line->end->x += offsetX;
				line->end->y += offsetY;
			}
		}
	}

	/* Create window */
	sf::RenderWindow window(sf::VideoMode(winSize, winSize), "Window");
	sf::View view(sf::FloatRect(0, 0, winSize, winSize));
	window.setView(view);

	bool drawPopMap = false,
		drawHeightMap = false,
		drawWaterBoundaryMap = false,
		drawQuadTree = false,
		drawMstNodes = false,
		drawRoads = true,
		drawMST = false,
		drawBuildingLots = false,
		drawBuildings = false,
		drawMinorRoads = false,
		drawVoronoiPointsForMinorRoads = false;

	std::cout << std::endl << "Instructions: " << std::endl;
	std::cout << "\t1: Toggle population map" << std::endl;
	std::cout << "\t2: Toggle height map" << std::endl;
	std::cout << "\t3: Toggle water-boundary map" << std::endl;
	std::cout << "\t4: Toggle quad-tree" << std::endl;
	std::cout << "\t5: Toggle mst nodes" << std::endl;
	std::cout << "\t6: Toggle roads" << std::endl;
	std::cout << "\t7: Toggle MST" << std::endl;
	std::cout << "\t8: Toggle building lots" << std::endl;
	std::cout << "\t9: Toggle buildings" << std::endl;
	std::cout << "\t0: Toggle minor roads" << std::endl;
	std::cout << "\tz: Toggle minor road voronoi points" << std::endl;
	std::cout << std::endl;
	std::cout << "\t WASD - Move camera" << std::endl;
	std::cout << "\t Q/E - (Un)zoom camera" << std::endl;

	while (window.isOpen())
	{
		window.setView(view);
		sf::Event event;
		while (window.pollEvent(event))
		{
			if (event.type == sf::Event::Closed) { window.close(); }

			if (event.type == sf::Event::KeyPressed)
			{
				if (sf::Keyboard::isKeyPressed(sf::Keyboard::Key::Num1)) { drawPopMap = !drawPopMap; }
				else if (sf::Keyboard::isKeyPressed(sf::Keyboard::Key::Num2)) { drawHeightMap = !drawHeightMap; }
				else if (sf::Keyboard::isKeyPressed(sf::Keyboard::Key::Num3)) { drawWaterBoundaryMap = !drawWaterBoundaryMap; }
				else if (sf::Keyboard::isKeyPressed(sf::Keyboard::Key::Num4)) { drawQuadTree = !drawQuadTree; }
				else if (sf::Keyboard::isKeyPressed(sf::Keyboard::Key::Num5)) { drawMstNodes = !drawMstNodes; }
				else if (sf::Keyboard::isKeyPressed(sf::Keyboard::Key::Num6)) { drawRoads = !drawRoads; }
				else if (sf::Keyboard::isKeyPressed(sf::Keyboard::Key::Num7)) { drawMST = !drawMST; }
				else if (sf::Keyboard::isKeyPressed(sf::Keyboard::Key::Num8)) { drawBuildingLots = !drawBuildingLots; }
				else if (sf::Keyboard::isKeyPressed(sf::Keyboard::Key::Num9)) { drawBuildings = !drawBuildings; }
				else if (sf::Keyboard::isKeyPressed(sf::Keyboard::Key::Num0)) { drawMinorRoads = !drawMinorRoads; }
				else if (sf::Keyboard::isKeyPressed(sf::Keyboard::Key::Z)) { drawVoronoiPointsForMinorRoads = !drawVoronoiPointsForMinorRoads; }

				// Camera movement
				if (sf::Keyboard::isKeyPressed(sf::Keyboard::Key::W))
					view.move(0, -10);
				if (sf::Keyboard::isKeyPressed(sf::Keyboard::Key::A))
					view.move(-10, 0);
				if (sf::Keyboard::isKeyPressed(sf::Keyboard::Key::S))
					view.move(0, 10);
				if (sf::Keyboard::isKeyPressed(sf::Keyboard::D))
					view.move(10, 0);

				// Camera zooming
				if (sf::Keyboard::isKeyPressed(sf::Keyboard::Key::Q))
				{
					view.zoom(1.1f);
				}
				else if (sf::Keyboard::isKeyPressed(sf::Keyboard::Key::E))
				{
					view.zoom(0.9);
				}
			}
		}

		window.clear();

		/* Population map */
		if (drawPopMap)
		{
			int numOfPoints = winSize*winSize;
			sf::VertexArray popPoints(sf::Points, numOfPoints);
			int counter = 0;
			int x = 0, y = 0;
			for (auto vec : popMap)
			{
				for (auto val : vec)
				{
					// Position
					popPoints[counter].position = sf::Vector2f(x, y);

					// Color
					sf::Color pointColour = sf::Color::White;
					float total = highestVal;
					float current = val;

					// How much percentage of the highest value is the current value
					float percentage = (current * 100) / total;

					// What is the alpha value of a single percent
					float singlePercentColour = 255 / 100;

					// How much must we therefore fill in this pixel for
					float fillAmount = singlePercentColour * percentage;

					pointColour.a = fillAmount;
					popPoints[counter].color = pointColour;

					counter++;
					x++;
				}
				y++;
				x = 0;
			}
			window.draw(popPoints);
		}

		/* Terrain (Heightmap) */
		if (drawHeightMap)
		{
			int numOfVerts = terrain.Points().size() * terrain.Points()[0].size();
			sf::VertexArray vertPoints(sf::Points, numOfVerts);
			int counter = 0;
			for (auto vec : terrain.Points())
			{
				for (auto v : vec)
				{
					// Position
					vertPoints[counter].position = sf::Vector2f(v->x, v->y);

					// Color
					sf::Color shapeColor = sf::Color::White;
					int total = terrain.Highest();
					float current = v->z;
					current += (abs(terrain.Lowest()));
					float percentage = (current * 100) / total;
					float singlePercent = 255 / 100;
					float fillAmount = singlePercent * percentage;
					shapeColor.a = fillAmount;
					vertPoints[counter].color = shapeColor;

					counter++;
				}
			}
			window.draw(vertPoints);
		}

		/* Water boundary map */
		if (drawWaterBoundaryMap)
		{
			int numOfPoints = winSize*winSize;
			sf::VertexArray waterPoints(sf::Points, numOfPoints);
			int counter = 0;
			for (int y = 0; y < winSize; y++)
			{
				for (int x = 0; x < winSize; x++)
				{
					waterPoints[counter].position = sf::Vector2f(x, y);
					waterPoints[counter].color = (waterData.IsWater(x, y)) ? sf::Color::White : sf::Color(0, 0, 0, 0);
					counter++;
				}
			}

			window.draw(waterPoints);

			//sf::VertexArray verts;
			//for (int y = 0; y < winSize; y++)
			//{
			//	for (int x = 0; x < winSize; x++)
			//	{
			//		if (waterData.IsWater(x, y))
			//		{
			//			verts.append(sf::Vertex(sf::Vector2f(x, y), sf::Color::White));
			//		}
			//		else
			//		{
			//			verts.append(sf::Vertex(sf::Vector2f(x, y), sf::Color::Black));
			//		}
			//	}
			//}
			//window.draw(verts);

			// window.draw(waterMapSprite);
		}

		/* Quad tree */
		if (drawQuadTree)
		{
			for (auto quad : qt->GetHead()->GetTreeChildren())
			{
				sf::Color colour;
				switch (quad->population)
				{
				case QuadPopulation::high:
					colour = sf::Color::Blue;
					break;
				case QuadPopulation::med:
					colour = sf::Color::Red;
					break;
				case QuadPopulation::low:
					colour = sf::Color::Yellow;
					break;
				}

				sf::Vertex vertices[5] =
				{
					sf::Vertex(sf::Vector2f(quad->xOrigin, quad->yOrigin), colour),
					sf::Vertex(sf::Vector2f(quad->xOrigin, quad->yOrigin + quad->height), colour),
					sf::Vertex(sf::Vector2f(quad->xOrigin + quad->width, quad->yOrigin + quad->height), colour),
					sf::Vertex(sf::Vector2f(quad->xOrigin + quad->width, quad->yOrigin), colour),
					sf::Vertex(sf::Vector2f(quad->xOrigin, quad->yOrigin), colour)
				};

				window.draw(vertices, 5, sf::LineStrip);
			}
		}

		/* Major roads */
		if (drawRoads)
		{
			for (Road* road : majorRoads)
			{
				// Draw each segment
				for (Road* segment : road->segments)
				{
					sf::Vertex vertices[2] =
					{
						sf::Vertex(sf::Vector2f(segment->start->x, segment->start->y), sf::Color(255, 0, 255)),
						sf::Vertex(sf::Vector2f(segment->end->x, segment->end->y), sf::Color(255, 0, 255))
					};
					window.draw(vertices, 2, sf::LinesStrip);
				}

				// Draw the road
				//sf::VertexArray roadVertices(sf::LineStrip, road->nodes.size());
				//int nodeCounter = 0;
				//for (RoadNode* node : road->nodes)
				//{
				//	roadVertices[nodeCounter].position = sf::Vector2f(node->position->x, node->position->y);
				//	roadVertices[nodeCounter].color = sf::Color(255, 0, 255);
				//	nodeCounter++;
				//}
				//window.draw(roadVertices);

				// Draw building lots
				if (road->lots.size() > 0)
				{
					if (drawBuildingLots)
					{
						for (BuildingLot* lot : road->lots)
						{
							sf::Vertex vertices[5] =
							{
								sf::Vertex(sf::Vector2f(lot->bottomLeft->x, lot->bottomLeft->y), sf::Color::White),
								sf::Vertex(sf::Vector2f(lot->topLeft->x, lot->topLeft->y), sf::Color::White),
								sf::Vertex(sf::Vector2f(lot->topRight->x, lot->topRight->y), sf::Color::White),
								sf::Vertex(sf::Vector2f(lot->bottomRight->x, lot->bottomRight->y), sf::Color::White),
								sf::Vertex(sf::Vector2f(lot->bottomLeft->x, lot->bottomLeft->y), sf::Color::White)
							};

							window.draw(vertices, 5, sf::LineStrip);
						}
					}
				}
			}
		}

		/* Minor roads */
		if (drawMinorRoads)
		{
			for (Road* road : minorRoads)
			{
				// Draw the road
				sf::VertexArray roadVertices(sf::LineStrip, 2);

				roadVertices[0].position = sf::Vector2f(road->start->x, road->start->y);
				roadVertices[0].color = sf::Color(255, 0, 0, 100);

				roadVertices[1].position = sf::Vector2f(road->end->x, road->end->y);
				roadVertices[1].color = sf::Color(255, 0, 0, 100);

				window.draw(roadVertices);

				if (road->lots.size() > 0)
				{
					if (drawBuildingLots)
					{
						for (BuildingLot* lot : road->lots)
						{
							sf::Vertex vertices[5] =
							{
								sf::Vertex(sf::Vector2f(lot->bottomLeft->x, lot->bottomLeft->y), sf::Color::White),
								sf::Vertex(sf::Vector2f(lot->topLeft->x, lot->topLeft->y), sf::Color::White),
								sf::Vertex(sf::Vector2f(lot->topRight->x, lot->topRight->y), sf::Color::White),
								sf::Vertex(sf::Vector2f(lot->bottomRight->x, lot->bottomRight->y), sf::Color::White),
								sf::Vertex(sf::Vector2f(lot->bottomLeft->x, lot->bottomLeft->y), sf::Color::White)
							};

							window.draw(vertices, 5, sf::LineStrip);
						}
					}
				}
			}
		}

		/* Minor road expansion points */
		if (drawVoronoiPointsForMinorRoads)
		{
			for (auto point : voronoiPoints)
			{
				sf::CircleShape shape;
				shape.setPosition(point.x(), point.y());
				shape.setFillColor(sf::Color::Cyan);
				shape.setRadius(1);
				window.draw(shape);
			}

			//for (V2* point : expandPoints)
			//{
			//	sf::CircleShape shape;
			//	shape.setPosition(point->x, point->y);
			//	shape.setFillColor(sf::Color::Cyan);
			//	shape.setRadius(2);
			//	window.draw(shape);
			//}
		}

		/* MST */
		if (drawMST)
		{
			for (Edge edge : mst.GetTreeEdges())
			{
				sf::Vertex roadVerts[2] =
				{
					sf::Vertex(sf::Vector2f(edge.start->position->x, edge.start->position->y), sf::Color::Blue),
					sf::Vertex(sf::Vector2f(edge.end->position->x, edge.end->position->y), sf::Color::Blue)
				};

				window.draw(roadVerts, 2, sf::Lines);
			}
		}

		/* Points */
		if (drawMstNodes)
		{
			for (auto node : mst.GetNodes())
			{
				sf::CircleShape shape;
				shape.setPosition(node->position->x, node->position->y);
				shape.setFillColor(sf::Color::Green);
				shape.setRadius(1);

				window.draw(shape);
			}
		}

		/* Floor plans, buildings */
		if (drawBuildings)
		{
			for (Road* road : majorRoads)
			{
				for (BuildingLot* lot : road->lots)
				{
					// lot->fp->bb->DrawSelf(&window);
					lot->fp->DrawPerimeter(&window);
				}
			}
		}

		window.display();
	}

	return 0;
}


#include <opencv2/opencv.hpp>
#include <opencv2/ximgproc.hpp>
#include "opencv2/highgui.hpp"
#include "opencv2/core/utility.hpp"
#include <ctime>
#include "core.h"
#include "boost/filesystem.hpp"   // includes all needed Boost.Filesystem declarations
#include <boost/program_options.hpp>
#include <boost/algorithm/string/replace.hpp>
#include <iostream>               // for std::cout
#include <Eigen/Core>

using namespace boost::filesystem;
using namespace boost::program_options;

using namespace cv;
using namespace cv::ximgproc;

int main(int argc, char** argv )
{
    Eigen::initParallel();
    int nthreads = Eigen::nbThreads( );

    options_description desc{"Options"};
    desc.add_options()
            ("help,h", "Help screen")
            ("path_to_data", value<std::string>()->default_value("./"), "Path to dataset")
            ("path_to_model", value<std::string>()->default_value("./model.yml.gz"), "Path to model")
            ("extension", value<std::string>()->default_value(".jpg"), "File extension")
            ("s1", value<float>()->default_value(1.0), "Sigma1 (RGB)")
            ("s2", value<float>()->default_value(0.01), "Sigma2 (edges)")
            ("thresh", value<float>()->default_value(1.5), "Threshold");

    variables_map vm;
    store(parse_command_line(argc, argv, desc), vm);
    notify(vm);

    if (vm.count("help"))
        std::cout << desc << '\n';

    path p (vm["path_to_data"].as<std::string>()+"/images/");

    directory_iterator end_itr;

    std::vector<std::string> fileList;
    for (directory_iterator itr(p); itr != end_itr; ++itr)
    {
        if (is_regular_file(itr->path())) {
            std::string current_file = itr->path().string();
            if(extension(current_file)==vm["extension"].as<std::string>()){
                fileList.push_back(current_file);
            }

        }
    }

    float sigma1 = vm["s1"].as<float>();
    float sigma2 = vm["s2"].as<float>();
    float thresh = vm["thresh"].as<float>();

    cv::Ptr<StructuredEdgeDetection> pDollar = createStructuredEdgeDetection(vm["path_to_model"].as<std::string>());

    #pragma omp parallel for
    for(int f=0; f<fileList.size(); f++){
        clock_t begin = clock();

        std::string current_imagename = fileList[f];
        std::string current_posename = fileList[f];
        boost::replace_all(current_posename, vm["extension"].as<std::string>(), ".png");
        boost::replace_all(current_posename, "images", "poseimgs");

        Mat image, poseImage;
        image = imread( current_imagename, 1 );
        image.convertTo(image, cv::DataType<float>::type, 1/255.0);
        var_t *imgData = (var_t*)(image.data);

        poseImage = imread( current_posename, 0 );
        poseImage.convertTo(poseImage, cv::DataType<float>::type);
        var_t *poseData = (var_t*)(poseImage.data);

        Mat img2;
        image.copyTo(img2);
        img2.convertTo(img2, cv::DataType<var_t>::type);
        cv::Mat edges(img2.size(), img2.type());

        pDollar->detectEdges(img2, edges);

        int height = image.rows;
        int width = image.cols;

        var_t *edgesData = (var_t*)(edges.data);
        var_t* output = segmentFromPoses(imgData, edgesData, poseData, height, width, sigma1, sigma2);

        Mat outimg(height, width, CV_8U);
        for(int i=0; i<height; i++) {
            for (int j = 0; j < width; j++) {
                if(output[i*width+j] > thresh)
                    outimg.at<uchar>(i,j) = 255;
                else
                    outimg.at<uchar>(i,j) = 0;

            }
        }

        std::string outname = fileList[f];
        boost::replace_all(outname, vm["extension"].as<std::string>(), ".png");
        boost::replace_all(outname, "images", "pose_masks");
        imwrite(outname, outimg);
	std::cout << "write " << outname << "\n";

//        clock_t end = clock();
//        double elapsed_secs = double(end - begin) / CLOCKS_PER_SEC;
//        std::cout<<"Total time: "<<elapsed_secs<<std::endl;
//        namedWindow("Display Image", WINDOW_AUTOSIZE );
//        imshow("Display Image", outimg);
//        waitKey(0);

    }

    return 0;
}


// Boost.Geometry

// Copyright (c) 2021, Oracle and/or its affiliates.

// Contributed and/or modified by Vissarion Fysikopoulos, on behalf of Oracle

// Licensed under the Boost Software License version 1.0.
// http://www.boost.org/users/license.html

#ifndef BOOST_GEOMETRY_ALGORITHMS_DETAIL_CLOSEST_POINTS_MULTIPOINT_TO_GEOMETRY_HPP
#define BOOST_GEOMETRY_ALGORITHMS_DETAIL_CLOSEST_POINTS_MULTIPOINT_TO_GEOMETRY_HPP

#include <iterator>

#include <boost/geometry/algorithms/covered_by.hpp>
#include <boost/geometry/algorithms/detail/closest_points/range_to_geometry_rtree.hpp>
#include <boost/geometry/algorithms/detail/closest_points/utilities.hpp>
#include <boost/geometry/algorithms/dispatch/closest_points.hpp>

#include <boost/geometry/core/point_type.hpp>
#include <boost/geometry/core/tags.hpp>

#include <boost/geometry/geometries/linestring.hpp>

namespace boost { namespace geometry
{

#ifndef DOXYGEN_NO_DETAIL
namespace detail { namespace closest_points
{

struct multipoint_to_multipoint
{
    template
    <
        typename MultiPoint1,
        typename MultiPoint2,
        typename Segment,
        typename Strategies
    >
    static inline void apply(MultiPoint1 const& multipoint1,
                             MultiPoint2 const& multipoint2,
                             Segment& shortest_seg,
                             Strategies const& strategies)
    {
        if (boost::size(multipoint1) < boost::size(multipoint2))
        {
            point_or_segment_range_to_geometry_rtree::apply(
                boost::begin(multipoint2),
                boost::end(multipoint2),
                multipoint1,
                shortest_seg,
                strategies);
            detail::closest_points::swap_segment_points::apply(shortest_seg);
            return;
        }
        point_or_segment_range_to_geometry_rtree::apply(
            boost::begin(multipoint1),
            boost::end(multipoint1),
            multipoint2,
            shortest_seg,
            strategies);
    }
};

struct multipoint_to_linear
{
    template
    <
        typename MultiPoint,
        typename Linear,
        typename Segment,
        typename Strategies
    >
    static inline void apply(MultiPoint const& multipoint,
                             Linear const& linear,
                             Segment& shortest_seg,
                             Strategies const& strategies)
    {
        point_or_segment_range_to_geometry_rtree::apply(
            boost::begin(multipoint),
            boost::end(multipoint),
            linear,
            shortest_seg,
            strategies);
    }
};

struct linear_to_multipoint
{
    template
    <
        typename Linear,
        typename MultiPoint,
        typename Segment,
        typename Strategies
    >
    static inline void apply(Linear const& linear,
                             MultiPoint const& multipoint,
                             Segment& shortest_seg,
                             Strategies const& strategies)
    {
        multipoint_to_linear::apply(multipoint, linear, shortest_seg, strategies);
        detail::closest_points::swap_segment_points::apply(shortest_seg);
    }
};

struct segment_to_multipoint
{
    template
    <
        typename Segment,
        typename MultiPoint,
        typename OutSegment,
        typename Strategies
    >
    static inline void apply(Segment const& segment,
                             MultiPoint const& multipoint,
                             OutSegment& shortest_seg,
                             Strategies const& strategies)
    {
        using linestring_type = geometry::model::linestring
            <
                typename point_type<Segment>::type
            >;
        linestring_type linestring;
        convert(segment, linestring);
        multipoint_to_linear::apply(multipoint, linestring, shortest_seg, strategies);
        detail::closest_points::swap_segment_points::apply(shortest_seg);
    }
};

struct multipoint_to_segment
{
    template
    <
        typename MultiPoint,
        typename Segment,
        typename OutSegment,
        typename Strategies
    >
    static inline void apply(MultiPoint const& multipoint,
                             Segment const& segment,
                             OutSegment& shortest_seg,
                             Strategies const& strategies)
    {
        using linestring_type = geometry::model::linestring
            <
                typename point_type<Segment>::type
            >;
        linestring_type linestring;
        convert(segment, linestring);
        multipoint_to_linear::apply(multipoint, linestring, shortest_seg,
            strategies);
    }
};

struct multipoint_to_areal
{

private:

    template <typename Areal, typename Strategies>
    struct covered_by_areal
    {
        covered_by_areal(Areal const& areal, Strategies const& strategy)
            : m_areal(areal), m_strategy(strategy)
        {}

        template <typename Point>
        inline bool operator()(Point const& point) const
        {
            return geometry::covered_by(point, m_areal, m_strategy);
        }

        Areal const& m_areal;
        Strategies const& m_strategy;
    };

public:

    template
    <
        typename MultiPoint,
        typename Areal,
        typename Segment,
        typename Strategies
    >
    static inline void apply(MultiPoint const& multipoint,
                             Areal const& areal,
                             Segment& shortest_seg,
                             Strategies const& strategies)
    {
        covered_by_areal<Areal, Strategies> predicate(areal, strategies);

        auto it = std::find_if(
                boost::begin(multipoint),
                boost::end(multipoint),
                predicate);

        if (it != boost::end(multipoint))
        {
            return set_segment_from_points::apply(*it, *it, shortest_seg);

        }

        point_or_segment_range_to_geometry_rtree::apply(
            boost::begin(multipoint),
            boost::end(multipoint),
            areal,
            shortest_seg,
            strategies);
    }
};

struct areal_to_multipoint
{
    template
    <
        typename Areal,
        typename MultiPoint,
        typename Segment,
        typename Strategies
    >
    static inline void apply(Areal const& areal,
                             MultiPoint const& multipoint,
                             Segment& shortest_seg,
                             Strategies const& strategies)
    {
        multipoint_to_areal::apply(multipoint, areal, shortest_seg, strategies);
        detail::closest_points::swap_segment_points::apply(shortest_seg);
    }
};

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

#ifndef DOXYGEN_NO_DISPATCH
namespace dispatch
{

template <typename MultiPoint1, typename MultiPoint2>
struct closest_points
    <
        MultiPoint1, MultiPoint2,
        multi_point_tag, multi_point_tag,
        false
    > : detail::closest_points::multipoint_to_multipoint
{};

template <typename MultiPoint, typename Linear>
struct closest_points
    <
        MultiPoint, Linear,
        multi_point_tag, linear_tag,
        false
    > : detail::closest_points::multipoint_to_linear
{};

template <typename Linear, typename MultiPoint>
struct closest_points
    <
         Linear, MultiPoint,
         linear_tag, multi_point_tag,
         false
    > : detail::closest_points::linear_to_multipoint
{};

template <typename MultiPoint, typename Segment>
struct closest_points
    <
        MultiPoint, Segment,
        multi_point_tag, segment_tag,
        false
    > : detail::closest_points::multipoint_to_segment
{};

template <typename Segment, typename MultiPoint>
struct closest_points
    <
         Segment, MultiPoint,
         segment_tag, multi_point_tag,
         false
    > : detail::closest_points::segment_to_multipoint
{};

template <typename MultiPoint, typename Areal>
struct closest_points
    <
        MultiPoint, Areal,
        multi_point_tag, areal_tag,
        false
    > : detail::closest_points::multipoint_to_areal
{};

template <typename Areal, typename MultiPoint>
struct closest_points
    <
        Areal, MultiPoint,
        areal_tag, multi_point_tag,
        false
    > : detail::closest_points::areal_to_multipoint
{};

} // namespace dispatch
#endif // DOXYGEN_NO_DISPATCH

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_ALGORITHMS_DETAIL_CLOSEST_POINTS_MULTIPOINT_TO_GEOMETRY_HPP


//
// Copyright (c) 2015-2018 CNRS
// Copyright (c) 2015-2016 Wandercraft, 86 rue de Paris 91400 Orsay, France.
//

#ifndef __pinocchio_joint_revolute_hpp__
#define __pinocchio_joint_revolute_hpp__

#include "pinocchio/math/sincos.hpp"
#include "pinocchio/spatial/inertia.hpp"
#include "pinocchio/multibody/constraint.hpp"
#include "pinocchio/multibody/joint/joint-base.hpp"
#include "pinocchio/spatial/spatial-axis.hpp"
#include "pinocchio/utils/axis-label.hpp"

namespace pinocchio
{

  template<typename Scalar, int Options, int axis> struct MotionRevoluteTpl;

  namespace internal
  {
    template<typename Scalar, int Options, int axis>
    struct SE3GroupAction< MotionRevoluteTpl<Scalar,Options,axis> >
    {
      typedef MotionTpl<Scalar,Options> ReturnType;
    };

    template<typename Scalar, int Options, int axis, typename MotionDerived>
    struct MotionAlgebraAction< MotionRevoluteTpl<Scalar,Options,axis>, MotionDerived>
    {
      typedef MotionTpl<Scalar,Options> ReturnType;
    };
  }

  template<typename _Scalar, int _Options, int axis>
  struct traits< MotionRevoluteTpl<_Scalar,_Options,axis> >
  {
    typedef _Scalar Scalar;
    enum { Options = _Options };
    typedef Eigen::Matrix<Scalar,3,1,Options> Vector3;
    typedef Eigen::Matrix<Scalar,6,1,Options> Vector6;
    typedef Eigen::Matrix<Scalar,6,6,Options> Matrix6;
    typedef typename EIGEN_REF_CONSTTYPE(Vector6) ToVectorConstReturnType;
    typedef typename EIGEN_REF_TYPE(Vector6) ToVectorReturnType;
    typedef Vector3 AngularType;
    typedef Vector3 LinearType;
    typedef const Vector3 ConstAngularType;
    typedef const Vector3 ConstLinearType;
    typedef Matrix6 ActionMatrixType;
    typedef MotionTpl<Scalar,Options> MotionPlain;
    enum {
      LINEAR = 0,
      ANGULAR = 3
    };
  }; // traits MotionRevoluteTpl

  template<typename Scalar, int Options, int axis> struct TransformRevoluteTpl;

  template<typename _Scalar, int _Options, int _axis>
  struct traits< TransformRevoluteTpl<_Scalar,_Options,_axis> >
  {
    enum {
      axis = _axis,
      Options = _Options,
      LINEAR = 0,
      ANGULAR = 3
    };
    typedef _Scalar Scalar;
    typedef SE3Tpl<Scalar,Options> PlainType;
    typedef Eigen::Matrix<Scalar,3,1,Options> Vector3;
    typedef Eigen::Matrix<Scalar,3,3,Options> Matrix3;
    typedef Matrix3 AngularType;
    typedef Matrix3 AngularRef;
    typedef Matrix3 ConstAngularRef;
    typedef typename Vector3::ConstantReturnType LinearType;
    typedef typename Vector3::ConstantReturnType LinearRef;
    typedef const typename Vector3::ConstantReturnType ConstLinearRef;
    typedef typename traits<PlainType>::ActionMatrixType ActionMatrixType;
    typedef typename traits<PlainType>::HomogeneousMatrixType HomogeneousMatrixType;
  }; // traits TransformRevoluteTpl

  namespace internal
  {
    template<typename Scalar, int Options, int axis>
    struct SE3GroupAction< TransformRevoluteTpl<Scalar,Options,axis> >
    { typedef typename traits <TransformRevoluteTpl<Scalar,Options,axis> >::PlainType ReturnType; };
  }

  template<typename _Scalar, int _Options, int axis>
  struct TransformRevoluteTpl : SE3Base< TransformRevoluteTpl<_Scalar,_Options,axis> >
  {
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    SE3_TYPEDEF_TPL(TransformRevoluteTpl);
    typedef typename traits<TransformRevoluteTpl>::PlainType PlainType;

    TransformRevoluteTpl() {}
    TransformRevoluteTpl(const Scalar & sin, const Scalar & cos)
    : m_sin(sin), m_cos(cos)
    {}

    PlainType plain() const
    {
      PlainType res(PlainType::Identity());
      _setRotation (res.rotation());
      return res;
    }

    operator PlainType() const { return plain(); }

    template<typename S2, int O2>
    typename internal::SE3GroupAction<TransformRevoluteTpl>::ReturnType
    se3action(const SE3Tpl<S2,O2> & m) const
    {
      typedef typename internal::SE3GroupAction<TransformRevoluteTpl>::ReturnType ReturnType;
      ReturnType res;
      switch(axis)
      {
        case 0:
        {
          res.rotation().col(0) = m.rotation().col(0);
          res.rotation().col(1).noalias() = m_cos * m.rotation().col(1) + m_sin * m.rotation().col(2);
          res.rotation().col(2).noalias() = res.rotation().col(0).cross(res.rotation().col(1));
          break;
        }
        case 1:
        {
          res.rotation().col(1) = m.rotation().col(1);
          res.rotation().col(2).noalias() = m_cos * m.rotation().col(2) + m_sin * m.rotation().col(0);
          res.rotation().col(0).noalias() = res.rotation().col(1).cross(res.rotation().col(2));
          break;
        }
        case 2:
        {
          res.rotation().col(2) = m.rotation().col(2);
          res.rotation().col(0).noalias() = m_cos * m.rotation().col(0) + m_sin * m.rotation().col(1);
          res.rotation().col(1).noalias() = res.rotation().col(2).cross(res.rotation().col(0));
          break;
        }
        default:
        {
          assert(false && "must nerver happened");
          break;
        }
      }
      res.translation() = m.translation();
      return res;
    }

    const Scalar & sin() const { return m_sin; }
    const Scalar & cos() const { return m_cos; }

    template<typename OtherScalar>
    void setValues(const OtherScalar & sin, const OtherScalar & cos)
    { m_sin = sin; m_cos = cos; }

    LinearType translation() const { return LinearType::Zero(3); };
    AngularType rotation() const {
      AngularType m(AngularType::Identity(3));
      _setRotation (m);
      return m;
    }

  protected:

    Scalar m_sin, m_cos;
    inline void _setRotation (typename PlainType::AngularRef& rot) const
    {
      switch(axis)
      {
        case 0:
        {
          rot.coeffRef(1,1) = m_cos; rot.coeffRef(1,2) = -m_sin;
          rot.coeffRef(2,1) = m_sin; rot.coeffRef(2,2) =  m_cos;
          break;
        }
        case 1:
        {
          rot.coeffRef(0,0) =  m_cos; rot.coeffRef(0,2) = m_sin;
          rot.coeffRef(2,0) = -m_sin; rot.coeffRef(2,2) = m_cos;
          break;
        }
        case 2:
        {
          rot.coeffRef(0,0) = m_cos; rot.coeffRef(0,1) = -m_sin;
          rot.coeffRef(1,0) = m_sin; rot.coeffRef(1,1) =  m_cos;
          break;
        }
        default:
        {
          assert(false && "must nerver happened");
          break;
        }
      }
    }
  };

  template<typename _Scalar, int _Options, int axis>
  struct MotionRevoluteTpl : MotionBase< MotionRevoluteTpl<_Scalar,_Options,axis> >
  {
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    MOTION_TYPEDEF_TPL(MotionRevoluteTpl);
    typedef SpatialAxis<axis+ANGULAR> Axis;
    typedef typename Axis::CartesianAxis3 CartesianAxis3;

    MotionRevoluteTpl() {}

    template<typename OtherScalar>
    MotionRevoluteTpl(const OtherScalar & w) : w(w)  {}

//    operator MotionPlain() const { return Axis() * w; }

    template<typename MotionDerived>
    void setTo(MotionDense<MotionDerived> & m) const
    {
      m.linear().setZero();
      for(Eigen::DenseIndex k = 0; k < 3; ++k)
        m.angular()[k] = k == axis ? w : (Scalar)0;
    }

    template<typename MotionDerived>
    void addTo(MotionDense<MotionDerived> & v) const
    {
      typedef typename MotionDense<MotionDerived>::Scalar OtherScalar;
      v.angular()[axis] += (OtherScalar)w;
    }

    template<typename S2, int O2, typename D2>
    void se3Action_impl(const SE3Tpl<S2,O2> & m, MotionDense<D2> & v) const
    {
      v.angular().noalias() = m.rotation().col(axis) * w;
      v.linear().noalias() = m.translation().cross(v.angular());
    }

    template<typename S2, int O2>
    MotionPlain se3Action_impl(const SE3Tpl<S2,O2> & m) const
    {
      MotionPlain res;
      se3Action_impl(m,res);
      return res;
    }

    template<typename S2, int O2, typename D2>
    void se3ActionInverse_impl(const SE3Tpl<S2,O2> & m, MotionDense<D2> & v) const
    {
      // Linear
      // TODO: use v.angular() as temporary variable
      Vector3 v3_tmp;
      CartesianAxis3::alphaCross(w,m.translation(),v3_tmp);
      v.linear().noalias() = m.rotation().transpose() * v3_tmp;

      // Angular
      v.angular().noalias() = m.rotation().transpose().col(axis) * w;
    }

    template<typename S2, int O2>
    MotionPlain se3ActionInverse_impl(const SE3Tpl<S2,O2> & m) const
    {
      MotionPlain res;
      se3ActionInverse_impl(m,res);
      return res;
    }

    template<typename M1, typename M2>
    EIGEN_STRONG_INLINE
    void motionAction(const MotionDense<M1> & v, MotionDense<M2> & mout) const
    {
      // Linear
      CartesianAxis3::alphaCross(-w,v.linear(),mout.linear());

      // Angular
      CartesianAxis3::alphaCross(-w,v.angular(),mout.angular());
    }

    template<typename M1>
    MotionPlain motionAction(const MotionDense<M1> & v) const
    {
      MotionPlain res;
      motionAction(v,res);
      return res;
    }

    // data
    Scalar w;
  }; // struct MotionRevoluteTpl

  template<typename S1, int O1, int axis, typename MotionDerived>
  typename MotionDerived::MotionPlain
  operator+(const MotionRevoluteTpl<S1,O1,axis> & m1,
            const MotionDense<MotionDerived> & m2)
  {
    typename MotionDerived::MotionPlain res(m2);
    res += m1;
    return res;
  }

  template<typename MotionDerived, typename S2, int O2, int axis>
  EIGEN_STRONG_INLINE
  typename MotionDerived::MotionPlain
  operator^(const MotionDense<MotionDerived> & m1, const MotionRevoluteTpl<S2,O2,axis>& m2)
  {
    return m2.motionAction(m1);
  }

  template<typename Scalar, int Options, int axis> struct ConstraintRevoluteTpl;

  namespace internal
  {
    template<typename Scalar, int Options, int axis>
    struct SE3GroupAction< ConstraintRevoluteTpl<Scalar,Options,axis> >
    { typedef Eigen::Matrix<Scalar,6,1,Options> ReturnType; };

    template<typename Scalar, int Options, int axis, typename MotionDerived>
    struct MotionAlgebraAction< ConstraintRevoluteTpl<Scalar,Options,axis>, MotionDerived >
    { typedef Eigen::Matrix<Scalar,6,1,Options> ReturnType; };
  }

  template<typename _Scalar, int _Options, int axis>
  struct traits< ConstraintRevoluteTpl<_Scalar,_Options,axis> >
  {
    typedef _Scalar Scalar;
    enum { Options = _Options };
    typedef Eigen::Matrix<Scalar,3,1,Options> Vector3;
    typedef Eigen::Matrix<Scalar,4,1,Options> Vector4;
    typedef Eigen::Matrix<Scalar,6,1,Options> Vector6;
    typedef Eigen::Matrix<Scalar,3,3,Options> Matrix3;
    typedef Eigen::Matrix<Scalar,4,4,Options> Matrix4;
    typedef Eigen::Matrix<Scalar,6,6,Options> Matrix6;
    typedef Matrix3 Angular_t;
    typedef Vector3 Linear_t;
    typedef const Matrix3 ConstAngular_t;
    typedef const Vector3 ConstLinear_t;
    typedef Matrix6 ActionMatrix_t;
    typedef Eigen::Quaternion<Scalar,0> Quaternion_t;
    typedef SE3Tpl<Scalar,Options> SE3;
    typedef ForceTpl<Scalar,Options> Force;
    typedef MotionTpl<Scalar,Options> Motion;
    typedef Symmetric3Tpl<Scalar,Options> Symmetric3;
    enum {
      LINEAR = 0,
      ANGULAR = 3
    };
    typedef MotionRevoluteTpl<Scalar,Options,axis> JointMotion;
    typedef Eigen::Matrix<Scalar,1,1,Options> JointForce;
    typedef Eigen::Matrix<Scalar,6,1,Options> DenseBase;
    typedef DenseBase MatrixReturnType;
    typedef const DenseBase ConstMatrixReturnType;
  }; // traits ConstraintRevoluteTpl

  template<typename _Scalar, int _Options, int axis>
  struct ConstraintRevoluteTpl
  : ConstraintBase< ConstraintRevoluteTpl<_Scalar,_Options,axis> >
  {
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    SPATIAL_TYPEDEF_TEMPLATE(ConstraintRevoluteTpl);
    enum { NV = 1, Options = _Options };

    typedef typename traits<ConstraintRevoluteTpl>::JointMotion JointMotion;
    typedef typename traits<ConstraintRevoluteTpl>::JointForce JointForce;
    typedef typename traits<ConstraintRevoluteTpl>::DenseBase DenseBase;
    typedef SpatialAxis<ANGULAR+axis> Axis;

    template<typename Vector1Like>
    JointMotion __mult__(const Eigen::MatrixBase<Vector1Like> & v) const
    { return JointMotion(v[0]); }

    template<typename S1, int O1>
    Eigen::Matrix<Scalar,6,1,Options>
    se3Action(const SE3Tpl<S1,O1> & m) const
    {
      Eigen::Matrix<Scalar,6,1,Options> res;
      res.template head<3>() = m.translation().cross(m.rotation().col(axis));
      res.template tail<3>() = m.rotation().col(axis);
      return res;
    }

    int nv_impl() const { return NV; }

    struct TransposeConst
    {
      const ConstraintRevoluteTpl & ref;
      TransposeConst(const ConstraintRevoluteTpl & ref) : ref(ref) {}

      template<typename Derived>
      typename ForceDense<Derived>::ConstAngularType::template ConstFixedSegmentReturnType<1>::Type
      operator* (const ForceDense<Derived> & f) const
      { return f.angular().template segment<1>(axis); }

        /// [CRBA]  MatrixBase operator* (Constraint::Transpose S, ForceSet::Block)
      template<typename D>
      typename Eigen::MatrixBase<D>::ConstRowXpr
      operator*(const Eigen::MatrixBase<D> & F) const
      {
        assert(F.rows()==6);
        return F.row(ANGULAR + axis);
      }
    }; // struct TransposeConst

    TransposeConst transpose() const { return TransposeConst(*this); }

    /* CRBA joint operators
     *   - ForceSet::Block = ForceSet
     *   - ForceSet operator* (Inertia Y,Constraint S)
     *   - MatrixBase operator* (Constraint::Transpose S, ForceSet::Block)
     *   - SE3::act(ForceSet::Block)
     */
     DenseBase matrix_impl() const
     {
       DenseBase S;
       MotionRef<DenseBase> v(S);
       v << Axis();
      return S;
    }

    template<typename MotionDerived>
    DenseBase motionAction(const MotionDense<MotionDerived> & m) const
    {
      DenseBase res;
      MotionRef<DenseBase> v(res);
      v = m.cross(Axis());
      return res;
    }
  }; // struct ConstraintRevoluteTpl

  template<typename _Scalar, int _Options, int _axis>
  struct JointRevoluteTpl
  {
    typedef _Scalar Scalar;

    enum
    {
      Options = _Options,
      axis = _axis
    };
  };

  /* [CRBA] ForceSet operator* (Inertia Y,Constraint S) */
  template<typename S1, int O1, typename S2, int O2>
  inline Eigen::Matrix<S2,6,1,O2>
  operator*(const InertiaTpl<S1,O1> & Y,const ConstraintRevoluteTpl<S2,O2,0> &)
  {
    typedef InertiaTpl<S1,O1> Inertia;
    /* Y(:,3) = ( 0,-z, y,  I00+yy+zz,  I01-xy   ,  I02-xz   ) */
    const S1
    &m = Y.mass(),
    &x = Y.lever()[0],
    &y = Y.lever()[1],
    &z = Y.lever()[2];
    const typename Inertia::Symmetric3 & I = Y.inertia();

    Eigen::Matrix<S2,6,1,O2> res;
    res << (S2)0,-m*z,m*y,
    I(0,0)+m*(y*y+z*z),
    I(0,1)-m*x*y,
    I(0,2)-m*x*z ;
    return res;
  }
  /* [CRBA] ForceSet operator* (Inertia Y,Constraint S) */
  template<typename S1, int O1, typename S2, int O2>
  inline Eigen::Matrix<S2,6,1,O2>
  operator*(const InertiaTpl<S1,O1> & Y,const ConstraintRevoluteTpl<S2,O2,1> &)
  {
    typedef InertiaTpl<S1,O1> Inertia;
    /* Y(:,4) = ( z, 0,-x,  I10-xy   ,  I11+xx+zz,  I12-yz   ) */
    const S1
    &m = Y.mass(),
    &x = Y.lever()[0],
    &y = Y.lever()[1],
    &z = Y.lever()[2];
    const typename Inertia::Symmetric3 & I = Y.inertia();
    Eigen::Matrix<S2,6,1,O2> res;
    res << m*z,(S2)0,-m*x,
    I(1,0)-m*x*y,
    I(1,1)+m*(x*x+z*z),
    I(1,2)-m*y*z ;
    return res;
  }
  /* [CRBA] ForceSet operator* (Inertia Y,Constraint S) */
  template<typename S1, int O1, typename S2, int O2>
  inline Eigen::Matrix<S2,6,1,O2>
  operator*(const InertiaTpl<S1,O1> & Y,const ConstraintRevoluteTpl<S2,O2,2> &)
  {
    typedef InertiaTpl<S1,O1> Inertia;
    /* Y(:,5) = (-y, x, 0,  I20-xz   ,  I21-yz   ,  I22+xx+yy) */
    const S1
    &m = Y.mass(),
    &x = Y.lever()[0],
    &y = Y.lever()[1],
    &z = Y.lever()[2];
    const typename Inertia::Symmetric3 & I = Y.inertia();
    Eigen::Matrix<S2,6,1,O2> res; res << -m*y,m*x,(S2)0,
    I(2,0)-m*x*z,
    I(2,1)-m*y*z,
    I(2,2)+m*(x*x+y*y) ;
    return res;
  }

  /* [ABA] I*S operator (Inertia Y,Constraint S) */
  template<typename M6Like, typename S2, int O2,int axis>
  inline const typename M6Like::ConstColXpr
  operator*(const Eigen::MatrixBase<M6Like> & Y, const ConstraintRevoluteTpl<S2,O2,axis> &)
  {
    EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(M6Like,6,6);
    return Y.col(Inertia::ANGULAR + axis);
  }

  template<typename _Scalar, int _Options, int axis>
  struct traits< JointRevoluteTpl<_Scalar,_Options,axis> >
  {
    enum {
      NQ = 1,
      NV = 1
    };
    typedef _Scalar Scalar;
    enum { Options = _Options };
    typedef JointDataRevoluteTpl<Scalar,Options,axis> JointDataDerived;
    typedef JointModelRevoluteTpl<Scalar,Options,axis> JointModelDerived;
    typedef ConstraintRevoluteTpl<Scalar,Options,axis> Constraint_t;
    typedef TransformRevoluteTpl<Scalar,Options,axis> Transformation_t;
    typedef MotionRevoluteTpl<Scalar,Options,axis> Motion_t;
    typedef BiasZeroTpl<Scalar,Options> Bias_t;
    typedef Eigen::Matrix<Scalar,6,NV,Options> F_t;

    // [ABA]
    typedef Eigen::Matrix<Scalar,6,NV,Options> U_t;
    typedef Eigen::Matrix<Scalar,NV,NV,Options> D_t;
    typedef Eigen::Matrix<Scalar,6,NV,Options> UD_t;

    JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE

    typedef Eigen::Matrix<Scalar,NQ,1,Options> ConfigVector_t;
    typedef Eigen::Matrix<Scalar,NV,1,Options> TangentVector_t;
  };

  template<typename Scalar, int Options, int axis>
  struct traits< JointDataRevoluteTpl<Scalar,Options,axis> >
  { typedef JointRevoluteTpl<Scalar,Options,axis> JointDerived; };

  template<typename Scalar, int Options, int axis>
  struct traits< JointModelRevoluteTpl<Scalar,Options,axis> >
  { typedef JointRevoluteTpl<Scalar,Options,axis> JointDerived; };

  template<typename _Scalar, int _Options, int axis>
  struct JointDataRevoluteTpl : public JointDataBase< JointDataRevoluteTpl<_Scalar,_Options,axis> >
  {
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    typedef JointRevoluteTpl<_Scalar,_Options,axis> JointDerived;
    PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE;
    JOINT_DATA_BASE_DEFAULT_ACCESSOR

    Constraint_t S;
    Transformation_t M;
    Motion_t v;
    Bias_t c;
    F_t F;

    // [ABA] specific data
    U_t U;
    D_t Dinv;
    UD_t UDinv;

    JointDataRevoluteTpl() {}

  }; // struct JointDataRevoluteTpl

  template<typename NewScalar, typename Scalar, int Options, int axis>
  struct CastType< NewScalar, JointModelRevoluteTpl<Scalar,Options,axis> >
  {
    typedef JointModelRevoluteTpl<NewScalar,Options,axis> type;
  };

  template<typename _Scalar, int _Options, int axis>
  struct JointModelRevoluteTpl : public JointModelBase< JointModelRevoluteTpl<_Scalar,_Options,axis> >
  {
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    typedef JointRevoluteTpl<_Scalar,_Options,axis> JointDerived;
    SE3_JOINT_TYPEDEF_TEMPLATE;

    typedef JointModelBase<JointModelRevoluteTpl> Base;
    using Base::id;
    using Base::idx_q;
    using Base::idx_v;
    using Base::setIndexes;

    JointDataDerived createData() const { return JointDataDerived(); }

    template<typename ConfigVector>
    EIGEN_DONT_INLINE
    void calc(JointDataDerived & data,
              const typename Eigen::MatrixBase<ConfigVector> & qs) const
    {
      typedef typename ConfigVector::Scalar OtherScalar;

      const OtherScalar & q = qs[idx_q()];
      OtherScalar ca,sa; SINCOS(q,&sa,&ca);
      data.M.setValues(sa,ca);
    }

    template<typename ConfigVector, typename TangentVector>
    EIGEN_DONT_INLINE
    void calc(JointDataDerived & data,
              const typename Eigen::MatrixBase<ConfigVector> & qs,
              const typename Eigen::MatrixBase<TangentVector> & vs) const
    {
      calc(data,qs.derived());

      data.v.w = (Scalar)vs[idx_v()];;
    }

    template<typename Matrix6Like>
    void calc_aba(JointDataDerived & data, const Eigen::MatrixBase<Matrix6Like> & I, const bool update_I) const
    {
      data.U = I.col(Inertia::ANGULAR + axis);
      data.Dinv[0] = Scalar(1)/I(Inertia::ANGULAR + axis,Inertia::ANGULAR + axis);
      data.UDinv.noalias() = data.U * data.Dinv[0];

      if (update_I)
        EIGEN_CONST_CAST(Matrix6Like,I) -= data.UDinv * data.U.transpose();
    }

    Scalar finiteDifferenceIncrement() const
    {
      using math::sqrt;
      return 2.*sqrt(sqrt(Eigen::NumTraits<Scalar>::epsilon()));
    }

    static std::string classname()
    {
      return std::string("JointModelR") + axisLabel<axis>();
    }
    std::string shortname() const { return classname(); }

    /// \returns An expression of *this with the Scalar type casted to NewScalar.
    template<typename NewScalar>
    JointModelRevoluteTpl<NewScalar,Options,axis> cast() const
    {
      typedef JointModelRevoluteTpl<NewScalar,Options,axis> ReturnType;
      ReturnType res;
      res.setIndexes(id(),idx_q(),idx_v());
      return res;
    }

  }; // struct JointModelRevoluteTpl

  typedef JointRevoluteTpl<double,0,0> JointRX;
  typedef JointDataRevoluteTpl<double,0,0> JointDataRX;
  typedef JointModelRevoluteTpl<double,0,0> JointModelRX;

  typedef JointRevoluteTpl<double,0,1> JointRY;
  typedef JointDataRevoluteTpl<double,0,1> JointDataRY;
  typedef JointModelRevoluteTpl<double,0,1> JointModelRY;

  typedef JointRevoluteTpl<double,0,2> JointRZ;
  typedef JointDataRevoluteTpl<double,0,2> JointDataRZ;
  typedef JointModelRevoluteTpl<double,0,2> JointModelRZ;

} //namespace pinocchio

#include <boost/type_traits.hpp>

namespace boost
{
  template<typename Scalar, int Options, int axis>
  struct has_nothrow_constructor< ::pinocchio::JointModelRevoluteTpl<Scalar,Options,axis> >
  : public integral_constant<bool,true> {};

  template<typename Scalar, int Options, int axis>
  struct has_nothrow_copy< ::pinocchio::JointModelRevoluteTpl<Scalar,Options,axis> >
  : public integral_constant<bool,true> {};

  template<typename Scalar, int Options, int axis>
  struct has_nothrow_constructor< ::pinocchio::JointDataRevoluteTpl<Scalar,Options,axis> >
  : public integral_constant<bool,true> {};

  template<typename Scalar, int Options, int axis>
  struct has_nothrow_copy< ::pinocchio::JointDataRevoluteTpl<Scalar,Options,axis> >
  : public integral_constant<bool,true> {};
}

#endif // ifndef __pinocchio_joint_revolute_hpp__


/* Copyright (C) 2010 Ion Torrent Systems, Inc. All Rights Reserved */

#include <stdio.h>
#include <string.h>
#include <errno.h>
#include <armadillo>
#include <fenv.h> // Floating point exceptions

#include "CommandLineOpts.h"
#include "OptBase.h"
#include "Mask.h"
#include "Region.h"
#include "SeqList.h"
#include "TrackProgress.h"
#include "SlicedPrequel.h"
#include "SeparatorInterface.h"
#include "SetUpForProcessing.h"

#include "IonErr.h"
#include "ImageSpecClass.h"
#include "MaskFunctions.h"

void PrintHelp()
{
    printf ("\n");
    printf ("Usage: justBeadFind [options] [dat-source-directory]\n");
    printf ("\n");
	printf ("[dat-source-directory]: if there is no option --dat-source-directory, this directory string must be the last one in the commandline\n");
    printf ("General options:(VECTOR input must be comma separated string)\n");
    printf ("  -h,--help                                    print this help message and exit\n");
    printf ("  -v,--version                                 print version and exit\n");
    printf ("     --region-list           INT VECTOR OF 4   region list for SlicedPrequel setup []\n");
	printf ("\n");

	CommandLineOpts cmdOpts;
	cmdOpts.PrintHelp();

    printf ("\n");
    exit (EXIT_SUCCESS);
}

void DumpStartingStateOfProgram (int argc, char *argv[], TrackProgress &my_progress)
{
  char myHostName[128] = { 0 };
  gethostname (myHostName, 128);
  fprintf (stdout, "\n");
  fprintf (stdout, "Hostname = %s\n", myHostName);
  fprintf (stdout, "Start Time = %s", ctime (&my_progress.analysis_start_time));
  fprintf (stdout, "Version = %s.%s (%s) (%s)\n",
           IonVersion::GetVersion().c_str(), IonVersion::GetRelease().c_str(),
           IonVersion::GetGitHash().c_str(), IonVersion::GetBuildNum().c_str());
  fprintf (stdout, "Command line = ");
  for (int i = 0; i < argc; i++)
    fprintf (stdout, "%s ", argv[i]);
  fprintf (stdout, "\n");
  fflush (NULL);
}

void TheSilenceOfTheArmadillos(ofstream &null_ostream)
{
    // Disable armadillo warning messages.
  arma::set_stream_err1(null_ostream);
  arma::set_stream_err2(null_ostream);
}

int TrapAndDeprecateOldArgs(int argc, char *argv[], char *argv2[])
{
  int datind = -1;
  for(int i = 1; i < argc; ++i)
  {
	  string s = argv[i];
	  int index = s.find("--dat-source-directory");
	  if(index == 0)
	  {
		  datind = i;
	  }
  }

  //jz check to see if the last argument is dat_source_directory
  if(datind < 0) // there is no "--dat-source-directory"
  {
	  if(argc > 1) // must have arg
	  {
		  if(argv[argc - 1][0] == '-')
		  {
			  string s = argv[argc - 1];
			  if(s != "-h" && s != "-v" && s != "--help" && s != "--version")
			  {
				delete [] argv2;

				cerr << "ERROR: dat_source_directory must be provided at the end of the command line." << endl;
				exit ( EXIT_FAILURE );
			  }
		  }

		  if(argc != 2)
		  {

			  if(argc == 3) // 2 args
			  {
				  if(argv[argc - 2][0] != '-') // not option + dat_source_directory
				  {
					  delete [] argv2;

					  cerr << "ERROR: both arguments of the command line are not option name. The last argument must be dat_source_directory and the other one must be an option." << endl;
					  exit ( EXIT_FAILURE );
				  }
				  else
				  {
					  string s = argv[argc - 2];
					  int index = s.find("=");
					  if(index < 0)
					  {
						  delete [] argv2;

						  cerr << "ERROR: the last argument must be dat_source_directory and the other one must be an option with assignment value." << endl;
						  exit ( EXIT_FAILURE );
					  }
				  }
			  }
			  else
			  {
				  if(argv[argc - 2][0] == '-')
				  {
					  string s = argv[argc - 2];
					  int index = s.find("=");
					  if(index < 0)
					  {
						  delete [] argv2;

						  cerr << "ERROR: the last argument must be dat_source_directory and the second last one must be an option with assignment value." << endl;
						  exit ( EXIT_FAILURE );
					  }
				  }
			  }
		  }
	  }
  }

  for(int i = 0; i < argc; ++i)
  {
	  size_t slen0 = strlen(argv[i]) + 1;

	  string s0 = argv[i];

	  int index0 = s0.find("--region-size");
	  if(index0 == 0)
	  {
		  if(s0 == "--region-size")
		  {
			  argv2[i] = new char[slen0];
			  memcpy(argv2[i], argv[i], slen0);

			  ++i;
			  if(i >= argc)
			  {
				  for(int k = 0; k <= i ; ++k)
				  {
				     delete [] argv2[k];
				  }
				  delete [] argv2;

				  cerr << "*ERROR* - Region-size option must be followed by a value." << endl;
			      exit ( EXIT_FAILURE );
			  }

			  string s1 = argv[i];
			  int index1 = s1.find("x");
			  if(index1 > 0)
			  {
				  cout << "*WARNING* - You are using a deprecated format of region-size value. Please change " << s1 << " to ";
				  s1.replace(index1, 1, ",");
				  cout << s1 << endl;
			  }

			  int slen1 = strlen(argv[i]) + 1;
			  argv2[i] = new char[slen1];
			  sprintf(argv2[i], "%s", s1.c_str());
		  }
		  else
		  {
			  int index2 = s0.find("x");
			  if(index2 > 0)
			  {
				  cout << "*WARNING* - You are using a deprecated format of region-size value. Please change " << s0 << " to ";
				  s0.replace(index2, 1, ",");
				  cout << s0 << endl;
			  }
			  argv2[i] = new char[slen0];
			  sprintf(argv2[i], "%s", s0.c_str());
		  }
	  }
	  else if(s0 == "on")
	  {
		  cout << "*WARNING* - You are using a deprecated format of " << argv[i - 1] << " value. Please change on to true." << endl;
		  argv2[i] = new char[5];
		  sprintf(argv2[i], "true");
	  }
	  else if(s0 == "off")
	  {
		  cout << "*WARNING* - You are using a deprecated format of " << argv[i - 1] << " value. Please change off to false." << endl;
		  argv2[i] = new char[6];
		  sprintf(argv2[i], "false");
	  }
	  else
	  {
		  int indexon = s0.find("=on");
		  int indexoff = s0.find("=off");
		  if(indexon > 0 && indexon == ((int)(s0.length()) - 3))
		  {
			  cout << "*WARNING* - You are using a deprecated format of " << argv[i] << ". Please change on to true as ";

			  string s3 = s0.substr(0, s0.length() - 2);
			  s3 += "true";

			  int slen3 = s3.length() + 1;
			  argv2[i] = new char[slen3];
			  sprintf(argv2[i], "%s", s3.c_str());

			  cout << argv2[i] << endl;
		  }
		  else if(indexoff > 0 && indexoff == ((int)(s0.length()) - 4))
		  {
  			  cout << "*WARNING* - You are using a deprecated format of " << argv[i] << ". Please change off to false as ";

			  string s4 = s0.substr(0, s0.length() - 3);
			  s4 += "false";

			  int slen4 = s4.length() + 1;
			  argv2[i] = new char[slen4];
			  sprintf(argv2[i], "%s", s4.c_str());

			  cout << argv2[i] << endl;
		  }
		  else
		  {
			  argv2[i] = new char[slen0];
			  memcpy( argv2[i], argv[i], slen0);
		  }
	  }

	  // convert long option to lower case
	  string s5 = argv2[i];
	  int index5 = s5.find("--");
	  int index6 = s5.find("=");
	  if(0 == index5)
	  {
		  if(index6 < 2)
		  {
			  index6 = strlen(argv2[i]) + 1;
		  }

		  for(int j = 2; j < index6; ++j)
		  {
		     argv2[i][j] = tolower (argv2[i][j]);
		  }
	  }
  }
  return datind;
}

void LoadArgsJson(const string& argsJsonFile, Json::Value& json_params, bool thumbnail)
{
    Json::Value json_params0;

    ifstream ifs(argsJsonFile.c_str());
    Json::Reader reader;
    reader.parse(ifs, json_params0, false);
    ifs.close();

    Json::Value json_tn;

     Json::Value::Members groups = json_params0.getMemberNames();
     for(Json::Value::Members::iterator it1 = groups.begin(); it1 != groups.end(); ++it1)
     {
         if(*it1 == "chipType")
         {
             json_params["chipType"] = json_params0["chipType"];
         }
         else if(*it1 == "ThumbnailControl")
         {
             if(thumbnail)
             {
                 Json::Value::Members items = json_params0[*it1].getMemberNames();
                 for(Json::Value::Members::iterator it2 = items.begin(); it2 != items.end(); ++it2)
                 {
                     string sname(*it2);
                     for(size_t i = 0; i < sname.size(); ++i)
                     {
                         if(sname[i] == '-')
                         {
                             sname[i] = '_';
                         }
                     }
                     json_tn[sname] = json_params0[*it1][*it2];
                 }
             }
         }
         else
         {
             Json::Value::Members items = json_params0[*it1].getMemberNames();
             for(Json::Value::Members::iterator it2 = items.begin(); it2 != items.end(); ++it2)
             {
                 string sname(*it2);
                 for(size_t i = 0; i < sname.size(); ++i)
                 {
                     if(sname[i] == '-')
                     {
                         sname[i] = '_';
                     }
                 }
                 json_params[sname] = json_params0[*it1][*it2];
             }
         }
     }

     if(thumbnail)
     {
         Json::Value::Members items = json_tn.getMemberNames();
         for(Json::Value::Members::iterator it2 = items.begin(); it2 != items.end(); ++it2)
         {
             string sname(*it2);

             if(json_params.isMember(sname))
             {
                json_params[sname] = json_tn[*it2];
             }
         }
     }
}

/*************************************************************************************************
 *************************************************************************************************
 *
 *  Start of Main Function
 *
 *************************************************************************************************
 ************************************************************************************************/
int main (int argc, char *argv[])
{
  init_salute();

  ofstream null_ostream("/dev/null"); // must stay live for entire scope, or crash when writing
  TheSilenceOfTheArmadillos(null_ostream);

  TrackProgress my_progress;
  DumpStartingStateOfProgram (argc,argv,my_progress);

  if(argc < 2)
  {
      PrintHelp();
  }

  for(int i = 1; i < argc; ++i)
  {
	  string s = argv[i];
	  if(s == "-" || s == "--")
	  {
	      cerr << "ERROR: command line input \"-\" must be followed by a short option name (a letter) and \"--\" must be followed by a long option name." << endl;
		  exit ( EXIT_FAILURE );
	  }
	  else if(s == "-?" || s == "-h" || s == "--help")
	  {
	      PrintHelp();
	  }
  }

  ValidateOpts validater;
  validater.Validate(argc, argv);

  char** argv2 = new char*[argc];
  int datind = TrapAndDeprecateOldArgs(argc, argv, argv2);

  OptArgs opts;
  opts.ParseCmdLine(argc, (const char**)argv2);

  // enable floating point exceptions during program execution
  if (opts.GetFirstBoolean('-', "float-exceptions", false)) {
      cout << "JustBeadFind: Floating point exceptions enabled." << endl;
      feenableexcept(FE_DIVBYZERO | FE_INVALID | FE_OVERFLOW);
  } //*/

  for(int k = 0; k < argc ; ++k)
  {
	  delete [] argv2[k];
  }
  delete [] argv2;

  Json::Value json_params;
  bool thumbnail = opts.GetFirstBoolean('-', "thumbnail", false);
  string argsJsonFile = opts.GetFirstString('-', "args-json", "");
  if(argsJsonFile.length() > 0)
  {
      struct stat sb0;
      if(!(stat(argsJsonFile.c_str(), &sb0) == 0 && S_ISREG(sb0.st_mode)))
      {
          cerr << "ERROR: " << argsJsonFile << " does not exist or it is not a regular file." << endl;
          exit ( EXIT_FAILURE );
      }

      LoadArgsJson(argsJsonFile, json_params, thumbnail);
  }
  else
  {
      cerr << "ERROR: --args-json must be provided." << endl;
      exit ( EXIT_FAILURE );
  }
  CommandLineOpts inception_state;
  inception_state.SetOpts(opts, json_params);
  inception_state.bfd_control.SetThumbnail(thumbnail);

  if(datind < 0) // there is no "--dat-source-directory"
  {
	  inception_state.sys_context.dat_source_directory = argv[argc - 1];
	  cout << "dat_source_directory = " << inception_state.sys_context.dat_source_directory << endl;
  }

  struct stat sb;
  if(!(stat(inception_state.sys_context.dat_source_directory, &sb) == 0 && S_ISDIR(sb.st_mode)))
  {
      cerr << "ERROR: " << inception_state.sys_context.dat_source_directory << " does not exist or it is not a directory." << endl;
	  exit ( EXIT_FAILURE );
  }

  string chipType = GetParamsString(json_params, "chipType", "");
  ChipIdDecoder::SetGlobalChipId ( chipType.c_str() );

  SeqListClass my_keys;
  ImageSpecClass my_image_spec;
  SlicedPrequel my_prequel_setup;

  SetUpOrLoadInitialState(inception_state, my_keys, my_progress, my_image_spec, my_prequel_setup);

  // Start logging process parameters & timing now that we have somewhere to log
  my_progress.InitFPLog(inception_state);

  // Write processParameters.parse file now that processing is about to begin
  my_progress.WriteProcessParameters(inception_state);

  // Do separator
  Region wholeChip(0, 0, my_image_spec.cols, my_image_spec.rows);
  IsolatedBeadFind( my_prequel_setup, my_image_spec, wholeChip, inception_state,
        inception_state.sys_context.GetResultsFolder(), inception_state.sys_context.analysisLocation,  my_keys, my_progress, chipType);

  exit (EXIT_SUCCESS);
}


#include "VelocityFilter.h"
#include <iostream>
#include <fstream>
#include <vector>
#include "Distance.h"
#include <Eigen/Core>

using namespace std;
using namespace Eigen;

static void writeMesh(const char *filename, const VectorXd &verts, const Matrix3Xi &faces)
{
	ofstream ofs(filename);

	for(int i=0; i<verts.size()/3; i++)
	{
		ofs << "v ";
		for(int j=0; j<3; j++)
			ofs << verts[3*i+j] << " ";
		ofs << endl;
	}

	for(int i=0; i<faces.cols(); i++)
	{
		ofs << "f ";
		for(int j=0; j<3; j++)
			ofs << faces.coeff(j, i)+1 << " ";
		ofs << endl;
	}
}

void loadMesh(const char *filename, VectorXd &verts, Matrix3Xi &faces)
{
	ifstream ifs(filename);
	if(!ifs)
		return;

	vector<double> pos;
	vector<int> faceidx;

	while(true)
	{
		char c;
		ifs >> c;
		if(!ifs)
			break;
		if(c == 'v')
		{
			for(int i=0; i<3; i++)
			{
				double p;
				ifs >> p;
				pos.push_back(p);
			}
		}
		if(c == 'f')
		{
			for(int i=0; i<3; i++)
			{
				int vert;
				ifs >> vert;
				faceidx.push_back(vert);
			}
		}
	}

	int nverts = pos.size()/3;
	int nfaces = faceidx.size()/3;

	verts.resize(3*nverts);
	for(int i=0; i<3*nverts; i++)
		verts[i] = pos[i];

	faces.resize(3, nfaces);
	for(int i=0; i<nfaces; i++)
		for(int j=0; j<3; j++)
			faces.coeffRef(j, i) = faceidx[3*i+j]-1;
}

int main(int argc, char *argv[])
{
	if(argc != 4)
	{
		std::cerr << "Usage: testVelocityFilter (startMesh) (endMesh) (numInfiniteMass)" << std::endl;
		return -1;
	}
	VectorXd q1, q2;
	Matrix3Xi f1, f2;
	loadMesh(argv[1], q1, f1);
	loadMesh(argv[2], q2, f2);

	int numinfinite = strtod(argv[3], NULL);

	std::cout << "Loaded " << q1.size() << " vertices, " << f1.size() << " faces." << std::endl;

	if(numinfinite >= q1.size()/3)
	{
		std::cerr << "Bad value of numInfiniteMass" << std::endl;
		return -1;
	}

	VectorXd invmasses(q1.size());
	for(int i=0; i<3*numinfinite; i++)
		invmasses[i] = 0;
	for(int i=3*numinfinite; i<q1.size(); i++)
		invmasses[i] = 1.0;
	std::cout << VelocityFilter::velocityFilter(q1, q2, f1, invmasses, 2e-8, 1e-8) << std::endl;

	writeMesh("out.obj", q2, f1);
}