#include <igl/opengl/glfw/Viewer.h>
#include <igl/writePLY.h>
#include <igl/hausdorff.h>
#include "../../MeshConnectivity.h"
#include <iostream>
#include <random>
#include <igl/opengl/glfw/imgui/ImGuiMenu.h>
#include <igl/opengl/glfw/imgui/ImGuiHelpers.h>
#include <igl/signed_distance.h>
#include <igl/cotmatrix.h>
#include <igl/per_vertex_normals.h>
#include <igl/boundary_loop.h>

#include <Eigen/CholmodSupport>
#include <imgui/imgui.h>
#include <json/json.h>

#include "../../ShellSolver.h"
#include "../../Obstacle.h"

#include "../../ElasticShell/ElasticEnergy.h"

#include "../../CommonFunctions.h"
#include "../../PressureEnergy.h"

#include "../../EigenNASOQ.h"
#include "../../external/eigengurobi/Gurobi.h"
#include "../../ElasticShell/ElasticIO.h"

static StretchingType stretchingType;
int bendingType;
static ElasticMaterialType materialType;

ElasticSetup setup;
ElasticState curState;
int numSteps;
FullSimOptimizationParams fullSimOptParams;

double thickness;
double pressure;
double PoissonRatio;
double YoungsModulus;
double materialDensity;
double x_externalForce;
double y_externalForce;
double z_externalForce;
int frameFreq;
double noiseNorm;
double penaltyK;
double maxStepSize;
double innerEta;
int iterations;

bool isVisualizeOriginalMesh; // visualize the origin mesh
bool isVisualizeObstacle; // visualize the obstacle
bool restFlat; // plot tension line
double scale;   // plot scale
bool isSPDProj;

std::string filePathPrefix;
std::string defaultPath;
std::string elasticPath;

void liftVerts(double mag) // lifing the verts in direction z
{
	for (int i = 0; i < curState.curPos.rows(); i++)
		curState.curPos(i, 2) += mag;
}

void jitter(double magnitude)
{
	assert(magnitude >= 0);
	if (magnitude == 0)
		return;
	std::cout << "Noise magnitude: " << magnitude << std::endl;
	std::default_random_engine generator;
	std::uniform_real_distribution<double> distribution(0, magnitude);

	Eigen::MatrixXd VN;

	igl::per_vertex_normals(curState.curPos, curState.mesh.faces(), VN);

	for (int i = 0; i < curState.curPos.rows(); i++)
	{
		if (setup.clampedDOFs.find(3 * i) == setup.clampedDOFs.end())
		{
			double offsetNoise = distribution(generator);
			curState.curPos.row(i) += offsetNoise * VN.row(i);
		}
	}
}

void reset()
{
	std::cout << std::endl << "Reset" << std::endl << std::endl;
	//curState.resetState();

}

void repaint(igl::opengl::glfw::Viewer& viewer)
{
	viewer.data().clear();

	/*viewer.data().set_mesh(curState.curPos, curState.mesh.faces());
	Eigen::MatrixXd colors(curState.curPos.rows(), 3);*/

	Eigen::MatrixXd curV, obsV;
	Eigen::MatrixXd curColor, obsColor;
	Eigen::MatrixXi curF, obsF;

	curV.resize(0, 3);
	curF.resize(0, 3);
	curColor.resize(0, 3);

	obsV.resize(0, 3);
	obsF.resize(0, 3);
	obsColor.resize(0, 3);

	bool isFaceBased = true;
	if (isVisualizeOriginalMesh)
	{
		curV = curState.curPos;
		curF = curState.mesh.faces();
		std::cout << "visualize base mesh." << std::endl;
		std::cout << "num of verts: " << curV.rows() << ", num of faces: " << curF.rows() << std::endl;

		isFaceBased = false;
		curColor.resize(curV.rows(), 3);
		curColor.col(0).setConstant(1.0);
		curColor.col(1).setConstant(1.0);
		curColor.col(2).setConstant(0.0);
		for (auto& it : setup.clampedDOFs)
		{
			int vid = it.first / 3;
			curColor(vid, 0) = 1.0;
			curColor(vid, 1) = 0.0;
			curColor(vid, 2) = 0.0;
		}
	}

	if (isVisualizeObstacle)
	{
		std::cout << "visualize obstacle: " << std::endl;
		obsV = setup.obs[0].V;
		obsF = setup.obs[0].F;

		int colorRows = isFaceBased ? obsF.rows() : obsV.rows();
		obsColor.resize(colorRows, 3);
		for (int i = 0; i < colorRows; i++)
		{
			obsColor.row(i) << 1, 1, 1;
		}
	}

	std::cout << "Setting data (V, F): " << std::endl;
	Eigen::MatrixXd dataV, dataColors;
	Eigen::MatrixXi dataF;

	int totalNumV = curV.rows() + obsV.rows();
	int totalNumF = curF.rows() + obsF.rows();
	int totalNumColors = curColor.rows() + obsColor.rows();

	dataV.resize(totalNumV, 3);
	dataF.resize(totalNumF, 3);
	dataColors.resize(totalNumColors, 3);

	dataV.block(0, 0, curV.rows(), 3) = curV;
	dataV.block(curV.rows(), 0, obsV.rows(), 3) = obsV;

	dataF.block(0, 0, curF.rows(), 3) = curF;

	for (int i = 0; i < obsF.rows(); i++)
	{
		for(int j = 0; j < 3; j++)
			dataF(curF.rows() + i, j) = obsF(i, j) + curV.rows();
	}

	std::cout << "Setting Colors: " << std::endl;
	dataColors.block(0, 0, curColor.rows(), 3) = curColor;
	dataColors.block(curColor.rows(), 0, obsColor.rows(), 3) = obsColor;

	// reset data
	std::cout << "Reset Viewer" << std::endl;
	viewer.data().clear();
	viewer.data().set_face_based(isFaceBased);

	viewer.data().set_mesh(dataV, dataF);
	viewer.data().set_colors(dataColors);

	std::cout << "Reset Viewer done" << std::endl;
}

void setParameters()
{
	thickness = setup.thickness;
	materialDensity = setup.density;
	YoungsModulus = setup.YoungsModulus;
	PoissonRatio = setup.PoissonsRatio;
	numSteps = setup.numInterp;
	frameFreq = setup.framefreq;
	x_externalForce = setup.gravity(0);
	y_externalForce = setup.gravity(1);
	z_externalForce = setup.gravity(2);
	scale = 1;
	stretchingType = setup.tensionField ? tensionField : elasticStretching;
	iterations = 2000;
	//set to false for default instead
	bendingType = setup.bendingType == "elastic" ? elasticBending : noBending;
	if (setup.bendingType == "quadratic")
		bendingType = quadraticBending;
	noiseNorm = 0.0;
	penaltyK = setup.penaltyK;

	isSPDProj = setup.penaltyK > 0 ? false : true;

	pressure = setup.pressure;
	restFlat = setup.restFlat;
	innerEta = setup.innerEta;
	maxStepSize = setup.maxStepSize;

	isVisualizeOriginalMesh = true;
	isVisualizeObstacle = false;
}

int main(int argc, char* argv[])
{
	defaultPath = "../../Sims/disc_elastic/disc_elastic.json";
	if (argc >= 2)
		defaultPath = argv[1];
	std::cout << defaultPath << std::endl;
	int index = defaultPath.rfind("_");
	filePathPrefix = defaultPath.substr(0, index);
	// generateElasticJson(filePathPrefix);
	bool ok = loadElastic(defaultPath, setup, curState);
	int loadingTimes = 0;
	while (!ok)
	{
		defaultPath = "../" + defaultPath;
		ok = loadElastic(defaultPath, setup, curState);
		loadingTimes++;
		if (loadingTimes >= 5)
		{
			std::cout << "Failed to load Problem with the path" << defaultPath << std::endl;
			return -1;
		}
	}
	index = defaultPath.rfind("_");
	filePathPrefix = defaultPath.substr(0, index);
	elasticPath = defaultPath;
	std::cout << "Model Name is: " << filePathPrefix << std::endl;

	std::cout << "Set Parameters" << std::endl;
	// parameters loaded from the file
	setParameters();
	std::cout << "curEdgeDOFs norm = " << curState.curEdgeDOFs.norm() << std::endl;

	igl::opengl::glfw::Viewer viewer;

	// Attach a menu plugin
	igl::opengl::glfw::imgui::ImGuiMenu menu;
	viewer.plugins.push_back(&menu);

	// Add content to the default menu window
	menu.callback_draw_viewer_menu = [&]()
	{
		// Draw parent menu content
		//        menu.draw_viewer_menu();
		if (ImGui::CollapsingHeader("I/O", ImGuiTreeNodeFlags_DefaultOpen))
		{
			float w = ImGui::GetContentRegionAvailWidth();
			float p = ImGui::GetStyle().FramePadding.x;
			if (ImGui::Button("Load Problem", ImVec2(ImVec2((w - p) / 2.f, 0))))
			{
				const std::string path = igl::file_dialog_open();
				if (path == "")
				{
					std::cout << "Failed to load problem. Set to the defualt path: " << defaultPath << std::endl;
					elasticPath = defaultPath;
				}
				else
				{
					int index = path.rfind("_");
					filePathPrefix = path.substr(0, index);
					std::cout << "Model Name is: " << filePathPrefix << std::endl;
					elasticPath = path;
				}
				ElasticSetup newSetup;
				ElasticState newState;
				bool ok = loadElastic(elasticPath, newSetup, newState);
				if (!ok)
				{
					std::cout << "Failed to load files !!!!" << std::endl;
					return;
				}
				else
				{
					setup = newSetup;
					curState = newState;
					setParameters();
					repaint(viewer);
					viewer.core().align_camera_center(viewer.data().V, viewer.data().F);
				}
			}
			ImGui::SameLine(0, p);
			if (ImGui::Button("Save Problem", ImVec2((w - p) / 2.f, 0)))
			{
				const std::string path = igl::file_dialog_save();
				if (path == "")
				{
					std::cout << "Invalid saving path" << std::endl;
				}
				else
				{
					bool ok = saveElastic(path, setup, curState);
					if (!ok)
						std::cout << "Failed to save problem." << std::endl;
				}
			}
			if (ImGui::Button("Save Simed Mesh", ImVec2(-1, 0)))
			{
				const std::string path = igl::file_dialog_save();
				if (!igl::writeOBJ(path, curState.curPos, curState.mesh.faces()))
				{
					std::cout << "Failed to save simulated mesh." << std::endl;
				}
			}
		}
		if (ImGui::CollapsingHeader("Viewing Options", ImGuiTreeNodeFlags_DefaultOpen))
		{
			if (ImGui::Button("Center object", ImVec2(-1, 0)))
			{
				viewer.core().align_camera_center(viewer.data().V, viewer.data().F);
			}
			if (ImGui::Button("Snap canonical view", ImVec2(-1, 0)))
			{
				viewer.snap_to_canonical_quaternion();
			}

			// Select rotation type
			int rotation_type = static_cast<int>(viewer.core().rotation_type);
			//int rotation_type = 0;
			static Eigen::Quaternionf trackball_angle = Eigen::Quaternionf::Identity();
			static bool orthographic = true;
			if (ImGui::Combo("Camera Type", &rotation_type, "Trackball\0Two Axes\02D Mode\0\0"))
			{
				using RT = igl::opengl::ViewerCore::RotationType;
				auto new_type = static_cast<RT>(rotation_type);
				if (new_type != viewer.core().rotation_type)
				{
					if (new_type == RT::ROTATION_TYPE_NO_ROTATION)
					{
						trackball_angle = viewer.core().trackball_angle;
						orthographic = viewer.core().orthographic;
						viewer.core().trackball_angle = Eigen::Quaternionf::Identity();
						viewer.core().orthographic = true;
					}
					else if (viewer.core().rotation_type == RT::ROTATION_TYPE_NO_ROTATION)
					{
						viewer.core().trackball_angle = trackball_angle;
						viewer.core().orthographic = orthographic;
					}
					viewer.core().set_rotation_type(new_type);
				}
			}

			// Orthographic view
			ImGui::Checkbox("Orthographic view", &(viewer.core().orthographic));
			//            ImGui::PopItemWidth();
		}
		// Draw options
		if (ImGui::CollapsingHeader("Draw Options", ImGuiTreeNodeFlags_DefaultOpen))
		{
			if (ImGui::InputDouble("plot scale", &scale))
			{
				repaint(viewer);
			}
			if (ImGui::Checkbox("Visualize Original Mesh", &isVisualizeOriginalMesh))
			{
				repaint(viewer);
			}
			if (ImGui::Checkbox("Visualize Obstacle", &isVisualizeObstacle))
			{
				repaint(viewer);
			}
			if (ImGui::Checkbox("Invert normals", &(viewer.data().invert_normals)))
			{
				viewer.data().dirty |= igl::opengl::MeshGL::DIRTY_NORMAL;
			}
			ImGui::ColorEdit4("Background", viewer.core().background_color.data(),
				ImGuiColorEditFlags_NoInputs | ImGuiColorEditFlags_PickerHueWheel);
			ImGui::ColorEdit4("Line color", viewer.data().line_color.data(),
				ImGuiColorEditFlags_NoInputs | ImGuiColorEditFlags_PickerHueWheel);
			ImGui::PushItemWidth(ImGui::GetWindowWidth() * 0.3f);
			ImGui::DragFloat("Shininess", &(viewer.data().shininess), 0.05f, 0.0f, 100.0f);
			//ImGui::PopItemWidth();
		}

		// Overlays
		auto make_checkbox = [&](const char* label, unsigned int& option)
		{
			return ImGui::Checkbox(label,
				[&]() { return viewer.core().is_set(option); },
				[&](bool value) { return viewer.core().set(option, value); }
			);
		};

		if (ImGui::CollapsingHeader("Overlays", ImGuiTreeNodeFlags_DefaultOpen))
		{
			make_checkbox("Wireframe", viewer.data().show_lines);
			make_checkbox("Fill", viewer.data().show_faces);
			bool showvertid = viewer.data().show_vertex_labels != 0;
			if (ImGui::Checkbox("Show vertex labels", &showvertid))
			{
				viewer.data().show_vertex_labels = (showvertid ? 1 : 0);
			}
			bool showfaceid = viewer.data().show_face_labels != 0;
			if (ImGui::Checkbox("Show faces labels", &showfaceid))
			{
				viewer.data().show_face_labels = showfaceid;
			}
		}
	};
	menu.callback_draw_custom_window = [&]()
	{
		// Define next window position + size
		ImGui::SetNextWindowPos(ImVec2(180.f * menu.menu_scaling(), 10), ImGuiCond_FirstUseEver);
		ImGui::SetNextWindowSize(ImVec2(0.0, 0.0), ImGuiCond_FirstUseEver);
		ImGui::Begin(
			"Optimization", nullptr,
			ImGuiWindowFlags_NoSavedSettings
		);

		if (ImGui::CollapsingHeader("Material Parameters", ImGuiTreeNodeFlags_DefaultOpen))
		{
			float w = ImGui::GetContentRegionAvailWidth();
			float p = ImGui::GetStyle().FramePadding.x;
			if (ImGui::InputDouble("Thickness", &thickness))
			{
				if (thickness > 0)
				{
					setup.thickness = thickness;
				}
			}
			if (ImGui::InputDouble("Density", &materialDensity))
			{
				if (materialDensity > 0)
				{
					setup.density = materialDensity;
				}
			}
			if (ImGui::InputDouble("Young's Modulus", &YoungsModulus))
			{
				if (YoungsModulus > 0)
				{
					setup.YoungsModulus = YoungsModulus;
				}
			}
			if (ImGui::InputDouble("Poisson's Ratio", &PoissonRatio))
			{
				if (PoissonRatio >= 0)
				{
					setup.PoissonsRatio = PoissonRatio;
				}
			}
			if (ImGui::InputDouble("fext_x", &x_externalForce))
			{
				setup.gravity(0) = x_externalForce;
			}
			if (ImGui::InputDouble("fext_y", &y_externalForce))
			{

				setup.gravity(1) = y_externalForce;
			}
			if (ImGui::InputDouble("fext_z", &z_externalForce))
			{
				setup.gravity(2) = z_externalForce;
			}

			if (ImGui::InputDouble("Collision Penalty", &penaltyK))
			{
					setup.penaltyK = penaltyK >= 0.0 ? penaltyK : 2000;
			}
			if (ImGui::InputDouble("Collision Eta", &innerEta))
			{
					setup.innerEta = innerEta >= 0.0 ? innerEta : 0.5 * thickness;
			}

			if (ImGui::InputDouble("Pressure", &pressure))
			{
				if (pressure > 0)
				{
					setup.pressure = pressure;
				}
			}
			if (ImGui::InputDouble("Noise Norm", &noiseNorm))
			{
				if (noiseNorm < 0)
				{
					noiseNorm = thickness / 2.0;
				}
			}
			if (ImGui::InputDouble("Max Step Size", &maxStepSize))
			{
				if (maxStepSize > 0)
				{
					setup.maxStepSize = maxStepSize;
				}
			}
		}
		if (ImGui::CollapsingHeader("Optimization Parameters", ImGuiTreeNodeFlags_DefaultOpen))
		{
			if (ImGui::InputInt("Frame Frequence", &frameFreq))
			{
				if (frameFreq > 0)
				{
					setup.framefreq = frameFreq;
				}
			}
			if (ImGui::InputInt("Interpolation Steps", &numSteps))
			{
				if (numSteps > 0)
				{
					setup.numInterp = numSteps;
				}
				fullSimOptParams.interp = 1.0 / numSteps;
			}

			if (ImGui::InputInt("num of iterations", &iterations))
			{
				if (!(iterations > 0))
				{
					fullSimOptParams.iterations = 2000;
				}
				else
				{
					fullSimOptParams.iterations = iterations;
				}
			}
			if (ImGui::InputDouble("Grad Tol", &fullSimOptParams.gradNorm))
			{
				if (!(fullSimOptParams.gradNorm >= 0))
				{
					fullSimOptParams.gradNorm = 1e-6;
				}
			}
			if (ImGui::InputDouble("delta_f", &fullSimOptParams.fDelta))
			{
				if (!(fullSimOptParams.fDelta >= 0))
				{
					fullSimOptParams.fDelta = 1e-10;
				}
			}
			if (ImGui::InputDouble("delta_x", &fullSimOptParams.xDelta))
			{
				if (!(fullSimOptParams.xDelta >= 0))
				{
					fullSimOptParams.xDelta = 1e-10;
				}
			}

			ImGui::Checkbox("SPD Proj H", &fullSimOptParams.isProjH);
			ImGui::Checkbox("StVK TBB Parallel", &fullSimOptParams.isParallel);
		}

		if (ImGui::Combo("Elastic Material", (int*)&materialType, "StVK\0NoeHookean\0\0"))
		{
			if (materialType == NeoHookean)
				setup.isNoeHookean = true;
			else
				setup.isNoeHookean = false;
		}
		if (ImGui::CollapsingHeader("Elastic Energy Type", ImGuiTreeNodeFlags_DefaultOpen))
		{
			if (ImGui::Combo("Stretching", (int*)&stretchingType, "Elastic\0TensionField\0\0"))
			{
				switch (stretchingType)
				{
				case elasticStretching:
					setup.tensionField = false;
					break;
				case tensionField:
					setup.tensionField = true;
					break;
				default:
					break;
				}
			}
			if (ImGui::Combo("Bending", (int*)&bendingType, "Elastic\0Quadratic\0NoBending\0\0"))
			{
				switch (bendingType)
				{
				case elasticBending:
					setup.bendingType = "elastic";
					break;
				case quadraticBending:
					setup.bendingType = "quadratic";
					break;
				case noBending:
					setup.bendingType = "nobending";
					break;
				default:
					break;
				}
			}
		}
		if (ImGui::Button("Optimize ElasticShell", ImVec2(-1, 0)))
		{
			double reg = 1e-6;
			int funcEvals = 0;
			double interp = 0;
			double prevEnergy = 0;
			double curEnergy = 0;

			/*double _lameAlpha = setup.YoungsModulus * setup.PoissonsRatio / (1.0 - setup.PoissonsRatio * setup.PoissonsRatio);
			double _lameBeta = setup.YoungsModulus / 2.0 / (1.0 + setup.PoissonsRatio);
			std::shared_ptr<ElasticShellMaterial> mat;
			mat = std::make_shared<StVKMaterial>();

			std::cout << "stretching = " << elasticStretchingEnergy(curState.mesh, curState.curPos, curState.curEdgeDOFs, _lameAlpha, _lameBeta, setup.thickness, setup.abars, *sff, *mat, NULL, NULL, false, true) << std::endl;
			std::cout << "bending = " << elasticBendingEnergy(curState.mesh, curState.curPos, curState.curEdgeDOFs, _lameAlpha, _lameBeta, setup.thickness, setup.abars, setup.bbars, *sff, *mat, NULL, NULL, false, true) << std::endl;*/
			jitter(noiseNorm);
			for (int k = 1; k <= numSteps; k++)
			{
				ShellSolver::fullSimNewtonStaticSolver(setup, curState, filePathPrefix, fullSimOptParams);
			}
			saveElastic(elasticPath, setup, curState);
			repaint(viewer);
		}

		if (ImGui::Button("Reset ElasticShell", ImVec2(-1, 0)))
		{
			reset();
			curState.resetState();
			repaint(viewer);
		}

		if (ImGui::CollapsingHeader("Useful functions"))
		{
			if (ImGui::Button("Compute Hausdorff Distance", ImVec2(-1, 0)))
			{
				Eigen::MatrixXd V1, V2;
				Eigen::MatrixXi F1, F2;
				std::string fileName1 = igl::file_dialog_open();
				if (!igl::readOBJ(fileName1, V1, F1))
				{
					std::cout << "Failed to load first mesh" << std::endl;
				}
				else
				{
					std::string fileName2 = igl::file_dialog_open();
					if (!igl::readOBJ(fileName2, V2, F2))
					{
						std::cout << "Failed to load second mesh" << std::endl;
					}
					else
					{
						double d = 0;
						igl::hausdorff(V1, F1, V2, F2, d);
						std::cout << "Hausdorff Distance: " << d << std::endl;
					}
				}
			}

			if (ImGui::Button("convert json file", ImVec2(-1, 0)))
			{
				const std::string path = igl::file_dialog_save();
				int index = path.rfind(".");
				std::string myprefix = path.substr(0, index);
				generateElasticJson(myprefix);
			}
		}
		ImGui::End();
	};
	viewer.data().set_face_based(false);
	std::cout << "set data done!" << std::endl;
	repaint(viewer);
	std::cout << "Repaint done!" << std::endl;
	viewer.launch();
	return 0;

}


/* \file Voronoi3D.hpp
\brief A 3D Voronoi
\Author Elad Steinberg
*/
#ifndef VORONOI3D_HPP
#define VORONOI3D_HPP 1

#define _USE_MATH_DEFINES
#include <cmath>
#include <vector>
#include <string>
#include "Delaunay3D.hpp"
#include "Intersections.hpp"
#include <stack>
#include <set>
#include <array>
#include "Tessellation3D.hpp"
#include <boost/container/flat_set.hpp>
#include <boost/container/small_vector.hpp>

#ifdef RICH_MPI
#include "../../newtonian/three_dimensional/computational_cell.hpp"
#include "../../mpi/mpi_commands.hpp"
#endif

typedef std::array<std::size_t, 4> b_array_4;
typedef std::array<std::size_t, 3> b_array_3;

class Voronoi3D : public Tessellation3D
{
private:
	Vector3D ll_, ur_;
	std::size_t Norg_, bigtet_;

	std::set<int> set_temp_;
	std::stack<int> stack_temp_;

	void output_buildextra(std::string const& filename)const;
	void FindIntersectionsSingle(vector<Face> const& box, std::size_t point, Sphere &sphere,
		vector<size_t> &intersecting_faces,std::vector<double> &Rtemp,std::vector<Vector3D> &vtemp);
	void FindIntersectionsRecursive(vector<std::size_t> &res,Tessellation3D const& tproc, std::size_t rank,
		std::size_t point, Sphere &sphere, size_t mode, boost::container::flat_set<size_t> &visited,
		std::stack<std::size_t> &to_check,bool &skipped,face_vec &faces, vector<size_t> &past_duplicate);
	void FindIntersectionsFirstMPI(vector<std::size_t> &res, std::size_t point,
		Sphere &sphere, std::vector<Face> const& faces, bool &skipped, face_vec const& face_index);
	std::size_t GetFirstPointToCheck(void)const;
	void GetPointToCheck(std::size_t point, vector<unsigned char> const& checked, vector<std::size_t> &res);
	void CalcRigidCM(std::size_t face_index);
	void GetTetraCM(std::array<Vector3D, 4> const& points, Vector3D &CM)const;
	double GetTetraVolume(std::array<Vector3D, 4> const& points)const;
	void CalcCellCMVolume(std::size_t index);
	double GetRadius(std::size_t index);
	double GetMaxRadius(std::size_t index);
	void CalcAllCM(void);
	vector<std::pair<std::size_t, std::size_t> > SerialFindIntersections(bool first_run);
	vector<std::pair<std::size_t, std::size_t> > SerialFirstIntersections(void);
	double CalcTetraRadiusCenterHiPrecision(std::size_t index);
#ifdef RICH_MPI
	vector<std::pair<std::size_t, std::size_t> > FindIntersections(Tessellation3D const& tproc, size_t mode,
		vector<unsigned char> &checked_clear);
	vector<Vector3D> CreateBoundaryPointsMPI(vector<std::pair<std::size_t, std::size_t> > const& to_duplicate,
		Tessellation3D const& tproc, vector<vector<size_t> > &self_duplicate);
	void MPIFirstIntersections(Tessellation3D const& tproc, vector<std::pair<std::size_t, std::size_t> > &ghost_index);
#endif
	double CalcTetraRadiusCenter(std::size_t index);
	vector<Vector3D> CreateBoundaryPoints(vector<std::pair<std::size_t, std::size_t> > const& to_duplicate,
		vector<vector<size_t> > &past_duplicate);
	void BuildVoronoi(std::vector<size_t> const& order);

	Delaunay3D del_;
	//vector<vector<std::size_t> > PointTetras_; // The tetras containing each point
	vector<tetra_vec > PointTetras_; // The tetras containing each point
	vector<double> R_; // The radius of the sphere of each tetra
	vector<Vector3D> tetra_centers_;
	// Voronoi Data
	//vector<vector<std::size_t> > FacesInCell_;
	vector<face_vec > FacesInCell_;
	std::vector<point_vec > PointsInFace_; // Right hand with regard to first neighbor
	//vector<vector<std::size_t> > PointsInFace_; // Right hand with regard to first neighbor
	vector<std::pair<std::size_t, std::size_t> > FaceNeighbors_;
	vector<Vector3D> CM_,Face_CM_;
	vector<double> volume_;
	vector<double> area_;
	vector<vector<std::size_t> > duplicated_points_;
	vector<int> sentprocs_, duplicatedprocs_;
	vector<vector<std::size_t> > sentpoints_, Nghost_;
	vector<std::size_t> self_index_;
	Voronoi3D();
	Voronoi3D(Voronoi3D const &other);
	std::array<Vector3D, 4> temp_points_;
	std::array<Vector3D, 5> temp_points2_;
public:
#ifdef RICH_MPI
	vector<Vector3D> UpdateMPIPoints(Tessellation3D const& vproc, int rank,
		vector<Vector3D> const& points, vector<std::size_t>& selfindex, vector<int>& sentproc,
		vector<vector<std::size_t> >& sentpoints);
#endif
	vector<int>& GetSentProcs(void);

	vector<vector<size_t> >& GetSentPoints(void);

	vector<size_t>& GetSelfIndex(void);

	vector<Vector3D>& GetAllFaceCM(void);

	Vector3D FaceCM(std::size_t index)const;

	Voronoi3D(Vector3D const& ll, Vector3D const& ur);

	void output(std::string const& filename)const;

	void Build(vector<Vector3D> const& points);

#ifdef RICH_MPI
	void Build(vector<Vector3D> const& points, Tessellation3D const& tproc);

	friend void SetLoad(Voronoi3D &tproc, vector<Vector3D> &points, size_t Niter, double speed, int mode,double round, bool display);

	friend void SetLoad(Voronoi3D &tproc, vector<Vector3D> &points,vector<ComputationalCell3D> &cells, size_t Niter, double speed, int mode, double round, bool display);
#endif

	void BuildDebug(int rank);

	std::size_t GetPointNo(void) const;

	Vector3D GetMeshPoint(std::size_t index) const;

	double GetArea(std::size_t index) const;

	Vector3D const& GetCellCM(std::size_t index) const;

	std::size_t GetTotalFacesNumber(void) const;

	double GetWidth(std::size_t index) const;

	double GetVolume(std::size_t index) const;

	face_vec const& GetCellFaces(std::size_t index) const;

	vector<Vector3D>& GetMeshPoints(void);

	vector<std::size_t> GetNeighbors(std::size_t index)const;

	Tessellation3D* clone(void) const;

	bool NearBoundary(std::size_t index) const;

	bool BoundaryFace(std::size_t index) const;

	vector<vector<std::size_t> >& GetDuplicatedPoints(void);

	vector<vector<std::size_t> >const& GetDuplicatedPoints(void)const;

	std::size_t GetTotalPointNumber(void)const;

	vector<Vector3D> & GetAllCM(void);

	vector<Vector3D > GetAllCM(void)const;

	void GetNeighborNeighbors(vector<std::size_t> &result, std::size_t point)const;

	Vector3D Normal(std::size_t faceindex)const;

	bool IsGhostPoint(std::size_t index)const;

	Vector3D CalcFaceVelocity(std::size_t index, Vector3D const& v0, Vector3D const& v1)const;

	vector<Vector3D>& GetFacePoints(void);

	vector<double>& GetAllArea(void);

	vector<Vector3D>const& GetFacePoints(void) const;

	vector<face_vec >& GetAllCellFaces(void);

	point_vec const& GetPointsInFace(std::size_t index) const;

	std::pair<std::size_t, std::size_t> GetFaceNeighbors(std::size_t face_index)const;

	vector<int> GetDuplicatedProcs(void)const;

	vector<int> GetSentProcs(void)const;

	vector<vector<std::size_t> > const& GetSentPoints(void)const;

	vector<std::size_t> const& GetSelfIndex(void) const;

	vector<vector<std::size_t> > const& GetGhostIndeces(void) const;

	vector<vector<std::size_t> >& GetGhostIndeces(void);

	void GetNeighbors(size_t index, vector<size_t> &res)const;

	std::pair<Vector3D, Vector3D> GetBoxCoordinates(void)const;

	void BuildNoBox(vector<Vector3D> const& points, vector<vector<Vector3D> > const& ghosts,vector<size_t> toduplicate);

	vector<double>& GetAllVolumes(void);

	vector<double> GetAllVolumes(void)const;

	std::vector<std::pair<size_t, size_t> >& GetAllFaceNeighbors(void);

	vector<point_vec > & GetAllPointsInFace(void);

	size_t& GetPointNo(void);

	bool IsPointOutsideBox(size_t index)const;

	void SetBox(Vector3D const& ll, Vector3D const& ur);
};

bool PointInPoly(Tessellation3D const& tess, Vector3D const& point, std::size_t index);

#endif // VORONOI3D_HPP


/*
 * LegController.hpp
 *
 *  Created on: July 13, 2019
 *      Author: Quincy Jones
 *
 * Copyright (c) <2019> <Quincy Jones - quincy@implementedrobotics.com/>
 * Permission is hereby granted, free of charge, to any person obtaining a
 * copy of this software and associated documentation files (the "Software"),
 * to deal in the Software without restriction, including without limitation
 * the rights to use, copy, modify, merge, publish, distribute, sublicense,
 * and/or sell copies of the Software, and to permit persons to whom the Software
 * is furnished to do so, subject to the following conditions:
 * The above copyright notice and this permission notice shall be included in all
 * copies or substantial portions of the Software.
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
 * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
 * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
 */

#ifndef NOMAD_CORE_CONTROLLERS_LEGCONTROLLER_H_
#define NOMAD_CORE_CONTROLLERS_LEGCONTROLLER_H_

// C System Files
#include <stdint.h>

// C++ System Files
#include <iostream>
#include <string>

// Third Party Includes
#include <Eigen/Dense>
#include <Communications/Messages/double_vec_t.hpp>
#include <Communications/Messages/generic_msg_t.hpp>
#include <Controllers/Messages/leg_controller_cmd_t.hpp>
#include <Controllers/Messages/joint_control_cmd_t.hpp>

// Project Includes
#include <Systems/SystemBlock.hpp>

namespace Controllers::Locomotion
{

    class LegController : public Core::Systems::SystemBlock
    {

    public:
        // TODO: Evaluate breaking these down into single message type, LegControllerSetpoint_t, LegControllerState_t etc
        enum OutputPort
        {
            SERVO_COMMAND = 0,
            NUM_OUTPUTS = 1
        };

        enum InputPort
        {
            LEG_COMMAND = 0,
            NUM_INPUTS = 1
        };
        // FL = 0, FR = 1, RL = 2, RR = 3

        // Base Class Leg ControllerTask Node
        // T_s = System sampling time
        LegController(const double T_s = -1);

    protected:

        // Update function for stateful outputs
        void UpdateStateOutputs();

        // Update function for stateless outputs
        void UpdateStatelessOutputs();

        // Update function for next state from inputs
        void UpdateState();

        // Number of legs
        unsigned int num_legs_;

        // Number of dofs per each leg
        unsigned int num_dofs_;

        // Total number of dofs
        unsigned int total_dofs_;

        // Input (Messages)
        leg_controller_cmd_t leg_command_input_;

        // Output (Messages)
        joint_control_cmd_t servo_command_output_;

        // TODO: Set these at compile time?
        Eigen::VectorXd torque_ff_out_;
        Eigen::VectorXd q_out_;
        Eigen::VectorXd q_d_out_;

        Eigen::MatrixXd k_P_joint_;
        Eigen::MatrixXd k_D_joint_;

        Eigen::MatrixXd k_P_cartesian_;
        Eigen::MatrixXd k_D_cartesian_;

        Eigen::VectorXd foot_pos_;
        Eigen::VectorXd foot_vel_;

        // Jacobian Matrix
        Eigen::MatrixXd J_;

        void ResetState();
    };
} // namespace Controllers::Locomotion

#endif // NOMAD_CORE_CONTROLLERS_STATEESTIMATOR_H_


// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG (haftungsbeschränkt), www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also license.mtl.txt in the distribution.

#ifndef MTL_POSITIVE_REAL_POWER_INCLUDE
#define MTL_POSITIVE_REAL_POWER_INCLUDE

#include <boost/numeric/linear_algebra/operators.hpp>
#include <boost/numeric/linear_algebra/concepts.hpp>
#include <boost/numeric/linear_algebra/identity.hpp>
#include <boost/numeric/linear_algebra/inverse.hpp>
#include <boost/numeric/linear_algebra/is_invertible.hpp>

#include <libs/numeric/linear_algebra/test/positive_real.hpp>

// User defined data types and operators

using mtl::positive_real;

struct magma_mult : public math::mult<positive_real> {};
struct semigroup_mult : public math::mult<positive_real> {};
struct monoid_mult : public math::mult<positive_real> {};
struct pim_mult : public math::mult<positive_real> {};     // Partially invertible monoid
struct group_mult : public math::mult<positive_real> {};

namespace math {
    template<> struct identity_t<monoid_mult, positive_real>
	: public identity_t<mult<positive_real>, positive_real> {};

    template<> struct identity_t<pim_mult, positive_real>
	: public identity_t<mult<positive_real>, positive_real> {};
    template<> struct inverse_t<pim_mult, positive_real>
	: public inverse_t<mult<positive_real>, positive_real> {};
    template<> struct is_invertible_t<pim_mult, positive_real>
	: public detail::non_zero_is_invertible_t<pim_mult, positive_real> {};

    template<> struct identity_t<group_mult, positive_real>
	: public identity_t<mult<positive_real>, positive_real> {};
    template<> struct inverse_t<group_mult, positive_real>
	: public inverse_t<mult<positive_real>, positive_real> {};
    template<> struct is_invertible_t<group_mult, positive_real>
	: public is_invertible_t<mult<positive_real>, positive_real> {};
}

template <typename Op>
void compute_power(positive_real base, int exp, Op op, const char* structure)
{
    using mtl::power;
    try {
	std::cout << base << "^" << exp << " as " << structure << "  " << power(base, exp, op) << '\n';
    } catch (char const* message) {
	std::cout << "\n== Exception: " << message << '\n';
    }
}

#endif // MTL_POSITIVE_REAL_POWER_INCLUDE


/// \file
/// \brief This file implements the landmark detection code and data association
///
/// SUBSCRIBES:
/// scan (sensor_msgs/LaserScan) - Topic where laser scan messages are published
/// filter_output (nuslam/FilterOutput) - Output from the EKF filter about the robot state and
///                                       landmark are published in this topic
/// PUBLISHES:
/// landmarks (nuslam/TurtleMap) - Detected landmarks, whose data association is known is published
///                                in this topic

#include <vector>
#include <iostream>
#include <functional>
#include <numeric>
#include <ros/ros.h>
#include <ros/console.h>

#include <Eigen/Dense>

#include "nuslam/circle_detection.hpp"
#include "sensor_msgs/LaserScan.h"
#include "rigid2d/rigid2d.hpp"
#include "nuslam/circle_detection.hpp"
#include "nuslam/TurtleMap.h"
#include "nuslam/FilterOutput.h"
#include "geometry_msgs/Pose2D.h"

using std::cout;
using std::accumulate;
using rigid2d::Vector2D;
using rigid2d::distance;
using rigid2d::PI;
using Eigen::Vector3d;
using circleDetection::fitCircle;
using rigid2d::Transform2D;
using rigid2d::Vector2D;

enum LandmarkStates{ADD, REMOVE, UNKNOWN};

/// \brief A struct for circle parameters
struct CircleParameters
{
  double centerX, centerY, radius, range, bearing;
};

struct LandmarkGuess
{
  CircleParameters landmark;
  size_t hit=0;
  size_t miss=0;
  size_t maxTries=10;
  double minDetectionRate = 0.5;
  /// \brief Returns the ratio of the measurements that the landmark was visible in
  double calculateDetectionRate()
  {
    return (double) hit / (double) (hit + miss);
  }

  /// \brief Returns if a landmark is reliable enough to be added to a filter state
  /// \returns An enum indicating if the landmark can be added, removed or the state is
  ///          yet to be known
  LandmarkStates isFinalised()
  {
    if(hit+miss < maxTries)
    {
      return UNKNOWN;
    }
    else
    {
      if(calculateDetectionRate() > minDetectionRate)
      {
        return ADD;
      }
      else
      {
        return REMOVE;
      }
    }
  }

};

/// \brief unwarps angle so that the range is between 0 to 2 PI instead of -PI to PI
/// \param angle - angle to be unwarped
/// \returns unwarped angle

double unwarpAngles(double angle)
{
  if(angle < 0)
  {
    return 2 * PI + angle;
  }
  else
  {
    return angle;
  }
}

/// \brief A class for maintaining all landmarks that have been detected but are yet to
///        be added to the filter states.
class LandmarkGuessList
{
  public:
  vector<LandmarkGuess> landmarkList;
  double intraDistance=0.1;
  double newLandmarkDist = 0.5;
  size_t i=0;

  /// \brief A function for updating landmark list with new landmarks that were detected
  /// \param detectedLandmarks - List of landmarks that have been detected in this new measurement

  void updateLandmarks(vector<CircleParameters> detectedLandmarks)
  {

    vector<size_t> landmarkIndices;

    for(auto measurementLandmark: detectedLandmarks)
    {

      bool isLandmarkinList=false;
      double minDistance = std::numeric_limits<double>::infinity();
      for(i=0; i <landmarkList.size();i++)
      {
        auto potentialLandmark = landmarkList[i];
        double distance = sqrt(pow(potentialLandmark.landmark.centerX- measurementLandmark.centerX, 2) +
                              (pow(potentialLandmark.landmark.centerY - measurementLandmark.centerY, 2)));
        vector<LandmarkGuess> newLandmarksList;

        if(distance < intraDistance)
        {
        landmarkIndices.push_back(i);
        landmarkList[i].landmark.centerX = (landmarkList[i].hit * landmarkList[i].landmark.centerX + measurementLandmark.centerX) / (double) (landmarkList[i].hit+1);
        landmarkList[i].hit += 1;
        isLandmarkinList = true;
        }
        else if(distance < minDistance)
        {
          minDistance = distance;
        }

      }
      if(not isLandmarkinList && minDistance > newLandmarkDist)
      {
        LandmarkGuess newLandmark;
        newLandmark.landmark.centerX = measurementLandmark.centerX;
        newLandmark.landmark.centerY = measurementLandmark.centerY;
        newLandmark.landmark.radius = measurementLandmark.radius;
        landmarkList.push_back(newLandmark);
        landmarkList[landmarkList.size()-1].hit += 1;
        landmarkIndices.push_back(landmarkList.size()-1);
      }

    }

    for(i=0; i<landmarkList.size(); i++)
    {
    auto it = std::find(landmarkIndices.begin(), landmarkIndices.end(), i);
    if(it == landmarkIndices.end())
    {
    landmarkList[i].miss += 1;
    }

    }
  }
  /// \brief Returns the landmarks which can be added to the filter states since they
  ///        have been reliably detected across multiple measurements
  /// \returns Landmarks that can be added to the new filter states. Old unreliable landmarks
  ///          are also removed.

  vector<CircleParameters> returnFinalisedLandmarks()
  {
  size_t i;
  vector<CircleParameters> finalisedLandmarks;
  vector<size_t> indicesTodelete;
  for(i=0; i<landmarkList.size(); i++)
  {
    switch(landmarkList[i].isFinalised())
    {
     case ADD: finalisedLandmarks.push_back(landmarkList[i].landmark);
               indicesTodelete.push_back(i);
               break;
     case REMOVE:indicesTodelete.push_back(i);
                 break;
     case UNKNOWN:break;
    }
  }

  for (auto idx = indicesTodelete.rbegin(); idx != indicesTodelete.rend(); ++idx)
  {

     landmarkList.erase(landmarkList.begin() + *idx);

  }
  return finalisedLandmarks;
  }

};

/// \brief Calculates the anticlockwise Distance distance between two angles
/// \param x - angle 1
/// \param y - angle 2
/// \returns anticlockwise distance between two angles
double anticlockwiseDistance(double x, double y)
{
  if(y >= x)
  {
    return y-x;
   }
  else
  {
    return 2 * PI -x + y;
  }

}

/// \brief Calculates the clockwise Distance distance between two angles
/// \param x - angle 1
/// \param y - angle 2
/// \returns anticlockwise distance between two angles

double clockwiseDistance(double x, double y)
{
  if(y <= x)
  {
    return x-y;
   }
  else
  {
    return x + 2 * PI -y ;
  }

}

 class LandmarkDetection
 {
   ros::Subscriber laserscanSubscriber, filterOutputSubscriber;
   ros::Publisher landmarkPublisher;
   double maxRadius, minRadius, maxAssociationDistance;
   geometry_msgs::Pose2D robotPose;
   vector<double> allLandmarksX;
   vector<double> allLandmarksY;
   vector<CircleParameters> newMeasurements;
   vector<size_t> newMeasurementIndex;
   LandmarkGuessList temporaryLandmarkList;

   public:
   /// \brief constructor
   /// \param argc - command line argument indicating number of arguments
   /// \param argv - command line argument indicating the arguments
   LandmarkDetection(int argc, char** argv)
   {
     ros::init(argc, argv, "landmark_detection");
     ros::NodeHandle n;
     ros::Rate loop_rate(100);
     laserscanSubscriber = n.subscribe("/scan", 1000, &LandmarkDetection::laserCallback,
                                     this);
     landmarkPublisher = n.advertise<nuslam::TurtleMap>("/landmarks", 1000);
     filterOutputSubscriber = n.subscribe("/filter_output", 1000, &LandmarkDetection::filterOutputCallback, this);
     maxRadius = 0.05;
     minRadius = 0.03;
     robotPose.x = 0;
     robotPose.y = 0;
     robotPose.theta = 0;
     maxAssociationDistance = 0.1;

   }

   /// \brief A callback function for filter_output topic. This function loads the new
   ///        state of the finalised landmarks from the topic.
   /// \param filterOutput - A message containing filter output state.

   void filterOutputCallback(const nuslam::FilterOutput filterOutput)
   {
     robotPose = filterOutput.robotPose;
     size_t i= 0;
     for(i=0; i < filterOutput.landmarkX.size(); i++)
     {
       allLandmarksX[i] = filterOutput.landmarkX[i];
       allLandmarksY[i] = filterOutput.landmarkY[i];
     }
   }

   /// \brief A lasercallback function. This function processes the laser message, detects
   ///        new landmarks, associated measurements corresponding to existing landmarks
   ///        and publishes the measurements.

   void laserCallback(const sensor_msgs::LaserScan& laserMessage)
   {
     nuslam::TurtleMap mapMessage;
     ros::NodeHandle n;
     mapMessage.header.stamp = ros::Time::now();
     cout<<"received";
     auto ranges = laserMessage.ranges;
     auto angleIncrement = laserMessage.angle_increment;
     vector<Vector2D> detectedPoints;
     size_t i=0, j=0;
     Vector2D point;
     double theta = 0;

     //  Converting from r, theta to x, y
     for(; i< ranges.size(); i++)
     {
       point.x = ranges[i] * cos(theta);
       point.y = ranges[i] * sin(theta);
       detectedPoints.push_back(point);
       theta += angleIncrement;
     }

     // Converting from robot frame to world frame
     //
     vector<vector<Vector2D>> clusteredPoints;
     size_t clusterIndex;
     double distanceThreshold = 0.03;

     for(auto pointr: detectedPoints)
     {

       double minDistance = std::numeric_limits<double>::infinity();

       for(i=0; i< clusteredPoints.size(); i++)
       {
         auto cluster = clusteredPoints[i];
         for(j=0;j<cluster.size(); j++)
         {
           double eucledianDistance = distance(pointr, cluster[j]);
           if(eucledianDistance < minDistance)
           {
             minDistance = eucledianDistance;
             clusterIndex = i;
           }
         }
       }

      if(minDistance > distanceThreshold)
      {
       vector<Vector2D> newCluster;
       newCluster.push_back(pointr);
       clusteredPoints.push_back(newCluster);
      }
      else
      {
      clusteredPoints[clusterIndex].push_back(pointr);

      }

     }

     vector<int> indicesTodelete;

     for(i=0; i < clusteredPoints.size(); i++)

     {
       if(clusteredPoints[i].size() < 4)
       {
         indicesTodelete.push_back(i);

       }

     }

      for (auto idx = indicesTodelete.rbegin(); idx != indicesTodelete.rend(); ++idx)
      {

         clusteredPoints.erase(clusteredPoints.begin() + *idx);

      }

      cout<<"finished";
      cout<<"cluster size"<<clusteredPoints.size();

      vector<CircleParameters> allCircleParams;
      indicesTodelete.clear();
      size_t clusterIdx;

      for(auto cluster: clusteredPoints)
      {
        auto circleParams = fitCircle(cluster);
        CircleParameters circle;
        circle.centerX= (double) circleParams[0];
        circle.centerY= (double) circleParams[1];
        circle.radius= (double) circleParams[2];
        allCircleParams.push_back(circle);
      }

      for(clusterIdx=0; clusterIdx < clusteredPoints.size(); clusterIdx++)
      {
        auto cluster = clusteredPoints[clusterIdx];
        double mean = 0;
        double std = 0;
        vector<double> angles;
        vector<double> Pangles;
        vector<size_t> endpointIndex;

        for(i=0; i<cluster.size(); i++)
        {
          double angle = atan2(cluster[i].x - allCircleParams[clusterIdx].centerX,
                                cluster[i].y - allCircleParams[clusterIdx].centerY);
          angles.push_back(unwarpAngles(angle));
        }

        double P1Angle = angles[0];
        size_t P1Index = 0;
        double P2Angle = angles[0];
        size_t P2Index = 0;

        for(i=1; i<angles.size(); i++)
        {
          double clkDistance = clockwiseDistance(P1Angle, angles[i]);
          double anticlkDistance = anticlockwiseDistance(P2Angle, angles[i]);
          if(clockwiseDistance(P2Angle, angles[i]) < clockwiseDistance(P2Angle, P1Angle))
            continue;
          if(clkDistance < anticlkDistance)
           {
            P1Angle = angles[i];
            P1Index = i;
           }
          else
          {
            P2Angle = angles[i];
            P2Index = i;
          }

        }
        auto P1 = cluster[P1Index];
        auto P2 = cluster[P2Index];
        endpointIndex.push_back(P1Index);
        endpointIndex.push_back(P2Index);

        // for cosine law
        double c = distance(P1, P2);
        std::sort(endpointIndex.begin(), endpointIndex.end());

         for (auto idx = endpointIndex.rbegin(); idx != endpointIndex.rend(); ++idx)
         {

            cluster.erase(cluster.begin() + *idx);

         }

        for(auto P: cluster)
        {
          double b = distance(P1, P);
          double a = distance(P2, P);
          double P1_P_P2Angle = acos((pow(a, 2) + pow(b, 2) - pow(c, 2)) / (2 * a * b));
          Pangles.push_back(P1_P_P2Angle);
        }
        mean = accumulate( Pangles.begin(), Pangles.end(), 0.0 ) / Pangles.size();
        for(auto sampleAngle: Pangles)
        {
          std += pow(sampleAngle - mean, 2);
        }
        std = sqrt(std / Pangles.size());
        // mean is between 90 - 135 degrees
        if(mean<0.52 || mean>3.2 || std > 0.5)
        {
          indicesTodelete.push_back(clusterIdx);
        }

      }

  for (auto idx = indicesTodelete.rbegin(); idx != indicesTodelete.rend(); ++idx)
  {
     allCircleParams.erase(allCircleParams.begin() + *idx);
  }

  indicesTodelete.clear();
  for(i=0; i < allCircleParams.size(); i++)
  {
    if(allCircleParams[i].radius > maxRadius)
    {
      indicesTodelete.push_back(i);

    }

    if(allCircleParams[i].radius < minRadius)
    {
      indicesTodelete.push_back(i);
    }
  }

  for (auto idx = indicesTodelete.rbegin(); idx != indicesTodelete.rend(); ++idx)
  {
     allCircleParams.erase(allCircleParams.begin() + *idx);
  }

  Vector2D robotPosition = {robotPose.x, robotPose.y};

  Transform2D Twr(robotPosition, robotPose.theta);

  for(auto& finalCircles:allCircleParams)
  {

    Vector2D center = {finalCircles.centerX, finalCircles.centerY};
    auto centerW = Twr(center);
    finalCircles.centerX = centerW.x;
    finalCircles.centerY = centerW.y;
    finalCircles.range = sqrt(pow(centerW.x - robotPose.x, 2) + pow(centerW.y - robotPose.y, 2));
    finalCircles.bearing = atan2(centerW.y - robotPose.y, centerW.x - robotPose.x);
  }

  indicesTodelete.clear();
  newMeasurements.clear();
  newMeasurementIndex.clear();

  size_t k=0;
  for(; k< allCircleParams.size(); k++)
  {
    auto finalCircles = allCircleParams[k];
    size_t idx=0;
    for(;idx < allLandmarksX.size(); idx++)
    {
      double distance = sqrt(pow(allLandmarksX[idx] - finalCircles.centerX, 2) + pow(allLandmarksY[idx] -finalCircles.centerY, 2));
      if(distance < maxAssociationDistance)
      {
        newMeasurements.push_back(finalCircles);
        newMeasurementIndex.push_back(idx);
        indicesTodelete.push_back(k);
        break;
      }

    }

  }

  for (auto idx = indicesTodelete.rbegin(); idx != indicesTodelete.rend(); ++idx)
  {
     allCircleParams.erase(allCircleParams.begin() + *idx);
  }

  temporaryLandmarkList.updateLandmarks(allCircleParams);
  auto fixedLandmarks = temporaryLandmarkList.returnFinalisedLandmarks();
  for(auto landmark:fixedLandmarks)
  {
    allLandmarksX.push_back(landmark.centerX);
    allLandmarksY.push_back(landmark.centerY);
  }

  mapMessage.header.stamp = ros::Time::now();
  for(i=0; i < newMeasurements.size(); i++)
  {
    auto finalCircles = newMeasurements[i];
    size_t idx = newMeasurementIndex[i];
    mapMessage.radius.push_back(finalCircles.radius);
    mapMessage.range.push_back(finalCircles.range);
    mapMessage.bearing.push_back(finalCircles.bearing);
    mapMessage.landmarkIndex.push_back(idx);
  }

  for(i=0; i< allLandmarksX.size(); i++)
  {
    mapMessage.centerX.push_back(allLandmarksX[i]);
    mapMessage.centerY.push_back(allLandmarksY[i]);
  }
  mapMessage.landmarkCount = allLandmarksX.size();
  landmarkPublisher.publish(mapMessage);

   }

};

 /// \brief The main function
int main(int argc, char** argv)
{
  LandmarkDetection detectLandmarks(argc, argv);
  ros::spin();
  return 0;
}


//
//  Copyright (c) 2000-2002
//  Joerg Walter, Mathias Koch
//
//  Distributed under the Boost Software License, Version 1.0. (See
//  accompanying file LICENSE_1_0.txt or copy at
//  http://www.boost.org/LICENSE_1_0.txt)
//
//  The authors gratefully acknowledge the support of
//  GeNeSys mbH & Co. KG in producing this work.
//

#ifndef _BOOST_UBLAS_OPERATION_BLOCKED_
#define _BOOST_UBLAS_OPERATION_BLOCKED_

#include <boost/numeric/ublas/traits.hpp>
#include <boost/numeric/ublas/detail/vector_assign.hpp> // indexing_vector_assign
#include <boost/numeric/ublas/detail/matrix_assign.hpp> // indexing_matrix_assign

namespace boost { namespace numeric { namespace ublas {

    template<class V, typename V::size_type BS, class E1, class E2>
    BOOST_UBLAS_INLINE
    V
    block_prod (const matrix_expression<E1> &e1,
                const vector_expression<E2> &e2) {
        typedef V vector_type;
        typedef const E1 expression1_type;
        typedef const E2 expression2_type;
        typedef typename V::size_type size_type;
        typedef typename V::value_type value_type;
        const size_type block_size = BS;

        V v (e1 ().size1 ());
#if BOOST_UBLAS_TYPE_CHECK
        vector<value_type> cv (v.size ());
        typedef typename type_traits<value_type>::real_type real_type;
        real_type verrorbound (norm_1 (v) + norm_1 (e1) * norm_1 (e2));
        indexing_vector_assign<scalar_assign> (cv, prod (e1, e2));
#endif
        size_type i_size = e1 ().size1 ();
        size_type j_size = BOOST_UBLAS_SAME (e1 ().size2 (), e2 ().size ());
        for (size_type i_begin = 0; i_begin < i_size; i_begin += block_size) {
            size_type i_end = i_begin + (std::min) (i_size - i_begin, block_size);
            // FIX: never ignore Martin Weiser's advice ;-(
#ifdef BOOST_UBLAS_NO_CACHE
            vector_range<vector_type> v_range (v, range (i_begin, i_end));
#else
            // vector<value_type, bounded_array<value_type, block_size> > v_range (i_end - i_begin);
            vector<value_type> v_range (i_end - i_begin);
#endif
            v_range.assign (zero_vector<value_type> (i_end - i_begin));
            for (size_type j_begin = 0; j_begin < j_size; j_begin += block_size) {
                size_type j_end = j_begin + (std::min) (j_size - j_begin, block_size);
#ifdef BOOST_UBLAS_NO_CACHE
                const matrix_range<expression1_type> e1_range (e1 (), range (i_begin, i_end), range (j_begin, j_end));
                const vector_range<expression2_type> e2_range (e2 (), range (j_begin, j_end));
                v_range.plus_assign (prod (e1_range, e2_range));
#else
                // const matrix<value_type, row_major, bounded_array<value_type, block_size * block_size> > e1_range (project (e1 (), range (i_begin, i_end), range (j_begin, j_end)));
                // const vector<value_type, bounded_array<value_type, block_size> > e2_range (project (e2 (), range (j_begin, j_end)));
                const matrix<value_type, row_major> e1_range (project (e1 (), range (i_begin, i_end), range (j_begin, j_end)));
                const vector<value_type> e2_range (project (e2 (), range (j_begin, j_end)));
                v_range.plus_assign (prod (e1_range, e2_range));
#endif
            }
#ifndef BOOST_UBLAS_NO_CACHE
            project (v, range (i_begin, i_end)).assign (v_range);
#endif
        }
#if BOOST_UBLAS_TYPE_CHECK
        BOOST_UBLAS_CHECK (norm_1 (v - cv) <= 2 * std::numeric_limits<real_type>::epsilon () * verrorbound, internal_logic ());
#endif
        return v;
    }

    template<class V, typename V::size_type BS, class E1, class E2>
    BOOST_UBLAS_INLINE
    V
    block_prod (const vector_expression<E1> &e1,
                const matrix_expression<E2> &e2) {
        typedef V vector_type;
        typedef const E1 expression1_type;
        typedef const E2 expression2_type;
        typedef typename V::size_type size_type;
        typedef typename V::value_type value_type;
        const size_type block_size = BS;

        V v (e2 ().size2 ());
#if BOOST_UBLAS_TYPE_CHECK
        vector<value_type> cv (v.size ());
        typedef typename type_traits<value_type>::real_type real_type;
        real_type verrorbound (norm_1 (v) + norm_1 (e1) * norm_1 (e2));
        indexing_vector_assign<scalar_assign> (cv, prod (e1, e2));
#endif
        size_type i_size = BOOST_UBLAS_SAME (e1 ().size (), e2 ().size1 ());
        size_type j_size = e2 ().size2 ();
        for (size_type j_begin = 0; j_begin < j_size; j_begin += block_size) {
            size_type j_end = j_begin + (std::min) (j_size - j_begin, block_size);
            // FIX: never ignore Martin Weiser's advice ;-(
#ifdef BOOST_UBLAS_NO_CACHE
            vector_range<vector_type> v_range (v, range (j_begin, j_end));
#else
            // vector<value_type, bounded_array<value_type, block_size> > v_range (j_end - j_begin);
            vector<value_type> v_range (j_end - j_begin);
#endif
            v_range.assign (zero_vector<value_type> (j_end - j_begin));
            for (size_type i_begin = 0; i_begin < i_size; i_begin += block_size) {
                size_type i_end = i_begin + (std::min) (i_size - i_begin, block_size);
#ifdef BOOST_UBLAS_NO_CACHE
                const vector_range<expression1_type> e1_range (e1 (), range (i_begin, i_end));
                const matrix_range<expression2_type> e2_range (e2 (), range (i_begin, i_end), range (j_begin, j_end));
#else
                // const vector<value_type, bounded_array<value_type, block_size> > e1_range (project (e1 (), range (i_begin, i_end)));
                // const matrix<value_type, column_major, bounded_array<value_type, block_size * block_size> > e2_range (project (e2 (), range (i_begin, i_end), range (j_begin, j_end)));
                const vector<value_type> e1_range (project (e1 (), range (i_begin, i_end)));
                const matrix<value_type, column_major> e2_range (project (e2 (), range (i_begin, i_end), range (j_begin, j_end)));
#endif
                v_range.plus_assign (prod (e1_range, e2_range));
            }
#ifndef BOOST_UBLAS_NO_CACHE
            project (v, range (j_begin, j_end)).assign (v_range);
#endif
        }
#if BOOST_UBLAS_TYPE_CHECK
        BOOST_UBLAS_CHECK (norm_1 (v - cv) <= 2 * std::numeric_limits<real_type>::epsilon () * verrorbound, internal_logic ());
#endif
        return v;
    }

    template<class M, typename M::size_type BS, class E1, class E2>
    BOOST_UBLAS_INLINE
    M
    block_prod (const matrix_expression<E1> &e1,
                const matrix_expression<E2> &e2,
                row_major_tag) {
        typedef M matrix_type;
        typedef const E1 expression1_type;
        typedef const E2 expression2_type;
        typedef typename M::size_type size_type;
        typedef typename M::value_type value_type;
        const size_type block_size = BS;

        M m (e1 ().size1 (), e2 ().size2 ());
#if BOOST_UBLAS_TYPE_CHECK
        matrix<value_type, row_major> cm (m.size1 (), m.size2 ());
        typedef typename type_traits<value_type>::real_type real_type;
        real_type merrorbound (norm_1 (m) + norm_1 (e1) * norm_1 (e2));
        indexing_matrix_assign<scalar_assign> (cm, prod (e1, e2), row_major_tag ());
        disable_type_check<bool>::value = true;
#endif
        size_type i_size = e1 ().size1 ();
        size_type j_size = e2 ().size2 ();
        size_type k_size = BOOST_UBLAS_SAME (e1 ().size2 (), e2 ().size1 ());
        for (size_type i_begin = 0; i_begin < i_size; i_begin += block_size) {
            size_type i_end = i_begin + (std::min) (i_size - i_begin, block_size);
            for (size_type j_begin = 0; j_begin < j_size; j_begin += block_size) {
                size_type j_end = j_begin + (std::min) (j_size - j_begin, block_size);
                // FIX: never ignore Martin Weiser's advice ;-(
#ifdef BOOST_UBLAS_NO_CACHE
                matrix_range<matrix_type> m_range (m, range (i_begin, i_end), range (j_begin, j_end));
#else
                // matrix<value_type, row_major, bounded_array<value_type, block_size * block_size> > m_range (i_end - i_begin, j_end - j_begin);
                matrix<value_type, row_major> m_range (i_end - i_begin, j_end - j_begin);
#endif
                m_range.assign (zero_matrix<value_type> (i_end - i_begin, j_end - j_begin));
                for (size_type k_begin = 0; k_begin < k_size; k_begin += block_size) {
                    size_type k_end = k_begin + (std::min) (k_size - k_begin, block_size);
#ifdef BOOST_UBLAS_NO_CACHE
                    const matrix_range<expression1_type> e1_range (e1 (), range (i_begin, i_end), range (k_begin, k_end));
                    const matrix_range<expression2_type> e2_range (e2 (), range (k_begin, k_end), range (j_begin, j_end));
#else
                    // const matrix<value_type, row_major, bounded_array<value_type, block_size * block_size> > e1_range (project (e1 (), range (i_begin, i_end), range (k_begin, k_end)));
                    // const matrix<value_type, column_major, bounded_array<value_type, block_size * block_size> > e2_range (project (e2 (), range (k_begin, k_end), range (j_begin, j_end)));
                    const matrix<value_type, row_major> e1_range (project (e1 (), range (i_begin, i_end), range (k_begin, k_end)));
                    const matrix<value_type, column_major> e2_range (project (e2 (), range (k_begin, k_end), range (j_begin, j_end)));
#endif
                    m_range.plus_assign (prod (e1_range, e2_range));
                }
#ifndef BOOST_UBLAS_NO_CACHE
                project (m, range (i_begin, i_end), range (j_begin, j_end)).assign (m_range);
#endif
            }
        }
#if BOOST_UBLAS_TYPE_CHECK
        disable_type_check<bool>::value = false;
        BOOST_UBLAS_CHECK (norm_1 (m - cm) <= 2 * std::numeric_limits<real_type>::epsilon () * merrorbound, internal_logic ());
#endif
        return m;
    }

    template<class M, typename M::size_type BS, class E1, class E2>
    BOOST_UBLAS_INLINE
    M
    block_prod (const matrix_expression<E1> &e1,
                const matrix_expression<E2> &e2,
                column_major_tag) {
        typedef M matrix_type;
        typedef const E1 expression1_type;
        typedef const E2 expression2_type;
        typedef typename M::size_type size_type;
        typedef typename M::value_type value_type;
        const size_type block_size = BS;

        M m (e1 ().size1 (), e2 ().size2 ());
#if BOOST_UBLAS_TYPE_CHECK
        matrix<value_type, column_major> cm (m.size1 (), m.size2 ());
        typedef typename type_traits<value_type>::real_type real_type;
        real_type merrorbound (norm_1 (m) + norm_1 (e1) * norm_1 (e2));
        indexing_matrix_assign<scalar_assign> (cm, prod (e1, e2), column_major_tag ());
        disable_type_check<bool>::value = true;
#endif
        size_type i_size = e1 ().size1 ();
        size_type j_size = e2 ().size2 ();
        size_type k_size = BOOST_UBLAS_SAME (e1 ().size2 (), e2 ().size1 ());
        for (size_type j_begin = 0; j_begin < j_size; j_begin += block_size) {
            size_type j_end = j_begin + (std::min) (j_size - j_begin, block_size);
            for (size_type i_begin = 0; i_begin < i_size; i_begin += block_size) {
                size_type i_end = i_begin + (std::min) (i_size - i_begin, block_size);
                // FIX: never ignore Martin Weiser's advice ;-(
#ifdef BOOST_UBLAS_NO_CACHE
                matrix_range<matrix_type> m_range (m, range (i_begin, i_end), range (j_begin, j_end));
#else
                // matrix<value_type, column_major, bounded_array<value_type, block_size * block_size> > m_range (i_end - i_begin, j_end - j_begin);
                matrix<value_type, column_major> m_range (i_end - i_begin, j_end - j_begin);
#endif
                m_range.assign (zero_matrix<value_type> (i_end - i_begin, j_end - j_begin));
                for (size_type k_begin = 0; k_begin < k_size; k_begin += block_size) {
                    size_type k_end = k_begin + (std::min) (k_size - k_begin, block_size);
#ifdef BOOST_UBLAS_NO_CACHE
                    const matrix_range<expression1_type> e1_range (e1 (), range (i_begin, i_end), range (k_begin, k_end));
                    const matrix_range<expression2_type> e2_range (e2 (), range (k_begin, k_end), range (j_begin, j_end));
#else
                    // const matrix<value_type, row_major, bounded_array<value_type, block_size * block_size> > e1_range (project (e1 (), range (i_begin, i_end), range (k_begin, k_end)));
                    // const matrix<value_type, column_major, bounded_array<value_type, block_size * block_size> > e2_range (project (e2 (), range (k_begin, k_end), range (j_begin, j_end)));
                    const matrix<value_type, row_major> e1_range (project (e1 (), range (i_begin, i_end), range (k_begin, k_end)));
                    const matrix<value_type, column_major> e2_range (project (e2 (), range (k_begin, k_end), range (j_begin, j_end)));
#endif
                    m_range.plus_assign (prod (e1_range, e2_range));
                }
#ifndef BOOST_UBLAS_NO_CACHE
                project (m, range (i_begin, i_end), range (j_begin, j_end)).assign (m_range);
#endif
            }
        }
#if BOOST_UBLAS_TYPE_CHECK
        disable_type_check<bool>::value = false;
        BOOST_UBLAS_CHECK (norm_1 (m - cm) <= 2 * std::numeric_limits<real_type>::epsilon () * merrorbound, internal_logic ());
#endif
        return m;
    }

    // Dispatcher
    template<class M, typename M::size_type BS, class E1, class E2>
    BOOST_UBLAS_INLINE
    M
    block_prod (const matrix_expression<E1> &e1,
                const matrix_expression<E2> &e2) {
        typedef typename M::orientation_category orientation_category;
        return block_prod<M, BS> (e1, e2, orientation_category ());
    }

}}}

#endif


#include <iostream>
#include <typeinfo>
#define BOOST_TEST_MODULE ImplicitMIAViewTests

#include "MIAConfig.h"

#ifdef MIA_USE_HEADER_ONLY_TESTS
#include <boost/test/included/unit_test.hpp>
#else
#include <boost/test/unit_test.hpp>
#endif
#include "LibMIAUtil.h"
#include "DenseMIA.h"
#include "ImplicitMIA.h"
#include "LibMIARanges.h"
using namespace LibMIA;
template<class _data_type>
void do_work(size_t dim1,size_t dim2){

    DenseMIA<_data_type,2> a(dim1,dim2);
    DenseMIA<_data_type,3> b(dim1,dim2,dim1);

    size_t _start=2,_end=2;
    a.randu(-10,10);

    auto implicit_a=a.view(Range<long long>(_start,dim1-_end),Range<long long>(_start,dim2-_end));
    bool passed=true;

    for(size_t i=0;i<implicit_a.dim(0);++i){

        for(size_t j=0;j<implicit_a.dim(1);++j){

            passed=passed&&(implicit_a.at(i,j)==a.at(_start+i,_start+j));
        }
    }
    BOOST_CHECK_MESSAGE(passed,std::string("Value test 1 for view operation for ")+typeid(_data_type).name());
    passed=true;
    for(size_t i=0;i<implicit_a.dim(0);++i){

        for(size_t j=0;j<implicit_a.dim(1);++j){

            passed=passed&&(&(implicit_a.at(i,j))==&(a.at(_start+i,_start+j)));
        }
    }
    BOOST_CHECK_MESSAGE(passed,std::string("Reference test 1 for view operation for ")+typeid(_data_type).name());
    for(size_t i=0;i<implicit_a.dim(0);++i){

        for(size_t j=0;j<implicit_a.dim(1);++j){

            implicit_a.at(i,j)=2;
        }
    }
    passed=true;
    for(size_t i=0;i<implicit_a.dim(0);++i){

        for(size_t j=0;j<implicit_a.dim(1);++j){

            passed=passed&&(a.at(_start+i,_start+j)==2);
        }
    }
    BOOST_CHECK_MESSAGE(passed,std::string("Assign to view test 1 for ")+typeid(_data_type).name());

    //now try with a step size of 2
    _start=1;_end=1;
    size_t _step=2;
    a.randu(-10,10);

    implicit_a=a.view(Range<long long>(_start,dim1-_end,_step),Range<long long>(_start,dim2-_end,_step));

    passed=true;

    for(size_t i=0;i<implicit_a.dim(0);++i){

        for(size_t j=0;j<implicit_a.dim(1);++j){

            passed=passed&&(implicit_a.at(i,j)==a.at(_start+i*_step,_start+j*_step));
        }
    }
    BOOST_CHECK_MESSAGE(passed,std::string("Value test 2 for view operation for ")+typeid(_data_type).name());
    passed=true;
    for(size_t i=0;i<implicit_a.dim(0);++i){

        for(size_t j=0;j<implicit_a.dim(1);++j){

            passed=passed&&(&(implicit_a.at(i,j))==&(a.at(_start+i*_step,_start+j*_step)));
        }
    }
    BOOST_CHECK_MESSAGE(passed,std::string("Reference test 2 for view operation for ")+typeid(_data_type).name());
    for(size_t i=0;i<implicit_a.dim(0);++i){

        for(size_t j=0;j<implicit_a.dim(1);++j){

            implicit_a.at(i,j)=2;
        }
    }
    passed=true;
    for(size_t i=0;i<implicit_a.dim(0);++i){

        for(size_t j=0;j<implicit_a.dim(1);++j){

            passed=passed&&(a.at(_start+i*_step,_start+j*_step)==2);
        }
    }

    BOOST_CHECK_MESSAGE(passed,std::string("Assign to view test 2 for ")+typeid(_data_type).name());

    //now let's try where the we get some 'slices' of the MIA, creating a lower-order MIA

     a.randu(-10,10);

    size_t const_index=3;
    ImplicitMIA<_data_type,1,true> first_order_a=a.view(Range<long long>(_start,dim1-_end,_step),const_index);
    passed=true;

    for(size_t i=0;i<first_order_a.dim(0);++i){
            passed=passed&&(first_order_a.at(i)==a.at(_start+i*_step,const_index));

    }
    BOOST_CHECK_MESSAGE(passed,std::string("Value test 1 for slice for ")+typeid(_data_type).name());

    passed=true;
    for(size_t i=0;i<first_order_a.dim(0);++i){
            passed=passed&&(&(first_order_a.at(i))==&(a.at(_start+i*_step,const_index)));

    }
    BOOST_CHECK_MESSAGE(passed,std::string("Ref test 1 for slice for ")+typeid(_data_type).name());

    for(size_t i=0;i<first_order_a.dim(0);++i){

        first_order_a.at(i)=2;

    }

    passed=true;
    for(size_t i=0;i<first_order_a.dim(0);++i){

        passed=passed&&(a.at(_start+i*_step,const_index)==2);

    }

    BOOST_CHECK_MESSAGE(passed,std::string("Assign to view test 1 for slice for ")+typeid(_data_type).name());

    //Now let's try with other dimension set to constant
    a.randu(-10,10);

    const_index=4;
    first_order_a=a.view(const_index,Range<long long>(_start,dim1-_end,_step));
    passed=true;

    for(size_t i=0;i<first_order_a.dim(0);++i){
            passed=passed&&(first_order_a.at(i)==a.at(const_index,_start+i*_step));

    }
    BOOST_CHECK_MESSAGE(passed,std::string("Value test 2 for slice for ")+typeid(_data_type).name());
    passed=true;
    for(size_t i=0;i<first_order_a.dim(0);++i){
            passed=passed&&(&(first_order_a.at(i))==&(a.at(const_index,_start+i*_step)));

    }
    BOOST_CHECK_MESSAGE(passed,std::string("Ref test 2 for slice for ")+typeid(_data_type).name());

    for(size_t i=0;i<first_order_a.dim(0);++i){

        first_order_a.at(i)=2;

    }
    passed=true;
    for(size_t i=0;i<first_order_a.dim(0);++i){

        passed=passed&&(a.at(const_index,_start+i*_step)==2);

    }

    BOOST_CHECK_MESSAGE(passed,std::string("Assign to view test 2 for slice for ")+typeid(_data_type).name());

    //Now we try with a thiro-order array, using a second-order view
    b.randu(-10,10);
    size_t _step2=4;
    auto implicit_b=b.view(Range<long long>(_start,dim1-_end,_step),const_index,Range<long long>(0,dim1,_step2));

    passed=true;

    for(size_t i=0;i<implicit_b.dim(0);++i){

        for(size_t j=0;j<implicit_b.dim(1);++j){

            passed=passed&&(implicit_b.at(i,j)==b.at(_start+i*_step,const_index,0+j*_step2));
        }
    }
    BOOST_CHECK_MESSAGE(passed,std::string("Value test 3 for slice test for ")+typeid(_data_type).name());
    passed=true;
    for(size_t i=0;i<implicit_b.dim(0);++i){

        for(size_t j=0;j<implicit_b.dim(1);++j){

            passed=passed&&(&(implicit_b.at(i,j))==&(b.at(_start+i*_step,const_index,0+j*_step2)));
        }
    }
    BOOST_CHECK_MESSAGE(passed,std::string("Reference test 3 for view operation for ")+typeid(_data_type).name());
    for(size_t i=0;i<implicit_b.dim(0);++i){

        for(size_t j=0;j<implicit_b.dim(1);++j){

            implicit_b.at(i,j)=2;
        }
    }
    passed=true;
    for(size_t i=0;i<implicit_b.dim(0);++i){

        for(size_t j=0;j<implicit_b.dim(1);++j){

            passed=passed&&(b.at(_start+i*_step,const_index,0+j*_step2)==2);
        }
    }
    BOOST_CHECK_MESSAGE(passed,std::string("Assign to view test 3 for ")+typeid(_data_type).name());

}

BOOST_AUTO_TEST_CASE( ImplicitMIAViewTests )
{

    do_work<double>(8,12);
//    do_work<float>(8,12);
//    do_work<int>(8,12);
//    do_work<long long int>(8,12);

}


//By Fatemeh Saki, University of Texas at Dallas, Spring 2017
// Modified by Nasim Alamdari, 2017
#include "stdafx.h"
#include <iostream>
#include <fstream>
#include <vector>
#include <dlib/svm.h>

// Macros for this module
#define R2_MULTIPLIER_CONST		1.1
#define THRESHOLD_CONST			0.3

// SVM Macros
#define SVM_TYPE				SVDD
#define KERNEL_TYPE				RBF
#define TOL						1e-8
#define PI						3.14159265358979323846
#define CACHE_SIZE				100

// -----------------------------------------------------------------------------------------------
// Definition of the Classification function
void Classification(const SVCluster* ClusterSet, double** frame, int* ClassOut, const int sizeC, const int m, const int n)
{
	/*
	initialize the output vector
	ClassOut[0] is equivalent of CloseCL	in MATLAB
	ClassOut[1] is equivalent of ClassType	in MATLAB
	ClassOut[2] is equivalent of NovOp		in MATLAB
	*/
	for (int i = 0; i < 2; i++)
		ClassOut[i] = 0;

	// allocating memory for the Dist results
	double **Dist = (double **)malloc(sizeC * sizeof(double *));
	for (int i = 0; i < sizeC; i++)
		Dist[i] = (double *)malloc(m* sizeof(double)); // modified by Nasim

	// allocating memory for the closeness index results
	double **CloseIdx = (double **)malloc(sizeC * sizeof(double *));
	for (int i = 0; i < sizeC; i++)
		CloseIdx[i] = (double *)malloc(m* sizeof(double)); // modified by Nasim

	// loop over the cluster set and perform the calculation
	for (int i = 0; i < sizeC; i++){

		// extract the structure information for the ith element
		int c = (ClusterSet + i)->c;
		int r = (ClusterSet + i)->r;
		double Kxx = (ClusterSet + i)->Kxx;
		double R2 = (ClusterSet + i)->R2;
		double sigma = (ClusterSet + i)->sigma;
		double offsets = (ClusterSet + i)->offsets;
		double* alpha = (ClusterSet + i)->alpha;
		double** SVvect = (ClusterSet + i)->SVvectors;

		// declare the kernelDist vector
		double *kernelDist = (double *)malloc(m * sizeof(double)); //modified by Nasim

		// allocating memory for the result pauerwise euclidian distance
		double **DistMtx = (double **)malloc(r * sizeof(double *));
		for (int j = 0; j < r; j++)
			DistMtx[j] = (double *)malloc(m* sizeof(double));

		// calculate the distance
		MyDistm(SVvect, frame, DistMtx, r, m, c);// corrected by Nasim

		// claculate alphaK (to save memory we reuse DistMtx)
		myExp(DistMtx, alpha, r, m, sigma); // corrected by Nasim

		//ofstream out_data;
		//out_data.open("OFC Results.txt", ios::trunc); //modified by Nasim
		// loop over j to calculate the kernelDist
		for (int j = 0; j < m; j++){ // modified by Nasim
			double sum = 0;
			for (int k = 0; k < r; k++){
				sum += DistMtx[k][j]; //modified by Nasim (previously was DistMtx[j][k];)
			}

			//kernelDist[j] = (1 - 2 * sum + Kxx) / (R2_MULTIPLIER_CONST * R2);
			kernelDist[j] = (1 - 2 * sum + Kxx); // Modified by Nasim
			Dist[i][j] = kernelDist[j] / (R2_MULTIPLIER_CONST * R2);

			// Perform a comparision for the calculation of CloseIdx
			if (kernelDist[j] <= (R2_MULTIPLIER_CONST * R2))
				CloseIdx[i][j] = 1; //modified by Nasim
			else
				CloseIdx[i][j] = 0;

			//out_data << CloseIdx[i][j] << '\n';

		}// end of the for loop
		//out_data.close();
		// free the memory for the current SVCluster
		for (int j = 0; j < r; j++) free(DistMtx[j]);
		free(DistMtx);

		free(kernelDist);

		DistMtx = NULL;
		kernelDist = NULL;
	}// end of the for loop

	// declare the sumCloseIdx vector
	double *sumCloseIdx = (double *)malloc(sizeC * sizeof(double));

	// calculate the sum of closeIdx matrix across the first dimension
	for (int i = 0; i < sizeC; i++){
		double sum = 0;
		for (int j = 0; j < m; j++) //modified by Nasim
			sum += CloseIdx[i][j];

		sumCloseIdx[i] = sum;
	}

	// define a threshold
	double threshold = floor(THRESHOLD_CONST * m);

	// perform the required comparisions
	if (sizeC == 1){
		if (compareArrays(sumCloseIdx, threshold, sizeC)){
			ClassOut[0] = 1;
			ClassOut[1] = 1;
			ClassOut[2] = 0; // modified by Nasim
		}
		else
			ClassOut[0] = 0; // modified by Nasim
		    ClassOut[1] = 0; // modified by Nasim
			ClassOut[2] = 1;

	}
	else if (sizeC > 1){
		// declare the Clust vector
		int *Clust = (int *)malloc(m * sizeof(int)); // modified by Nasim
		// find the min of matrix dist across the first dimension
		minMatrixR(Dist, Clust, sizeC, m);

		//ofstream out_data;
		//out_data.open("OFC Results.txt", ios::trunc); //modified by Nasim
		int *Clust2 = (int *)malloc(m * sizeof(int)); //added by Nasim
		for (int i = 0; i < m; i++){ //added by Nasim
			Clust2[i] = Clust[i] + 1;
			//out_data << Clust2[i] << ',';
		}
		//out_data.close();
		// take the statistical mode of array Clust
		int CloseCL2 = mode(Clust2, m); // modified by Nasim
		// now compute the sum
		double sum = 0;
		for (int i = 0; i < m; i++) //modified by Nasim
			sum += CloseIdx[CloseCL2-1][i]; // modified by Nasim

		if (sum>=threshold){
			ClassOut[0] = CloseCL2;
			ClassOut[1] = 1;
		}
		else if (sum==0)
			ClassOut[2] = 1;

	}

	//free the allocated memory
	for (int i = 0; i < sizeC; i++) free(Dist[i]);
	free(Dist);

	for (int i = 0; i < sizeC; i++) free(CloseIdx[i]);
	free(CloseIdx);

	free(sumCloseIdx);

	Dist		= NULL;
	CloseIdx	= NULL;
	sumCloseIdx = NULL;

	return;
}

// -----------------------------------------------------------------------------------------------
// Definition of the connection Check function
int connectionCheck(double** frame1, double** frame2, const int Rowsize1, const int Rowsize2, const int ColSize, const double eps1, const double eps2){

	// allocating memory for the euclidian distance matrix
	double **DistMtx = (double **)malloc(Rowsize1 * sizeof(double *));
	for (int i = 0; i < Rowsize1; i++)
		DistMtx[i] = (double *)malloc(Rowsize2 * sizeof(double)); //modified byNasim

	// run the Eclidean distance function
	MyDistm(frame1, frame2, DistMtx, Rowsize1, Rowsize2, ColSize);

	// calculate the conditional sum of the matrix entries
	double clustDist = sumMatrixCond(DistMtx, Rowsize1, Rowsize2, eps1 + eps2); // modified by Nasim

	// free the allocated memory
	for (int i = 0; i < Rowsize1; i++) free(DistMtx[i]); //modified by Nasim
	free(DistMtx);

	// avoid danggling pointer
	DistMtx = NULL;

	if (clustDist > (THRESHOLD_CONST * Rowsize1 * THRESHOLD_CONST * Rowsize2))
		return +1;

	return -1;
}

// -----------------------------------------------------------------------------------------------
// Definition of the Frame analysis function
//Confirmed by Nasim
double frameAnalysis(double** frame, const int RowSize, const int ColSize)
{
	// allocating memory for the euclidian distance matrix
	double **DistMtx = (double **)malloc(RowSize * sizeof(double *));
	for (int i = 0; i < RowSize; i++)
		DistMtx[i] = (double *)malloc(RowSize * sizeof(double));

	// run the Eclidean distance function
	MyDistm(frame, frame, DistMtx, RowSize, RowSize, ColSize);

	// find the mean of the matrix entries
	double mDist = meanMatrix(DistMtx, RowSize, ColSize)/2;

	// find the maximum element
	double radius = maxMatrix(DistMtx, RowSize, ColSize);

	// for debug purpose only
	//printFrame(DistMtx, Rowsize, ColSize);

	// free the allocated memory
	for (int i = 0; i < RowSize; i++) free(DistMtx[i]);
	free(DistMtx);

	// avoid danggling pointer
	DistMtx = NULL;

	return radius;
}

// -----------------------------------------------------------------------------------------------
// Definition of the Cluster Creation function //modified by Nasim
SVCluster* ClusterCreation(double** svData, const int svRow, const int svCol, const double fracRejection, const double sigma){

	//******************************* Nasim added: dlib one-class SVM****************
	// We will use column vectors to store our points.  Here we make a convenient typedef
	// for the kind of vector we will use.
	typedef dlib::matrix<double,0, 1> sample_type; //Nasim: This typedef declares a matrix with unknow(0) rows and 1 column. This is column vector.

	// Then we select the kernel we want to use.  For our present problem the radial basis
  // kernel is quite effective.
	typedef dlib::radial_basis_kernel<sample_type> kernel_type; //Nasim: use same

	std::vector<sample_type> samples;
	sample_type samp(svCol); //Nasim: my data should be m(8), since 8 features every vector

	for (int i = 0; i< svRow; i++)
	{
			samp(0) = svData[i][0];
			samp(1) = svData[i][1];
			samp(2) = svData[i][2];
			samp(3) = svData[i][3];
			samp(4) = svData[i][4];
			samp(5) = svData[i][5];
			samp(6) = svData[i][6];
			samp(7) = svData[i][7];
			samples.push_back(samp);
	}

	//vector_normalizer<sample_type> normalizer;
	// let the normalizer learn the mean and standard deviation of the samples
	//normalizer.train(samples);
	// now normalize each sample
	//for (unsigned long i = 0; i < samples.size(); ++i)
	//	samples[i] = normalizer(samples[i]);

	// Now make the object responsible for training one-class SVMs.
	dlib::svm_one_class_trainer<kernel_type> trainer;

	// Here we set the width of the radial basis kernel
	trainer.set_kernel(kernel_type(sigma)); // Nasim 4.0 is width of rbf kernel (sigma)
	double NU = svRow*fracRejection;
	trainer.set_nu((NU < 1) ? 1 : 1 / NU);
	trainer.set_epsilon(TOL);
	//trainer.set_nu(1/svRow*fracRejection);
	//trainer.set_lambda(0.00001);
	//const double C = get_option(parser, "c", 1.0);
	//trainer.set_c(C);

	// Now train a one-class SVM.  The result is a function, df(),
	dlib::decision_function<kernel_type> df = trainer.train(samples);

	//cout << "\n number of support vectors in our learned_function is "
	//	<< df.basis_vectors.size() << endl;

	//df.alpha;
	//df.b;
	//df.basis_vectors;
	//df.kernel_function;
	//df.kernel_function.gamma;

	//***********************************************************************************
	// initialize mySVClust by allocating memory dynamically
	SVCluster* mySVClust = (SVCluster *)malloc(sizeof(SVCluster));

	// allocating memory for the SVvectors matrix //modified by Nasim
	double **SVvectors = (double **)malloc(df.basis_vectors.size() * sizeof(double *));
	for (int j = 0; j < df.basis_vectors.size(); j++)
	SVvectors[j] = (double *)malloc(svCol * sizeof(double));

	// populate the SVvectors matrix from the model
	for (int i = 0; i < df.basis_vectors.size(); i++) {
		for (int j = 0; j < svCol; j++) {
			SVvectors[i][j] = df.basis_vectors(i)(j);

		}

	}

	double sumAlpha = 0;
	for (int i = 0; i < df.basis_vectors.size(); i++) {
		sumAlpha += df.alpha(i);
	}

	double *alpha = (double *)malloc(df.basis_vectors.size() * sizeof(double));
	for (int i = 0; i < df.basis_vectors.size(); i++) {
		alpha[i] = df.alpha(i) / sumAlpha; //Nasim: not sure about devision by sumAlpha
		//alpha[i] = df.alpha(i); //Nasim

	}

	// allocating memory for the result pauerwise euclidian distance
	double **DistMtx = (double **)malloc(df.basis_vectors.size() * sizeof(double *));
	for (int j = 0; j < df.basis_vectors.size(); j++)
	DistMtx[j] = (double *)malloc(df.basis_vectors.size() * sizeof(double));

	// calculate the distance
	MyDistm(SVvectors, SVvectors, DistMtx, df.basis_vectors.size(), df.basis_vectors.size(), svCol);

	// claculate Kxx (to save memory we reuse DistMtx)
	double Kxx = myExp_alpha(DistMtx, alpha, df.basis_vectors.size(), sigma);

	//------added by Nasim----------------------------------------
	double *Dx = (double *)malloc(df.basis_vectors.size() * sizeof(double));
	for (int i = 0; i < df.basis_vectors.size(); i++) {
		double K = 0;
		for (int j = 0; j < df.basis_vectors.size(); j++) {
			K += alpha[i]*exp(-DistMtx[j][i] / (sigma * sigma));
		}
		Dx[i] = -2 * K;
	}

	double offs = 1 + Kxx;
	double sum = 0;
	for (int i = 0; i < df.basis_vectors.size(); i++) {
		sum += Dx[i];
	}
	double R2 = sum / df.basis_vectors.size(); //
	double threshold = R2 + offs;
	//------------------------------------------------------------------
	// free the memory for DistMtx
	for (int j = 0; j < df.basis_vectors.size(); j++) free(DistMtx[j]);
	free(DistMtx);

	DistMtx = NULL;

	// pack the data in the structure
	mySVClust->SVvectors    = SVvectors;
	mySVClust->alpha        = alpha;
	mySVClust->r			= df.basis_vectors.size();
	mySVClust->c            = svCol;
	mySVClust->Kxx			= Kxx;
	mySVClust->R2           = threshold;//model->r_square;	// had to make changes to the svm library to get access to this
	mySVClust->sigma        = df.kernel_function.gamma;			// redundant
	mySVClust->offsets      = offs;				// // Modified by Nasim

										// free the svm-related dynamic memory
										//free(prob.x);
										//free(prob.y);
										//free(x_space);
										//svm_free_and_destroy_model(&model);
										//svm_destroy_param(&param);

	return mySVClust;
}


#include <boost/numeric/ublas/matrix.hpp>


// Copyright 2017 UBC Sailbot

#include <chrono>
#include <iostream>
#include <iomanip>

#include <boost/program_options.hpp>

#include <pathfinding/HaversineHeuristic.h>
#include <pathfinding/HaversineCostCalculator.h>
#include <pathfinding/WeatherCostCalculator.h>
#include <pathfinding/AStarPathfinder.h>
#include <pathfinding/PathfinderResultPrinter.h>
#include "pathfinding/WeatherHexMap.h"
#include <logic/StandardCalc.h>

#include "network_table_api/NonProtoConnection.h"
#include "network_table_api/Help.h"

constexpr uint8_t kInvalidIndirectNeighbourDepth = static_cast<uint8_t> (-1);

enum class OutputFormat {
  kDefault,
  kKML
};

int start_lat, start_lon, end_lat, end_lon, pointToPrint;
bool preserveKml = false;

void find_neighbours(const HexPlanet &planet, HexVertexId id) {
  std::cout << "Finding neighbours for vertex ID: " << id << std::endl;

  const HexVertex &vertex = planet.vertex(id);
  const std::array<HexVertexId, 6> &neighbours = vertex.neighbours;
  for (size_t i = 0; i < vertex.neighbour_count; i++) {
    std::cout << neighbours[i] << std::endl;
  }
}

void find_coordinates(HexPlanet &planet, const std::vector<HexVertexId> &ids) {
  std::cout << "Finding coordinates for vertices: <id> (<lat>, <lng>)" << std::endl;
  for (HexVertexId id : ids) {
    std::cout << id << " " << planet.vertex(id).coordinate.to_string() << std::endl;
  }
}

Pathfinder::Result run_pathfinder(HexPlanet &planet,
                                  HexVertexId source,
                                  HexVertexId target,
                                  int weather_factor,
                                  bool generate_new_grib,
                                  const std::string & file_name,
                                  int time_steps,
                                  bool use_csvs,
                                  const std::string & output_csvs_folder,
                                  bool silent,
                                  bool verbose) {
  HaversineHeuristic heuristic = HaversineHeuristic(planet);
  WeatherHexMap weather_map = WeatherHexMap(planet, time_steps, start_lat, start_lon, end_lat, end_lon, generate_new_grib, file_name, use_csvs, output_csvs_folder, preserveKml);
  auto wmap_pointer = std::make_unique<WeatherHexMap>(weather_map);
  WeatherCostCalculator cost_calculator = WeatherCostCalculator(planet, wmap_pointer, weather_factor);
  AStarPathfinder pathfinder(planet, heuristic, cost_calculator, source, target, true);

  if (!silent) {
    std::cout << "Pathfinding from " << source << " to " << target << std::endl;
  }
  auto start_time = std::chrono::system_clock::now();

  auto result = pathfinder.Run();

  if (!silent) {
    auto end_time = std::chrono::system_clock::now();
    std::chrono::duration<double> elapsed_seconds = end_time - start_time;
    std::cout << std::fixed
              << "Pathfinding Complete (" << elapsed_seconds.count() << "s)" << std::endl;

    if (verbose) {
      auto stats = pathfinder.stats();
      std::cout << std::fixed
                << "Closed Set: " << stats.closed_set_size << std::endl
                << "Open Set:   " << stats.open_set_size << " (on exit)" << std::endl;
    }

    std::cout << std::endl;
  }

  return result;
}

HexPlanet generate_planet(uint8_t subdivision_level, uint8_t indirect_neighbour_depth, bool silent, bool verbose, bool store_planet, bool use_cached_planet) {
  auto start_time = std::chrono::system_clock::now();
  const std::string path_to_cached_planet = "cached_planets/size_" + std::to_string(subdivision_level) + ".txt";
  if (!silent) {
    if (use_cached_planet) {
      std::cout << "Looking for cached planet at " << path_to_cached_planet << std::endl;
    }
    else {
      std::cout << "Generating HexPlanet of Size: " << static_cast<int>(subdivision_level) << std::endl;
    }
  }
  HexPlanet planet = (use_cached_planet) ? HexPlanet(path_to_cached_planet) :
                     (indirect_neighbour_depth != kInvalidIndirectNeighbourDepth) ?
                     HexPlanet(subdivision_level, indirect_neighbour_depth) :
                     HexPlanet(subdivision_level);

  if (!silent) {
    auto end_time = std::chrono::system_clock::now();
    std::chrono::duration<double> elapsed_seconds = end_time - start_time;
    std::cout << std::fixed
              << "Planet Generation Complete (" << elapsed_seconds.count() << "s)" << std::endl;

    if (verbose) {
      std::cout << std::fixed
                << "Vertices:  " << planet.vertex_count() << std::endl
                << "Triangles: " << planet.triangle_count() << std::endl;
    }

    std::cout << std::endl;
  }

  if (store_planet)
  {
    if (!silent) {
      std::cout << "Storing planet at " << path_to_cached_planet << std::endl;
    }
    planet.WriteToFile(path_to_cached_planet);
  }

  return planet;
}

int main(int argc, char const *argv[]) {
  try {
    boost::program_options::options_description desc{"Options"};
    desc.add_options()
        ("help,h", "Help screen")
        ("v,verbose", "Verbose output")
        ("s,silent", "Silence useful output")
        ("grib", boost::program_options::value<std::string>(),
         "Relative path to grb file for weather info. If not given, uses new weather data download.")
        ("input_csvs", boost::program_options::value<std::string>(),
         "Relative path to folder from which to read in csvs as weather data.")
        ("output_csvs", boost::program_options::value<std::string>(),
         "Relative path to existing folder in which weather data csvs will be created")
        ("p,planet_size", boost::program_options::value<int>()->default_value(1), "Planet Size")
        ("w,weather_factor", boost::program_options::value<int>()->default_value(1500), "Weather Factor")
        ("n,neighbour", boost::program_options::value<HexVertexId>(), "Vertex to find neighbours")
        ("i,indirect", boost::program_options::value<int>(), "Indirect neighbour depth")
        ("t,time_steps", boost::program_options::value<int>()->default_value(4), "Max time steps for wind speed")
        ("c,coordinates",
         boost::program_options::value<std::vector<HexVertexId>>()->multitoken(),
         "Vertices for which to find GPS Coordinates")
        ("f,find_path",
         boost::program_options::value<std::vector<HexVertexId>>()->multitoken(),
         "<start> <end> Vertex IDs")
        ("table", "Connect to network table")
        ("navigate",
         boost::program_options::value<std::vector<double>>()->multitoken(),
         "<start_latitude> <start_longitude> <end_latitude> <end_longitude>")
        ("kml", "Output the a KML file for the pathfinding result")
        ("store_planet", "Output the a file to store the planet as a cache")
        ("use_cached_planet", "Use cached_planet in cached_planets/size_<size>.txt")
        ("printn", boost::program_options::value<int>(), "Output the nth coordinate pair at the end of the program, starting with 1")
        ("save", "Save the current weather as a timestamped KML")
        ("hardcoded", boost::program_options::value<std::string>(), "Default use: --hardcoded {Month}, {Month} = Oct, Nov, Dec etc.");

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

    if (vm.count("help") || vm.size() <= 1) {
      // Show help if no non-default args are passed (p is always defined).
      std::cout << desc;
      return EXIT_FAILURE;
    }

    bool generate_new_grib = true;
    bool use_csvs = false;
    std::string file_name = "data.grb";
    std::string output_csvs_folder = (vm.count("output_csvs")) ? vm["output_csvs"].as<std::string>() : "";
    if (vm.count("grib")) {
      generate_new_grib = false;
      file_name = vm["grib"].as<std::string>();
      use_csvs = false;
    } else if (vm.count("input_csvs")) {
      generate_new_grib = false;
      file_name = vm["input_csvs"].as<std::string>();
      use_csvs = true;
    }

    bool silent = vm.count("s") > 0;
    OutputFormat format = OutputFormat::kDefault;

    if (vm.count("kml") > 0) {
      silent = true;
      format = OutputFormat::kKML;
    }

    if (vm.count("printn") > 0) {
      pointToPrint = (vm["printn"].as<int>());
    }

    else {
      pointToPrint = 0;
    }

    if (vm.count("save") > 0) {
      preserveKml = true;
    }

    bool verbose = vm.count("v") > 0 && !silent;

    uint8_t planet_size = static_cast<uint8_t> (vm["p"].as<int>());
    uint8_t indirect_neighbour_depth = (vm.count("i") > 0) ? static_cast<uint8_t> (vm["i"].as<int>())
                                                           : kInvalidIndirectNeighbourDepth;
    const bool store_planet = vm.count("store_planet") > 0;
    const bool use_cached_planet = vm.count("use_cached_planet") > 0;
    HexPlanet planet = generate_planet(planet_size, indirect_neighbour_depth, silent, verbose, store_planet, use_cached_planet);

    int time_steps = vm["t"].as<int>();

    int weather_factor = vm["w"].as<int>() * std::pow(2,10-planet_size);

    if (vm.count("n")) {
      find_neighbours(planet, vm["n"].as<HexVertexId>());
    } else if (vm.count("c")) {
      find_coordinates(planet, vm["c"].as<std::vector<HexVertexId>>());
    } else if (vm.count("f")) {
      auto points = vm["f"].as<std::vector<HexVertexId>>();

      if (points.size() != 2) {
        throw std::runtime_error("Pathfinding requires two hex IDs: <start> <end>");
      }

      auto result = run_pathfinder(planet, points[0], points[1], weather_factor, generate_new_grib, file_name,
                                   time_steps, use_csvs, output_csvs_folder, silent, verbose);

      switch (format) {
        case OutputFormat::kDefault:
          // Print default
          std::cout << PathfinderResultPrinter::PrintDefault(result);

          if (verbose) {
            std::cout << std::endl;
            std::cout << PathfinderResultPrinter::PrintCoordinates(planet, result);
          }
          break;
        case OutputFormat::kKML:
          // Print KML
          std::cout << PathfinderResultPrinter::PrintKML(planet, result, weather_factor, file_name, time_steps, use_csvs, output_csvs_folder, pointToPrint, preserveKml);
          break;
      }
    } else if (vm.count("navigate")) {
      // Find a path betweeen two GPS coordinates, print in KML format
      //TODO() Enable Inputs to be in degrees West/South
      auto points = vm["navigate"].as<std::vector<double>>();

      NetworkTable::NonProtoConnection connection;
      if (vm.count("table")) {
        // Connect to the network table
        int error_count = 0;

        try {
            std::cout << "Connecting to network table" <<std::endl;
            connection.Connect(100);
        } catch (NetworkTable::TimeoutException) {
            std::cout << "Failed to connect" << std::endl;
            ++error_count;
        }

        std::pair<double, double> gps_coords;
        try {
            gps_coords = connection.GetCurrentGpsCoords();
        } catch (NetworkTable::NodeNotFoundException ex) {
            connection.Disconnect();
            std::cout << "Gps Coords Not Found in Network Table" << std::endl;
            ++error_count;
        }

        if (error_count == 0) {
          start_lat = (int) gps_coords.first;
          start_lon = (int) gps_coords.second;
        } else {
          start_lat = (points[0]);
          start_lon = (points[1]);
        }

      } else {
        start_lat = (points[0]);
        start_lon = (points[1]);
      }

      end_lat = (points[2]);
      end_lon = (points[3]);

      auto adj_start_lon = start_lon < 0 ? start_lon : start_lon - 360;
      auto adj_end_lon = end_lon < 0 ? end_lon : end_lon - 360;

      const GPSCoordinateFast start_coord(start_lat*10000000, adj_start_lon*10000000);
      const GPSCoordinateFast end_coord(end_lat*10000000, adj_end_lon*10000000);

      Eigen::Vector3f start_point;
      Eigen::Vector3f end_point;

      standard_calc::CoordToPoint(start_coord, &start_point);
      standard_calc::CoordToPoint(end_coord, &end_point);

      HexVertexId start_vertex = planet.HexVertexFromPoint(start_point);
      HexVertexId end_vertex = planet.HexVertexFromPoint(end_point);

      auto result = run_pathfinder(planet, start_vertex, end_vertex, weather_factor, generate_new_grib, file_name,
                                   time_steps, use_csvs, output_csvs_folder, silent, verbose);

      std::vector<std::pair<double, double>> waypoints;

      if (vm.count("hardcoded")) {
       try{
         waypoints = PathfinderResultPrinter::GetHardcoded(vm["hardcoded"].as<std::string>());
         std::cout << PathfinderResultPrinter::PrintHardcoded(waypoints);
       }
       catch(std::string test_name){
         std::cout << "Error: " << test_name << " is not a valid hardcoded test" << std::endl;
       }
      } else {
        waypoints = PathfinderResultPrinter::GetVector(planet, result);
      }

      if (vm.count("table")) {
        try {
          connection.SetWaypointValues(waypoints);
          connection.Disconnect();
        }
        catch(NetworkTable::TimeoutException){
          std::cout << "Could not set waypoint values" << std::endl;
        }
      } else if (!vm.count("hardcoded")) {
        std::cout << PathfinderResultPrinter::PrintKML(planet, result, weather_factor, file_name, time_steps, use_csvs, output_csvs_folder, pointToPrint, preserveKml);
      }

    } else {
      std::cerr << "Invalid Program options." << std::endl
                << desc;
      return EXIT_FAILURE;
    }
  } catch (const boost::program_options::error &ex) {
    std::cerr << ex.what() << std::endl;
    return EXIT_FAILURE;
  } catch (const boost::bad_any_cast &ex) {
    std::cerr << ex.what() << std::endl;
    return EXIT_FAILURE;
  } catch (const std::runtime_error &ex) {
    std::cerr << "Pathfinding Error:" << std::endl;
    std::cerr << ex.what() << std::endl;
    return EXIT_FAILURE;
  }
}