#pragma once

#include <stdio.h>
#include <stdlib.h>
#include <time.h>
#include <cfloat>
#include <math.h>
#include <vector>
#include <ctime>
#include <cstdlib>
#include <unistd.h>
#include <iostream>
#include <fstream>
#include <string>
#include <cstddef>
#include <nlopt.hpp>
#include <pangolin/pangolin.h>
#include <opencv2/opencv.hpp>
#include <opencv2/opencv_modules.hpp>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <assimp/cimport.h>
#include <assimp/scene.h>
#include <assimp/postprocess.h>
#include <assimp/Importer.hpp>

#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/registration/icp.h>
#include <pcl/registration/icp_nl.h>
#include <pcl/features/normal_3d.h>
#include <pcl/common/geometry.h>
#include <pcl/features/normal_3d_omp.h>
#include <pcl/features/crh.h>
#include <pcl/kdtree/kdtree_flann.h>

#include <df/camera/camera.h>
#include <df/camera/linear.h>
#include <df/camera/poly3.h>
#include <df/camera/rig.h>
#include <df/image/backprojection.h>
#include <df/prediction/glRender.h>
#include <df/prediction/glRenderTypes.h>
#include <df/util/args.h>
#include <df/util/glHelpers.h>
#include <df/util/pangolinHelpers.h>
#include <df/util/tensor.h>
#include <df/optimization/icp.h>

#include <ros/ros.h>
#include <geometry_msgs/Point32.h>

typedef pcl::PointXYZ PointT;
typedef pcl::PointCloud<PointT> PointCloud;
typedef pcl::PointNormal PointNormalT;
typedef pcl::PointCloud<PointNormalT> PointCloudWithNormals;
typedef Eigen::Matrix<float,3,1,Eigen::DontAlign> Vec3;

template <typename Derived>
inline void operator >>(std::istream & stream, Eigen::MatrixBase<Derived> & M)
{

    for (int r = 0; r < M.rows(); ++r) {
        for (int c = 0; c < M.cols(); ++c) {
            stream >> M(r,c);
        }
    }

}

struct DataForOpt
{
  int width, height, objID;

  std::vector<pangolin::GlBuffer*> attributeBuffers;
  std::vector<pangolin::GlBuffer*> modelIndexBuffers;
  df::GLRenderer<df::VertAndNormalRenderType>* renderer;
  pangolin::View* view;
  df::Poly3CameraModel<float>* model;

  const int* labelmap;
  std::vector<int>* label_indexes;
  Eigen::Vector2f depthRange;
  df::ManagedHostTensor2<Vec3>* vertex_map;
  df::ManagedHostTensor2<Eigen::UnalignedVec4<float> >* predicted_verts;
  df::ManagedDeviceTensor2<Eigen::UnalignedVec4<float> >* predicted_verts_device;
};

class Synthesizer
{
 public:

  Synthesizer(std::string model_file, std::string pose_file);
  ~Synthesizer();

  void setup(int width, int height);
  void create_window(int width, int height);
  void destroy_window();
  void loadModels(std::string filename);
  void loadPoses(const std::string filename);
  aiMesh* loadTexturedMesh(const std::string filename, std::string & texture_name);
  void initializeBuffers(int model_index, aiMesh* assimpMesh, std::string textureName,
    pangolin::GlBuffer & vertices, pangolin::GlBuffer & canonicalVertices, pangolin::GlBuffer & colors, pangolin::GlBuffer & normals,
    pangolin::GlBuffer & indices, pangolin::GlBuffer & texCoords, pangolin::GlTexture & texture, bool is_textured);

  // pose refinement with ICP
  void refineDistance(const int* labelmap, unsigned char* depth, int height, int width, float fx, float fy, float px, float py, float znear, float zfar,
                float factor, int num_roi, int channel_roi, const float* rois, const float* poses, float* outputs, float* outputs_icp,
                std::vector<std::vector<geometry_msgs::Point32> >& output_points, float maxError);
  void solveICP(const int* labelmap, unsigned char* depth, int height, int width, float fx, float fy, float px, float py, float znear, float zfar,
                float factor, int num_roi, int channel_roi, const float* rois, const float* poses, float* outputs, float* outputs_icp, float maxError);
  void visualizePose(int height, int width, float fx, float fy, float px, float py, float znear, float zfar, const float* rois, float* outputs, int num_roi, int channel_roi);
  double poseWithOpt(std::vector<double> & vec, DataForOpt data, int iterations);
  void refinePose(int width, int height, int objID, float znear, float zfar,
                  const int* labelmap, DataForOpt data, df::Poly3CameraModel<float> model, Sophus::SE3f & T_co, int iterations, float maxError, int is_icp);

 private:
  int counter_;
  int setup_;
  std::string model_file_, pose_file_;

  df::ManagedDeviceTensor2<int>* labels_device_;

  // depths
  float depth_factor_;
  float depth_cutoff_;
  std::vector<int> label_indexes_;
  df::ManagedTensor<2, float>* depth_map_;
  df::ManagedTensor<2, float, df::DeviceResident>* depth_map_device_;

  // 3D points
  df::ManagedDeviceTensor2<Vec3>* vertex_map_device_;
  df::ManagedHostTensor2<Vec3>* vertex_map_;
  df::ManagedDeviceTensor2<Eigen::UnalignedVec4<float> >* predicted_verts_device_;
  df::ManagedDeviceTensor2<Eigen::UnalignedVec4<float> >* predicted_normals_device_;
  df::ManagedHostTensor2<Eigen::UnalignedVec4<float> >* predicted_verts_;
  df::ManagedHostTensor2<Eigen::UnalignedVec4<float> >* predicted_normals_;

  // poses
  std::vector<float*> poses_;
  std::vector<int> pose_nums_;
  std::vector<bool> is_textured_;

  // rois
  std::vector<std::vector<cv::Vec<float, 12> > > rois_;

  // 3D models
  std::vector<aiMesh*> assimpMeshes_;

  // pangoline views
  pangolin::View* gtView_;

  // buffers
  std::vector<pangolin::GlBuffer> texturedVertices_;
  std::vector<pangolin::GlBuffer> canonicalVertices_;
  std::vector<pangolin::GlBuffer> vertexColors_;
  std::vector<pangolin::GlBuffer> vertexNormals_;
  std::vector<pangolin::GlBuffer> texturedIndices_;
  std::vector<pangolin::GlBuffer> texturedCoords_;
  std::vector<pangolin::GlTexture> texturedTextures_;

  df::GLRenderer<df::CanonicalVertRenderType>* renderer_;
  df::GLRenderer<df::VertAndNormalRenderType>* renderer_vn_;
};


//Copyright Florian Goujeon 2021.
//Distributed under the Boost Software License, Version 1.0.
//(See accompanying file LICENSE or copy at
//https://www.boost.org/LICENSE_1_0.txt)
//Official repository: https://github.com/fgoujeon/fsm-benchmark

#define BOOST_MPL_CFG_NO_PREPROCESSED_HEADERS
#define BOOST_MPL_LIMIT_VECTOR_SIZE PROBLEM_SIZE_X_2
#define BOOST_MPL_LIMIT_MAP_SIZE PROBLEM_SIZE_X_2

#include "common.hpp"
#include <boost/msm/back/state_machine.hpp>
#include <boost/msm/front/state_machine_def.hpp>
#include <boost/msm/front/functor_row.hpp>

namespace msm = boost::msm;
namespace mpl = boost::mpl;
using namespace msm::front;

template<int Index>
struct state_transition_event
{
    int data = 1;
};

struct internal_transition_event
{
    int data = 1;
};

template<int Index>
struct state_tpl: msm::front::state<>
{
    template<class Event, class Fsm>
    void on_entry(const Event& event, Fsm& fsm)
    {
        fsm.process_event(internal_transition_event{});
    }
};

template<int Index>
struct state_transition_action
{
    template<class Event, class Fsm, class SourceState, class TargetState>
    void operator()(const Event& evt, Fsm& sm, SourceState&, TargetState&)
    {
        sm.counter += evt.data;
    }
};

template<int Index>
struct internal_transition_action
{
    template<class Event, class Fsm, class SourceState, class TargetState>
    void operator()(const Event& evt, Fsm& sm, SourceState&, TargetState&)
    {
        sm.counter += evt.data;
    }
};

template<int Index>
struct guard
{
    template<class Event, class Fsm, class SourceState, class TargetState>
    bool operator()(const Event& evt, Fsm& sm, SourceState&, TargetState&)
    {
        return evt.data >= 0;
    }
};

struct fsm_: public msm::front::state_machine_def<fsm_>
{
    using initial_state = state_tpl<0>;

    struct transition_table: mpl::vector
    <
#define X(N) \
        COMMA_IF_NOT_0(N) Row<state_tpl<N>, state_transition_event<N>, state_tpl<(N + 1) % PROBLEM_SIZE>, state_transition_action<N>, guard<N>> \
        , Row<state_tpl<N>, internal_transition_event, none, internal_transition_action<N>>
        COUNTER
#undef X
    >{};

    int counter = 0;
};

using fsm = msm::back::state_machine<fsm_>;

int test()
{
    auto sm = fsm{};

    sm.start();

    for(auto i = 0; i < test_loop_size; ++i)
    {
#define X(N) \
    sm.process_event(state_transition_event<N>{});
        COUNTER
#undef X
    }

    //The entry action of state0 is called by the constructor of the FSM, so we
    //have to cancel what this action does.
    --sm.counter;

    return sm.counter;
}


#include <iostream>
#include <fstream>
#include <Eigen/Dense>
#include "Graph.h"

namespace GraphLib
{
#ifdef GRAPH_ENABLE_DEBUG
	size_t Graph::instanceCount = 0;
#endif

	double Graph::dampingFactor = 0.85;

	Graph::Graph(bool directed)
	{
		this->directed = directed;
		adjMatrix = NULL;
		comment = NULL;
#ifdef GRAPH_ENABLE_DEBUG
		std::cout << "Creating an instance of Graph\n";
		instanceCount++;
#endif
	}

	Graph::~Graph()
	{
		for(std::size_t i=0; i<vertexes.size(); i++)
		{
			Node* n = vertexes[i];
			delete n;
		}

		delete adjMatrix;

		vertexes.clear();
		edges.clear();

#ifdef GRAPH_ENABLE_DEBUG
		std::cout << "Removing an instance of Graph\n";
		instanceCount--;
#endif
	}

	Graph::EdgeSet Graph::getEdges()
	{
		return edges;
	}

	/* number of nodes */
	std::size_t Graph::getNodesCount()const
	{
		return vertexes.size();
	}

	/* number of edges */
	std::size_t Graph::getEdgesCount()const
	{
		return edges.size();
	}

	size_t& Graph::createNode()
	{
		Node* n = new Node();
		addNode(n);
		return n->getIndex();
	}

	Node* Graph::nodeAt(std::size_t n)
	{
		assert(n<vertexes.size());
		return vertexes[n];
	}

	bool Graph::link(std::size_t n1, std::size_t n2)
	{
		assert((n1<vertexes.size()) && (n2<vertexes.size()));
		assert(n1!=n2);

		Node *node1 = vertexes[n1], *node2 = vertexes[n2];
		Edge *e = NULL;

		e = new Edge(node1, node2);

		if(!addEdge(e))
		{
			delete e;
			return false;
		}
		return true;
	}

	bool Graph::unlink(std::size_t n1, std::size_t n2)
	{
		assert(n1<vertexes.size());
		assert(n2<vertexes.size());
		assert(n1!=n2);

		Node *node1 = vertexes[n1], *node2 = vertexes[n2];

		Edge *e = NULL;

		e = new Edge(vertexes[n1], vertexes[n2]);

		EdgeSet::const_iterator it = edges.find(e);

		//std::cout << (it==edges.end()) << " X\n";

		if(it == edges.end())
			return false;

		edges.erase(e);
		delete e;

		return true;
	}

	bool Graph::areLinked(std::size_t n1, std::size_t n2)
	{
		assert(n1<vertexes.size());
		assert(n2<vertexes.size());
		assert(n1!=n2);

		Edge *e = NULL;

		e = new Edge(vertexes[n1], vertexes[n2]);

		EdgeSet::const_iterator it = edges.find(e);
		if(it == edges.end())
			return false;

		return true;

	}

	void Graph::removeNode(size_t ref)
	{
		assert(ref<vertexes.size());
		Node *n = vertexes[ref];
		removeNode(n);
	}

	/* Add a node to the graph, used internally */
	bool Graph::addNode(Node *node)
	{
		node->setIndex(vertexes.size());
		for( std::size_t i = 0; i < vertexes.size(); i++)
			if (node == vertexes[i])
				return false;

		vertexes.push_back(node);
		return true;
	}

	/* Removes a node from the graph, used internally  */
	void Graph::removeNode(Node *node)
	{
		std::size_t size = vertexes.size();
		std::size_t pos = node->getIndex();

		vertexes.erase(vertexes.begin()+pos);

		std::vector<Edge*> toRemove;

		for(auto e: edges)
		{
			if(e->contains(node))
				toRemove.push_back(e);
		}

		std::size_t s = toRemove.size();

		for(std::size_t i=0; i<s; i++)
		{
			Edge *e  = toRemove[i];
			edges.erase(e);
			delete e;
		}

		/* Update other nodes indexes */
		for(std::size_t i=pos; i<size-1; i++)
			vertexes[i]->setIndex(i);

		node->setIndex(-1);
		delete node;
	}

	bool Graph::addEdge(Edge *edge)
	{
		EdgeSet::const_iterator e = edges.find(edge);

		if(e != edges.end())
			return false;

		std::size_t start = edge->start->getIndex();
		std::size_t end   = edge->end->getIndex();

		if (start == -1 || end == -1)
			return false;

		edges.insert(edge);
		return true;
	}

	bool Graph::removeEdge(Edge *edge)
	{
		EdgeSet::const_iterator e = edges.find(edge);
		if(e == edges.end())
			return false;

		edges.erase(edge);
		return true;
	}

	void Graph::calculateAjdMatrix()
	{
		std::size_t SIZE = vertexes.size();

		adjMatrix = new MatrixXb(SIZE, SIZE);

		for(std::size_t i = 0; i < SIZE; i++)
		{
			for(std::size_t j = 0; j < SIZE; j++)
			{
				(*adjMatrix)(i, j) = false;
			}
		}

		for(const auto& e: edges)
		{
			std::size_t i1 = e->start->getIndex();
			std::size_t i2 = e->end->getIndex();
			(*adjMatrix)(i1, i2) = true;
			if(!this->directed)
				(*adjMatrix)(i2, i1) = true;
		}
	}

	void Graph::printAdjMatrix()
	{
		assert(adjMatrix != NULL);

		auto adjMat = *adjMatrix;

		std::size_t SIZE = vertexes.size();
		for( std::size_t i = 0; i < SIZE; i++)
		{
			for(std::size_t j = 0; j < SIZE; j++)
			{
				std::cout << "\t" << adjMat(i, j);
			}
			std::cout << "\n";
		}
	}

	bool Graph::get(std::size_t n1, std::size_t n2)
	{
		assert(adjMatrix != NULL);
		assert(n1<vertexes.size());
		assert(n2<vertexes.size());

		return (*adjMatrix)(n1, n2);
	}

	/* Initializes every node with a rank value 1/numberOfNodes */

	void Graph::calculatePageRank()
	{
		assert(adjMatrix != NULL);

		auto adjMat = *adjMatrix;

		/* initial value */
		double ival = 1.f/vertexes.size();

		/* This vector will contain the number of edges in which each node participates */
		std::vector <std::size_t> nodeEdgesCount;

		/* Step 0 (Optimisation) */
		for(std::size_t i = 0; i < vertexes.size(); i++)
		{
			nodeEdgesCount.push_back(0);

			for(std::size_t j = 0; j < vertexes.size(); j++)
			{
				if (adjMat(i, j)){
					nodeEdgesCount[i] ++;
				}
			}
		}

		/* Step 1 Initialize all nodes PR to 1/numberOfNodes */
		for(std::size_t i = 0; i < vertexes.size(); i++)
		{
			vertexes[i]->setRank(ival);
		}

		/* Step 2 Loop 100x to get a good page rank value. */
		for(std::size_t k = 0; k <100; k++)
		{
			/* Calculate PR of all nodes */
			for(std::size_t i = 0; i < vertexes.size(); i++)
			{

				double sum = 0;

				/* calculate the sum of RP(Ti)/L(Ti) where Ti links to the current node */
				for(std::size_t j = 0; j < vertexes.size(); j++)
				{
					/* node i must be targeted not the source */
					if (adjMat(j, i))
					{
						sum += vertexes[j]->getRank()/nodeEdgesCount[j];
					}
				}
				vertexes[i]->setRank( (1-dampingFactor) + dampingFactor * sum );
			}

		}
	}

	void Graph::calculatePageRankMatrix()
	{
		assert(adjMatrix != NULL);

		auto adjMat = *adjMatrix;

		std::vector<double> R;
		std::size_t SIZE = vertexes.size();
		Eigen::MatrixXd M(SIZE, SIZE);

		/* This vector will contain the number of edges in which each node participates */
		std::vector <std::size_t> nodeEdgesCount;

		/* Step 0 (Optimisation) */
		for(std::size_t i = 0; i < SIZE; i++)
		{
			nodeEdgesCount.push_back(0);

			for(std::size_t j = 0; j < SIZE; j++)
			{
				if (adjMat(i, j)){
					nodeEdgesCount[i] ++;
				}
			}
		}

		/* Creating M matrix */
		for(std::size_t i = 0; i < SIZE; i++)
		{
			for(std::size_t j = 0; j < SIZE; j++)
			{
				M(i, j) = 0;
				if(adjMat(j,i))
					M(i, j) = (1.0f/nodeEdgesCount[j]);
			}
		}

		/* Creating R matrix */
		for(std::size_t i = 0; i < SIZE; i++)
		{
			R.push_back(1./SIZE);
		}

		for(std::size_t k = 0; k < 100; k++)
		{
			std::vector<double> tmp(SIZE);

			for(std::size_t i = 0; i < SIZE; i++)
			{
				double value = 0;
				for(std::size_t j = 0; j < SIZE; j++)
				{
					value += M(i, j) * R[j];
				}
				tmp[i] = value *dampingFactor +(1.0f-dampingFactor);
			}
			R = tmp;
		}

		for(std::size_t i = 0; i < R.size(); i++)
		{
			vertexes[i]->setRank(R[i]);
		}

	}

	double Graph::getRank(std::size_t n)
	{
		assert(n<vertexes.size());
		double rank = vertexes[n]->getRank();
		/* To make sure this is called after calculating PR */
		assert(rank >= 0);
		return rank;
	}

	void Graph::printPageRank() const
	{
		double sum = 0;
		for( std::size_t i = 0; i < vertexes.size(); i++)
		{
			std::cout << "\tNode " << i << ":\t\t" << vertexes[i]->getRank() << "\n";
			sum += vertexes[i]->getRank();
		}
		std::cout << "\n\n\tTotal PR: " << sum << "\n\n";
	}

	void Graph::saveGML(wchar_t *url)
	{
		std::cout << "\n\tSaving GML to \""<< url << "\"...\n";
		std::ofstream out;
		out.open(url);
		out << "graph [";

		if(comment != NULL)
			out << "\tcomment \"" << comment << "\"\n";
		out << "\tdirected 0\n";

		for(std::size_t i = 0; i < vertexes.size(); i++)
		{
			out << "\tnode [";
			out << "\t\tid " << i << '\n';
			out << "\t\tlabel \"" << i << "\"\n";
			out << "\t]\n";
		}

		for(auto e: edges)
		{
			out << "\tedge [";
			out << "\t\tsource " << e->start->getIndex() << '\n';
			out << "\t\ttarget " << e->end->getIndex() << '\n';
			out << "\t]\n";
		}

		out << "]";

		out.close();
	}

	void Graph::saveDOT(wchar_t *url)
	{
		std::cout << "\n\tSaving DOT to \""<< url << "\"...\n";
		std::ofstream out;
		out.open(url);
		out << "graph g{\n";
		for(auto e: edges)
		{
			out << "\t" << e->start->getIndex() << " -- " << e->end->getIndex() << ";\n";
		}

		out << "}";

		out.close();
	}

	void Graph::saveCSV(wchar_t *url)
	{
		std::cout << "saving file\n";
		std::ofstream out;
		out.open(url);
		out << "X, ";
		for(std::size_t i = 0; i < vertexes.size(); i++)
		{
			out << "Node " << i << ',';
		}

		for(std::size_t i = 0; i < vertexes.size(); i++)
		{
			out << "\nNode " << i << ',';
			for(std::size_t j = 0; j < vertexes.size(); j++)
			{
				out << (*adjMatrix)(i, j) << ',';
			}
		}
		out << "\n\n";

		double sum = 0;
		for( std::size_t i = 0; i < vertexes.size(); i++)
		{
			out << "Node " << i << ',' << vertexes[i]->getRank() << '\n';
			sum += vertexes[i]->getRank();
		}

		out << "\n\nTotal PR," << sum << "\n\n";

		out.close();
	}

}


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
/*!
 * \file
**/
#ifndef BOOST_SIMD_BOOLEAN_FUNCTIONS_IF_ELSE_ZERO_HPP_INCLUDED
#define BOOST_SIMD_BOOLEAN_FUNCTIONS_IF_ELSE_ZERO_HPP_INCLUDED
#include <boost/simd/include/functor.hpp>
#include <boost/dispatch/include/functor.hpp>
/*!
 * \ingroup boost_simd_boolean
 * \defgroup boost_simd_boolean_if_else_zero if_else_zero
 *
 * \par Description
 * If a0 is true returns a1 else returns zero
 * \par
 * The first operand must be a boolean type according to
 * the SIMD architecture compatible to the  other one.
 * the result type is the same as the second parameter one.
 *
 * \par Header file
 *
 * \code
 * #include <nt2/include/functions/if_else_zero.hpp>
 * \endcode
 *
 * \par Alias
 * \arg ifelsezero
 * \arg ifnot_zero_else
 * \arg ifnotzeroelse
 *
 * \synopsis
 *
 * \code
 * namespace boost::simd
 * {
 *   template <class A0,class A1>
 *     meta::call<tag::if_else_zero_(A0,A1)>::type
 *     if_else_zero(const A0 & a0,const A1 & a1);
 * }
 * \endcode
 *
 * \param a0 the first parameter of if_else_zero
 * \param a1 the second parameter of if_else_zero
 *
 * \return a value of the type of the second parameter
 *
 * \par Notes
 * In SIMD mode, this function acts elementwise on the inputs vectors elements
 * \par
 *
**/

namespace boost { namespace simd { namespace tag
  {
    /*!
     * \brief Define the tag if_else_zero_ of functor if_else_zero
     *        in namespace boost::simd::tag for toolbox boost.simd.boolean
    **/
    struct if_else_zero_ : ext::elementwise_<if_else_zero_> { typedef ext::elementwise_<if_else_zero_> parent; };
  }
  BOOST_DISPATCH_FUNCTION_IMPLEMENTATION(tag::if_else_zero_, if_else_zero, 2)
  BOOST_DISPATCH_FUNCTION_IMPLEMENTATION(tag::if_else_zero_, ifelsezero, 2)
  BOOST_DISPATCH_FUNCTION_IMPLEMENTATION(tag::if_else_zero_, ifnot_zero_else, 2)
  BOOST_DISPATCH_FUNCTION_IMPLEMENTATION(tag::if_else_zero_, ifnotzeroelse, 2)
} }

#endif


/*!
 * \file training_data.cc
 *
 * \author Ethan Adams
 * \date
 *
 * This file contains the cpp version of TrainingData.py
 */

#include "BingoCpp/training_data.h"
#include <iostream>
#include <stdlib.h>
#include <Eigen/Dense>
#include <Eigen/Core>
#include "BingoCpp/utils.h"

namespace bingo {

ExplicitTrainingData::ExplicitTrainingData(Eigen::ArrayXXd vx,
    Eigen::ArrayXXd vy) {
  x = vx;
  y = vy;
}

ExplicitTrainingData* ExplicitTrainingData:: get_item(std::list<int> items) {
  int i = 0;
  Eigen::ArrayXXd temp_x(items.size(), x.cols());
  Eigen::ArrayXXd temp_y(items.size(), y.cols());

  for (std::list<int>::iterator it = items.begin(); it != items.end(); ++it) {
    temp_x.block(i, 0, 1, x.cols()) = x.block(*it, 0, 1, x.cols());
    temp_y.block(i, 0, 1, y.cols()) = y.block(*it, 0, 1, y.cols());
    ++i;
  }

  ExplicitTrainingData* temp = new ExplicitTrainingData(temp_x, temp_y);
  return temp;
}

ImplicitTrainingData::ImplicitTrainingData(Eigen::ArrayXXd vx) {
  std::vector<Eigen::ArrayXXd> temp = calculate_partials(vx);
  x = temp[0];
  dx_dt = temp[1];
}

ImplicitTrainingData::ImplicitTrainingData(Eigen::ArrayXXd vx,
    Eigen::ArrayXXd vdx_dt) {
  x = vx;
  dx_dt = vdx_dt;
}

ImplicitTrainingData* ImplicitTrainingData:: get_item(std::list<int> items) {
  int i = 0;
  Eigen::ArrayXXd temp_x(items.size(), x.cols());
  Eigen::ArrayXXd temp_dx_dt(items.size(), dx_dt.cols());

  for (std::list<int>::iterator it = items.begin(); it != items.end(); ++it) {
    temp_x.block(i, 0, 1, x.cols()) = x.block(*it, 0, 1, x.cols());
    temp_dx_dt.block(i, 0, 1, dx_dt.cols()) = dx_dt.block(*it, 0, 1, dx_dt.cols());
    ++i;
  }

  ImplicitTrainingData* temp = new ImplicitTrainingData(temp_x, temp_dx_dt);
  return temp;
}
} // namespace bingo


/*
 * Copyright (c) 2019, The Regents of the University of California (Regents).
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are
 * met:
 *
 *    1. Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *
 *    2. Redistributions in binary form must reproduce the above
 *       copyright notice, this list of conditions and the following
 *       disclaimer in the documentation and/or other materials provided
 *       with the distribution.
 *
 *    3. Neither the name of the copyright holder nor the names of its
 *       contributors may be used to endorse or promote products derived
 *       from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS AS IS
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 * Please contact the author(s) of this library if you have any questions.
 * Authors: David Fridovich-Keil   ( dfk@eecs.berkeley.edu )
 */

///////////////////////////////////////////////////////////////////////////////
//
// Check whether or not a particular set of strategies is a local Nash
// equilibrium.
//
///////////////////////////////////////////////////////////////////////////////

#include <ilqgames/cost/player_cost.h>
#include <ilqgames/dynamics/multi_player_flat_system.h>
#include <ilqgames/dynamics/multi_player_integrable_system.h>
#include <ilqgames/utils/compute_strategy_costs.h>
#include <ilqgames/utils/operating_point.h>
#include <ilqgames/utils/quadratic_cost_approximation.h>
#include <ilqgames/utils/strategy.h>
#include <ilqgames/utils/types.h>

#include <glog/logging.h>
#include <Eigen/Dense>
#include <random>
#include <vector>

namespace ilqgames {

bool NumericalCheckLocalNashEquilibrium(
    const std::vector<PlayerCost>& player_costs,
    const std::vector<Strategy>& strategies,
    const OperatingPoint& operating_point,
    const MultiPlayerIntegrableSystem& dynamics, const VectorXf& x0,
    Time time_step, float max_perturbation, bool open_loop) {
  CHECK_EQ(strategies.size(), player_costs.size());
  CHECK_EQ(strategies.size(), dynamics.NumPlayers());
  CHECK_EQ(x0.size(), dynamics.XDim());

  const size_t num_time_steps = strategies[0].Ps.size();
  CHECK_EQ(num_time_steps, strategies[0].alphas.size());

  // Compute nominal equilibrium cost and be sure to use only 1-step Euler
  // integration.
  const bool was_integrating_using_euler =
      MultiPlayerIntegrableSystem::IntegrationUsesEuler();
  if (!was_integrating_using_euler)
    MultiPlayerIntegrableSystem::IntegrateUsingEuler();
  const std::vector<float> nominal_costs =
      ComputeStrategyCosts(player_costs, strategies, operating_point, dynamics,
                           x0, time_step, open_loop);

  // For each player, perturb strategies with Gaussian noise a bunch of times
  // and if cost decreases then return false.
  std::vector<Strategy> perturbed_strategies_lower(strategies);
  std::vector<Strategy> perturbed_strategies_upper(strategies);
  for (PlayerIndex ii = 0; ii < dynamics.NumPlayers(); ii++) {
    for (size_t kk = 0; kk < num_time_steps - 1; kk++) {
      VectorXf& alphak_lower = perturbed_strategies_lower[ii].alphas[kk];
      VectorXf& alphak_upper = perturbed_strategies_upper[ii].alphas[kk];

      for (size_t jj = 0; jj < alphak_lower.size(); jj++) {
        alphak_lower(jj) -= max_perturbation;
        alphak_upper(jj) += max_perturbation;

        // Compute new costs.
        const std::vector<float> perturbed_costs_lower = ComputeStrategyCosts(
            player_costs, perturbed_strategies_lower, operating_point, dynamics,
            x0, time_step, open_loop);
        const std::vector<float> perturbed_costs_upper = ComputeStrategyCosts(
            player_costs, perturbed_strategies_upper, operating_point, dynamics,
            x0, time_step, open_loop);

        // Check Nash condition.
        if (std::min(perturbed_costs_lower[ii], perturbed_costs_upper[ii]) <
            nominal_costs[ii]) {
          // std::printf(
          //     "player %hu, timestep %zu: nominal %f > perturbed %f\n ", ii,
          //     kk, nominal_costs[ii], std::min(perturbed_costs_lower[ii],
          //     perturbed_costs_lower[ii]));
          // std::cout << "nominal u: " <<
          // operating_point.us[kk][ii].transpose()
          //           << ", alpha original: "
          //           << strategies[ii].alphas[kk].transpose()
          //           << ", vs. perturbed " << alphak_lower.transpose()
          //           << std::endl;

          // Other users will likely want RK4 integration.
          if (!was_integrating_using_euler)
            MultiPlayerIntegrableSystem::IntegrateUsingRK4();
          return false;
        }

        // Reset this alpha.
        alphak_lower = strategies[ii].alphas[kk];
        alphak_upper = strategies[ii].alphas[kk];
      }
    }
  }

  // Other users will likely want RK4 integration.
  MultiPlayerIntegrableSystem::IntegrateUsingRK4();
  return true;
}

bool CheckSufficientLocalNashEquilibrium(
    const std::vector<PlayerCost>& player_costs,
    const OperatingPoint& operating_point, Time time_step,
    const std::shared_ptr<const MultiPlayerFlatSystem>& dynamics) {
  // Unpack number of players and number of time steps.
  const PlayerIndex num_players = player_costs.size();
  const size_t num_time_steps = operating_point.xs.size();
  const Dimension xdim = operating_point.xs[0].size();

  // Set up quadratic cost approximations.
  std::vector<QuadraticCostApproximation> quadraticization(
      num_players, QuadraticCostApproximation(xdim));

  // Quadraticize costs and check PSD conditions.
  for (size_t kk = 0; kk < num_time_steps; kk++) {
    const Time t = operating_point.t0 + static_cast<Time>(kk) * time_step;
    VectorXf x = operating_point.xs[kk];
    std::vector<VectorXf> us = operating_point.us[kk];

    if (dynamics.get()) {
      // Previous x, us are actually xi, vs.
      x = dynamics->FromLinearSystemState(x.eval());
      us = dynamics->LinearizingControls(x, std::vector<VectorXf>(us));
    }

    std::transform(player_costs.begin(), player_costs.end(),
                   quadraticization.begin(),
                   [&t, &x, &us](const PlayerCost& cost) {
                     return cost.Quadraticize(t, x, us);
                   });

    // Check if Q, Rs PSD.
    constexpr float kErrorMargin = 1e-4;
    for (const auto& q : quadraticization) {
      const auto eig_Q = Eigen::SelfAdjointEigenSolver<MatrixXf>(q.state.hess);
      if (eig_Q.eigenvalues().minCoeff() < -kErrorMargin) {
        // std::cout << "Failed at timestep " << kk << std::endl;
        // std::cout << "Q is: \n" << q.Q << std::endl;
        // std::cout << "Q evals are: " << eig_Q.eigenvalues().transpose()
        //           << std::endl;
        return false;
      }

      for (const auto& entry : q.control) {
        const auto eig_R =
            Eigen::SelfAdjointEigenSolver<MatrixXf>(entry.second.hess);
        if (eig_R.eigenvalues().minCoeff() < -kErrorMargin) return false;
      }
    }
  }

  return true;
}

}  // namespace ilqgames


#define BOOST_TEST_MODULE pcraster old_calc mask_compressor
#include <boost/test/unit_test.hpp>
#include "com_algorithm.h"
#include "com_csfcell.h"
#include "com_exception.h"
#include "com_new.h"
#include "geo_csfmap.h"
#include "geo_filecreatetester.h"
#include "calc_maskcompressor.h"
#include "calc_nullcompressor.h"
#include "calc_compressioninput.h"
#include "calc_spatial.h"
#include "calc_decompresseddata.h"
#include "calc_modelbuilder.h"

BOOST_AUTO_TEST_CASE(compressor)
{
  using namespace calc;

 {
  geo::RasterSpace rs(2,3);
  UINT1 mask[6]= { 1, 1, 1, 1, 1, 1 };
  MaskCompressor mc(rs,mask);
  NullCompressor nc(rs);
  Compressor *cs[2] = { &mc, &nc };

  for (size_t i=0; i < 2; i++) {
    REAL4 *values = new REAL4[6];
    std::generate_n(values,6,com::SeqInc<REAL4>(1));
    // 1,2,3,4,5,6

    BOOST_CHECK(cs[i]->nrCellsCompressed()    == 6);
    BOOST_CHECK(cs[i]->toDecompressedIndex(0) == 0);
    BOOST_CHECK(cs[i]->toDecompressedIndex(2) == 2);
    BOOST_CHECK(cs[i]->toDecompressedIndex(5) == 5);

    CompressionInput ci(VS_S, values, *(cs[i]));
    Spatial *s=cs[i]->createSpatial(ci);
    BOOST_CHECK(s->nrValues() == cs[i]->nrCellsCompressed());
    ci.detachData(); // do not call [] delete => stack data
    REAL4 *compressed= (REAL4 *)const_cast<void*>(s->srcValue());
    BOOST_CHECK(compressed[0]==1);
    BOOST_CHECK(compressed[2]==3);
    BOOST_CHECK(compressed[5]==6);

    DecompressedData dd(VS_S);
    cs[i]->decompress(dd,s->srcValue());
    REAL4 *dCopy = (REAL4 *)const_cast<void*>(dd.decompressed());
    BOOST_CHECK(dCopy[0]==1);
    BOOST_CHECK(dCopy[2]==3);
    BOOST_CHECK(dCopy[4]==5);
    BOOST_CHECK(dCopy[5]==6);

    delete s;
  }
 }
 {
  geo::RasterSpace rs(2,3);
  UINT1 mask[6]= { 1, 1, MV_UINT1, 1, 0, 1 };
  REAL4 values[6] = { 1,2,3,4,5,6 };
  MaskCompressor mc(rs,mask);

  BOOST_CHECK(mc.nrCellsCompressed() == 4);
  BOOST_CHECK(mc.toDecompressedIndex(0) == 0);
  BOOST_CHECK(mc.toDecompressedIndex(2) == 3);
  BOOST_CHECK(mc.toDecompressedIndex(3) == 5);

  CompressionInput ci(VS_S, values, mc);
  Spatial *s=mc.createSpatial(ci);
  BOOST_CHECK(s->nrValues() == mc.nrCellsCompressed());
  ci.detachData(); // do not call [] delete => stack data
  REAL4 *compressed= (REAL4 *)const_cast<void*>(s->srcValue());
  BOOST_CHECK(compressed[0]==1);
  BOOST_CHECK(compressed[2]==4);
  BOOST_CHECK(compressed[3]==6);

  DecompressedData dd(VS_S);
  mc.decompress(dd,s->srcValue());
  REAL4 *dCopy = (REAL4 *)const_cast<void*>(dd.decompressed());
  BOOST_CHECK(dCopy[0]==1);
  BOOST_CHECK(pcr::isMV(dCopy[2]));
  BOOST_CHECK(pcr::isMV(dCopy[4]));
  BOOST_CHECK(dCopy[5]==6);

  delete s;
 }
}

BOOST_AUTO_TEST_CASE(script)
{
  using namespace calc;

  geo::RasterSpace rs(80,50);
  /* make boolean mask with:
   *  - triangle of 1, where mask is defined
   *  - triangle of MV
   *  - lower part if 1
   * - set boolean mask als areamap
   * TODO fill assumes all equal on non-spatial need
   *       for masking?
   */
  UINT1 **mask   = com::new2d<UINT1>(rs.nrRows(),rs.nrCols());
  REAL4 **result = com::new2d<REAL4>(rs.nrRows(),rs.nrCols());
  REAL4 **area   = com::new2d<REAL4>(rs.nrRows(),rs.nrCols());
  REAL4 count=1;
  for(size_t r=0; r< rs.nrRows(); r++)
   for(size_t c=0; c< rs.nrCols(); c++) {
     mask[r][c] = (r < c ) ? MV_UINT1 : (r < rs.nrCols());
     pcr::setMV(result[r][c]);
     if (mask[r][c]==1)
       result[r][c]=count++;
   }
   std::fill_n(area[0],rs.nrCells(), count-1);

  {
   geo::CSFMap mFile(std::string("mvComprMask.map"),rs,VS_BOOLEAN);
   mFile.putCells(mask[0]);
  }
  {
   geo::CSFMap mFile(std::string("mvComprResult.map"),rs,VS_SCALAR);
   mFile.putCells(result[0]);
  }
  {
   geo::CSFMap mFile(std::string("mvComprArea.map"),rs,VS_SCALAR);
   mFile.putCells(area[0]);
  }

  bool succes=true;
  try {

    geo::FileCreateTester mt("resultMvComprScript.map");
    geo::FileCreateTester ma("areaMvComprScript.map");

    ModelBuilder mb;
    mb.setClone("mvComprMask.map");
    mb.setMVCompression(true);
    mb.addStatement("resultMvComprScript.map = order((-ycoordinate(1))+10)");
    mb.addStatement("areaMvComprScript.map = maparea(resultMvComprScript.map)");

    mb.execute();

    BOOST_CHECK(mt.equalTo("mvComprResult.map",false));
    BOOST_CHECK(ma.equalTo("mvComprArea.map",false));

  } catch (const com::Exception& e) {
    std::cerr << e.messages();
    succes=false;
  }
  BOOST_CHECK(succes);

  com::delete2d<UINT1>(mask);
  com::delete2d<REAL4>(result);
  com::delete2d<REAL4>(area);

}

BOOST_AUTO_TEST_CASE(_0_option)
{
  using namespace calc;

  geo::RasterSpace rs(8,8);
  UINT1 **mask   = com::new2d<UINT1>(rs.nrRows(),rs.nrCols());
  REAL4 **result = com::new2d<REAL4>(rs.nrRows(),rs.nrCols());

  for(size_t r=0; r< rs.nrRows(); r++)
   for(size_t c=0; c< rs.nrCols(); c++) {
     mask[r][c] = (r < c ) ? MV_UINT1 : (r < rs.nrCols());
     pcr::setMV(result[r][c]);
     if (mask[r][c]==1)
       result[r][c]=1;
   }

  {
   geo::CSFMap mFile(std::string("zeroComprMask.map"),rs,VS_BOOLEAN);
   mFile.putCells(mask[0]);
   com::delete2d<UINT1>(mask);
  }
  {
   geo::CSFMap mFile(std::string("zeroComprResult.map"),rs,VS_SCALAR);
   mFile.putCells(result[0]);
   com::delete2d<REAL4>(result);
  }

  bool succes=true;
  try {

    geo::FileCreateTester mt("zeroCompr.map");

    ModelBuilder mb;
    mb.setClone("zeroComprMask.map");
    mb.setMVCompression(true);
    mb.set0Compression(true);
    mb.addStatement("X = scalar(0)");
    mb.addStatement("zeroCompr.map = X + spatial(1)");

    mb.execute();

    BOOST_CHECK(mt.equalTo("zeroComprResult.map",true));

  } catch (const com::Exception& e) {
    std::cerr << e.messages();
    succes=false;
  }
  BOOST_CHECK(succes);
}


//
// Created by cedric on 2021-07-25.
//

#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE SixteenBitArithmeticTest

#include <boost/test/included/unit_test.hpp>
#include <exception>
#include "opcode_parsing_math.h"
#include "cpu/CPU.h"
#include "util/testing_utilities.h"

BOOST_AUTO_TEST_SUITE(SixteenBitArithmeticTest)

    BOOST_AUTO_TEST_CASE(validate0x03) {
        CPU cpu;
        cpu.load_16bit_register_immediate(REGISTER_BC_INDEX, 0x4F2F);
        run_next_opcode(&cpu, {0x03});
        BOOST_CHECK(cpu.get_16bit_register(REGISTER_BC_INDEX) == 0x4F30);

        cpu.load_16bit_register_immediate(REGISTER_BC_INDEX, 0xFFFF);
        run_next_opcode(&cpu, {0x03});
        BOOST_CHECK(cpu.get_16bit_register(REGISTER_BC_INDEX) == 0x0000);
    }

    BOOST_AUTO_TEST_CASE(validate0x13) {
        CPU cpu;
        cpu.load_16bit_register_immediate(REGISTER_DE_INDEX, 0x2132);
        run_next_opcode(&cpu, {0x13});
        BOOST_CHECK(cpu.get_16bit_register(REGISTER_DE_INDEX) == 0x2133);

        cpu.load_16bit_register_immediate(REGISTER_DE_INDEX, 0xFFFF);
        run_next_opcode(&cpu, {0x13});
        BOOST_CHECK(cpu.get_16bit_register(REGISTER_DE_INDEX) == 0x0000);
    }

    BOOST_AUTO_TEST_CASE(validate0x23) {
        CPU cpu;
        cpu.load_16bit_register_immediate(REGISTER_HL_INDEX, 0x1E71);
        run_next_opcode(&cpu, {0x23});
        BOOST_CHECK(cpu.get_16bit_register(REGISTER_HL_INDEX) == 0x1E72);

        cpu.load_16bit_register_immediate(REGISTER_HL_INDEX, 0xFFFF);
        run_next_opcode(&cpu, {0x23});
        BOOST_CHECK(cpu.get_16bit_register(REGISTER_HL_INDEX) == 0x0000);
    }

    BOOST_AUTO_TEST_CASE(validate0x33) {
        CPU cpu;
        cpu.load_16bit_register_immediate(REGISTER_SP_INDEX, 0x62F1);
        run_next_opcode(&cpu, {0x33});
        BOOST_CHECK(cpu.get_16bit_register(REGISTER_SP_INDEX) == 0x62F2);

        cpu.load_16bit_register_immediate(REGISTER_SP_INDEX, 0xFFFF);
        run_next_opcode(&cpu, {0x33});
        BOOST_CHECK(cpu.get_16bit_register(REGISTER_SP_INDEX) == 0x0000);
    }

    BOOST_AUTO_TEST_CASE(validate0x0B) {
        CPU cpu;
        cpu.load_16bit_register_immediate(REGISTER_BC_INDEX, 0x4F2F);
        run_next_opcode(&cpu, {0x0B});
        BOOST_CHECK(cpu.get_16bit_register(REGISTER_BC_INDEX) == 0x4F2E);

        cpu.load_16bit_register_immediate(REGISTER_BC_INDEX, 0x0000);
        run_next_opcode(&cpu, {0x0B});
        BOOST_CHECK(cpu.get_16bit_register(REGISTER_BC_INDEX) == 0xFFFF);
    }

    BOOST_AUTO_TEST_CASE(validate0x1B) {
        CPU cpu;
        cpu.load_16bit_register_immediate(REGISTER_DE_INDEX, 0x2132);
        run_next_opcode(&cpu, {0x1B});
        BOOST_CHECK(cpu.get_16bit_register(REGISTER_DE_INDEX) == 0x2131);

        cpu.load_16bit_register_immediate(REGISTER_DE_INDEX, 0x0000);
        run_next_opcode(&cpu, {0x1B});
        BOOST_CHECK(cpu.get_16bit_register(REGISTER_DE_INDEX) == 0xFFFF);
    }

    BOOST_AUTO_TEST_CASE(validate0x2B) {
        CPU cpu;
        cpu.load_16bit_register_immediate(REGISTER_HL_INDEX, 0x1E71);
        run_next_opcode(&cpu, {0x2B});
        BOOST_CHECK(cpu.get_16bit_register(REGISTER_HL_INDEX) == 0x1E70);

        cpu.load_16bit_register_immediate(REGISTER_HL_INDEX, 0x0000);
        run_next_opcode(&cpu, {0x2B});
        BOOST_CHECK(cpu.get_16bit_register(REGISTER_HL_INDEX) == 0xFFFF);
    }

    BOOST_AUTO_TEST_CASE(validate0x3B) {
        CPU cpu;
        cpu.load_16bit_register_immediate(REGISTER_SP_INDEX, 0x62F1);
        run_next_opcode(&cpu, {0x3B});
        BOOST_CHECK(cpu.get_16bit_register(REGISTER_SP_INDEX) == 0x62F0);

        cpu.load_16bit_register_immediate(REGISTER_SP_INDEX, 0x0000);
        run_next_opcode(&cpu, {0x3B});
        BOOST_CHECK(cpu.get_16bit_register(REGISTER_SP_INDEX) == 0xFFFF);
    }

BOOST_AUTO_TEST_SUITE_END()


/*
 *
 * Copyright (c) 2003 Kresimir Fresl, Toon Knapen and Karl Meerbergen
 *
 * 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)
 *
 * KF author acknowledges the support of the Faculty of Civil Engineering,
 * University of Zagreb, Croatia.
 *
 */

#ifndef BOOST_NUMERIC_BINDINGS_TRAITS_SPARSE_TRAITS_H
#define BOOST_NUMERIC_BINDINGS_TRAITS_SPARSE_TRAITS_H

#include <boost/numeric/bindings/traits/config.hpp>

#ifndef BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS

#include <boost/numeric/bindings/traits/traits.hpp>

namespace boost { namespace numeric { namespace bindings { namespace traits {

  /// There is no default implementation since there is no reasonable default.
  /// Most matrix libraries provide totally different functions.

  /// Auxiliary traits class to reduce the number of specializations.
  template <typename MIdentifier, typename MType>
  struct sparse_matrix_detail_traits {
    typedef MIdentifier identifier_type;
    typedef MType       matrix_type;
  };

  /// sparse_matrix_traits<> generic version:
  template <typename M>
  struct sparse_matrix_traits
    : sparse_matrix_detail_traits <typename boost::remove_const<M>::type, M>
  {
    // typedefs:
    //   matrix_structure
    //   storage_format
    //   ordering_type
    //   value_type
    //   value_pointer
    //   index_pointer
    // enum (or static size_t const):
    //   index_base
    // static functions:
    //   index_pointer index1_storage()
    //     - compressed column: array of column start locations
    //     - compressed row: array of row start locations
    //     - coordinate, column major: column indices of nonzeros
    //     - coordinate, row major: row indices of nonzeros
    //   index_pointer index2_storage()
    //     - compressed column: array of row indices of nonzeros
    //     - compressed row: array of column indices of nonzeros
    //     - coordinate, column major: row indices of nonzeros
    //     - coordinate, row major: column indices of nonzeros
    //   value_pointer value_storage()
    //     - array of nonzeros
    //   std::ptrdiff_t num_rows()
    //   std::ptrdiff_t num_columns()
    //   std::ptrdiff_t num_nonzeros()
  };

  // storage format tags
  struct compressed_t {};
  struct coordinate_t {};

  ///////////////////////////
  //
  // free accessor functions
  //
  ///////////////////////////

  template <typename M>
  inline
  typename sparse_matrix_traits<M>::index_pointer
  spmatrix_index1_storage (M& m) {
    return sparse_matrix_traits<M>::index1_storage (m);
  }
  template <typename M>
  inline
  typename sparse_matrix_traits<M>::index_pointer
  spmatrix_index2_storage (M& m) {
    return sparse_matrix_traits<M>::index2_storage (m);
  }

  template <typename M>
  inline
  typename sparse_matrix_traits<M>::value_pointer
  spmatrix_value_storage (M& m) {
    return sparse_matrix_traits<M>::value_storage (m);
  }

  template <typename M>
  inline
  std::ptrdiff_t spmatrix_num_rows (M& m) { return sparse_matrix_traits<M>::num_rows (m); }
  template <typename M>
  inline
  std::ptrdiff_t spmatrix_num_columns (M& m) { return sparse_matrix_traits<M>::num_columns (m); }

  template <typename M>
  inline
  std::ptrdiff_t spmatrix_num_nonzeros (M& m) {
    return sparse_matrix_traits<M>::num_nonzeros (m);
  }

  // Retain for older codes

  template <typename M>
  inline
  std::ptrdiff_t spmatrix_size1 (M& m) { return sparse_matrix_traits<M>::num_rows (m); }
  template <typename M>
  inline
  std::ptrdiff_t spmatrix_size2 (M& m) { return sparse_matrix_traits<M>::num_columns (m); }

}}}}

#else // BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS

#error with your compiler sparse matrices cannot be used in bindings

#endif // BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS

#endif // BOOST_NUMERIC_BINDINGS_TRAITS_SPARSE_TRAITS_H


/* Copyright (C) 2010-2019, The Regents of The University of Michigan.
 All rights reserved.

 This software was developed as part of the The Vulcan project in the Intelligent Robotics Lab
 under the direction of Benjamin Kuipers, kuipers@umich.edu. Use of this code is governed by an
 MIT-style License that can be found at "https://github.com/h2ssh/Vulcan".
*/

/**
* \file     svm.cpp
* \author   Collin Johnson
*
* Definition of learn_svm_classifier.
*/

#include <utils/svm.h>
#include <core/float_comparison.h>
#include <core/vector.h>
#include <boost/range/iterator_range.hpp>
#include <future>
#include <map>
#include <vector>
#include <utility>

namespace vulcan
{
namespace utils
{

struct SVMParamAccuracy
{
    double C;
    double gamma;
    double accuracy;

    SVMParamAccuracy(double C, double gamma)
    : C(C)
    , gamma(gamma)
    {
    }
};

using ParamVec = std::vector<SVMParamAccuracy>;

std::vector<std::pair<int, double>> calculate_label_weights(const IntMatrix& labels);
svm_parameter select_best_parameters(svm_problem* problem, const std::vector<std::pair<int, double>>& weights);
void evaluate_params(ParamVec::iterator begin, ParamVec::iterator end, svm_problem* problem, svm_parameter params);
int svm_classifier_accuracy(double* truth, double* estimated, int size);

svm_model* learn_svm_classifier(const Matrix& features, const IntMatrix& labels)
{
    // Convert the feature data into an svm_problem so svm_train can be called
    std::vector<double> svmLabels;
    std::vector<SVMFeatures> svmNodes;

    for(std::size_t n = 0; n < features.n_cols; ++n)
    {
        SVMFeatures nodes;
        features_to_svm_nodes(features.col(n), nodes);

        svmNodes.push_back(nodes);
        svmLabels.push_back(labels(n));
    }

    std::vector<svm_node*> nodeStarts;
    for(auto& n : svmNodes)
    {
        nodeStarts.push_back(n.data());
    }

    assert(nodeStarts.size() == svmLabels.size());

    svm_problem problem;
    problem.l = svmLabels.size();
    problem.y = svmLabels.data();
    problem.x = nodeStarts.data();

    // Assign the parameters to use for the learning
    auto trainingParams = select_best_parameters(&problem, calculate_label_weights(labels));

    // When training the final model, the probability estimates are needed, so ensure they are turned on
    trainingParams.probability = 1;

    svm_model* trainedModel = svm_train(&problem, &trainingParams);

    // Due to the trained model storing data from the svm_problem, the original pointer can't be returned. Instead,
    // save the model to a temp file and reload it so it allocates the memory itself and makes it safe to pass
    // ownership onward to someone else
    const char* kDummyModelFile = "dummy_svm_model.svm";

    if(svm_save_model(kDummyModelFile, trainedModel) != 0)
    {
        perror("ERROR: learn_svm_classifier: Failed to save temporary model");
        svm_free_and_destroy_model(&trainedModel);
        svm_destroy_param(&trainingParams);
        return nullptr;
    }
    else
    {
        auto loadedModel = svm_load_model(kDummyModelFile);

        if(loadedModel == nullptr)
        {
            std::cerr << "ERROR: learn_svm_classifier: Failed to load temporary model.\n";
        }

        svm_free_and_destroy_model(&trainedModel);
        svm_destroy_param(&trainingParams);
        return loadedModel;
    }
}

void features_to_svm_nodes(const Vector& features, SVMFeatures& nodes)
{
    for(int featIdx = 0; featIdx < static_cast<int>(features.n_rows); ++featIdx)
    {
        nodes.push_back(svm_node{featIdx + 1, features(featIdx)});
    }

    // The end of the nodes are marked by an index of -1
    nodes.push_back(svm_node{-1, 0});
}

std::vector<std::pair<int, double>> calculate_label_weights(const IntMatrix& labels)
{
    std::map<int, double> counts;

    for(arma::uword n = 0; n < labels.n_rows; ++n)
    {
        if(counts.find(labels[n]) == counts.end())
        {
            counts[labels[n]] = 0.0;
        }
        else
        {
            counts[labels[n]] += 1.0;
        }
    }

    // The weight to assign will make the weights of the total number of labels for a given instance sum to 1.
    std::vector<std::pair<int, double>> weights;
    for(auto& c : counts)
    {
        weights.emplace_back(c.first, (1.0 - (c.second / labels.n_rows)));
    }

    return weights;
}

svm_parameter select_best_parameters(svm_problem* problem, const std::vector<std::pair<int, double>>& weights)
{
    // Default parameters snagged from svm-train
    svm_parameter params;

    // default values
    params.svm_type = C_SVC;
    params.kernel_type = RBF;
    params.degree = 3;
    params.gamma = 0;    // 1/num_features
    params.coef0 = 0;
    params.nu = 0.5;
    params.cache_size = 1000;
    params.C = 1;
    params.eps = 1e-3;
    params.p = 0.1;
    params.shrinking = 1;
    params.probability = 0;
    params.nr_weight = weights.size();

    // Assign weights if they are present
    if(params.nr_weight > 0)
    {
        params.weight_label = new int[params.nr_weight];
        params.weight = new double[params.nr_weight];

        for(int n = 0; n < params.nr_weight; ++n)
        {
            params.weight_label[n] = weights[n].first;
            params.weight[n] = weights[n].second;
        }
    }
    // Otherwise null them out
    else
    {
        params.weight_label = NULL;
        params.weight = NULL;
    }

    ParamVec allParams;
    for(double g = -5.0, gEnd = 5.0; g <= gEnd; g += 1.0)
    {
        allParams.emplace_back(1.0, std::pow(10.0, g));

        // There is no coeff0 for RBF
//         for(double c = -4.0, cEnd = 4.0; c <= cEnd; c += 1.0)
//         {
//             allParams.emplace_back(std::pow(10.0, c), std::pow(10.0, g));
//         }
    }

    // Random because different sets of parameters take wildly different amounts of computation
    std::random_device rd;
    std::mt19937 generator(rd());
    std::shuffle(allParams.begin(), allParams.end(), generator);

    // Create up to four threads for finding the isovists. Assign 1/4th of the work to each thread.
    std::size_t kMaxThreads = 4;
    const int kNumThreads = std::min(kMaxThreads, (allParams.size() / kMaxThreads) + 1);
    std::size_t paramsPerThread = (allParams.size() / kNumThreads) + 1; // Add one to the positions/thread to account for truncating due to integer division
    // The last thread will take up to 3 fewer isovist calculations

    // Launch the threads
    std::vector<std::future<void>> asyncParams;
    for(int n = 0; n < kNumThreads; ++n)
    {
        std::size_t start = paramsPerThread * n;
        std::size_t end   = std::min(paramsPerThread * (n + 1), allParams.size());

        asyncParams.push_back(std::async(std::launch::async,
                                            evaluate_params,
                                            allParams.begin() + start,
                                            allParams.begin() + end,
                                            problem,
                                            params));
    }

    for(auto& f : asyncParams)
    {
        f.get();
    }

    // Sort in descending order of accuracy. Tie goes to the lower C, as softer margins are generally better.
    std::sort(allParams.begin(), allParams.end(), [](SVMParamAccuracy lhs, SVMParamAccuracy rhs) {
        return (lhs.accuracy > rhs.accuracy) || ((lhs.accuracy == rhs.accuracy) && (lhs.gamma < rhs.gamma));
    });

    std::cout << "SVM parameter accuracy: (C, gamma) -> accuracy\n";
    for(auto p : allParams)
    {
        std::cout << '(' << p.C << ',' << p.gamma << ") -> " << p.accuracy << "%\n";
    }

    params.C = allParams.front().C;
    params.gamma = allParams.front().gamma;

    return params;
}

void evaluate_params(ParamVec::iterator begin, ParamVec::iterator end, svm_problem* problem, svm_parameter params)
{
    std::vector<double> validationResults(problem->l);

    for(auto& p : boost::make_iterator_range(begin, end))
    {
        params.C = p.C;
        params.gamma = p.gamma;

        svm_cross_validation(problem, &params, 5, validationResults.data());
        p.accuracy = svm_classifier_accuracy(problem->y, validationResults.data(), problem->l);
    }
}

int svm_classifier_accuracy(double* truth, double* estimated, int size)
{
    int numCorrect = 0;
    for(int n = 0; n < size; ++n)
    {
        if(absolute_fuzzy_equal(truth[n], estimated[n]))
        {
            ++numCorrect;
        }
    }

    return (numCorrect / static_cast<double>(size)) * 100.0;
}

} // namespace utils
} // namespace vulcan


#ifndef OTOO_MODULE_H
#define OTOO_MODULE_H
#include <math.h>
#include <time.h>
#include <sys/time.h>
#include <sys/resource.h>

#include <iostream>
#include <fstream>
#include <sstream>
#include <algorithm>
#include <parallel/algorithm>

#define EIGEN_DEFAULT_TO_ROW_MAJOR
//#include <Eigen/Core>
//#include <Eigen/Geometry>

namespace OTOO {
  typedef unsigned long long int uint64;
  typedef unsigned int           uint32;
//  typedef Eigen::Vector3f  Vector3;
//  typedef Eigen::Vector4f  Vector4;
//  typedef Eigen::ArrayX3f  ArrayX3;
//  typedef Eigen::ArrayX4f  ArrayX4;
//  typedef Eigen::ArrayX2f  ArrayX2;
//  typedef Eigen::ArrayXf   ArrayX;

  double e_time(void)
  {
    static struct timeval now;
    gettimeofday(&now, NULL);
    return (double)(now.tv_sec  + now.tv_usec/1000000.0);
  }

//  template<typename AR>
//  void Allocate(const uint64 n, AR &x)
//  {
//    x.resize(n, Eigen::NoChange);
//  }

  typedef struct keys {
    uint64 k, i;
  } Keys;

  struct KeyCmp {
    bool operator()(const keys &a, const keys &b) const {
      return a.k < b.k;
    }
  };
}
#endif


// shasta.
#include "Assembler.hpp"
#include "LocalReadGraph.hpp"
#include "orderPairs.hpp"
#include "timestamp.hpp"
using namespace ChanZuckerberg;
using namespace shasta;

// Boost libraries.
#include <boost/pending/disjoint_sets.hpp>
#include <boost/graph/iteration_macros.hpp>

// Standard libraries.
#include "chrono.hpp"
#include <queue>
#include "tuple.hpp"

// Compute connected components of the global read graph.
// We only keep components that are large enough.
void Assembler::computeReadGraphComponents(
    size_t minComponentSize,        // Minimum size for a connected component to be kept.
    size_t minAlignedMarkerCount,
    size_t maxTrim
    )
{
    checkReadsAreOpen();
    checkKmersAreOpen();
    checkMarkersAreOpen();
    checkOverlapsAreOpen();
    checkAlignmentDataAreOpen();
    const OrientedReadId::Int orientedReadCount = OrientedReadId::Int(2*reads.size());

    // Initialize the disjoint set data structures.
    vector<ReadId> rank(orientedReadCount);
    vector<ReadId> parent(orientedReadCount);
    boost::disjoint_sets<ReadId*, ReadId*> disjointSets(&rank[0], &parent[0]);
    for(OrientedReadId::Int orientedReadId=0; orientedReadId<orientedReadCount; orientedReadId++) {
        disjointSets.make_set(orientedReadId);
    }

    // Loop over alignments which satisfy our criteria.
    for(size_t i=0; i<alignmentData.size(); i++) {
        if((i%1000000) == 0) {
            cout << timestamp << i << " of " << alignmentData.size() << endl;
        }
        const AlignmentData& ad = alignmentData[i];

        // If the number of markers in the alignment is too small, skip.
        if(ad.info.markerCount < minAlignedMarkerCount) {
            continue;
        }

        // Compute the left and right trim (bases excluded from
        // the alignment).
        OrientedReadId orientedReadId0(ad.readIds[0], 0);
        OrientedReadId orientedReadId1(ad.readIds[1], ad.isSameStrand ? 0 : 1);
        uint32_t leftTrim;
        uint32_t rightTrim;
        tie(leftTrim, rightTrim) = computeTrim(
            orientedReadId0,
            orientedReadId1,
            ad.info);

        // If the trim is too high, skip.
        if(leftTrim>maxTrim || rightTrim>maxTrim) {
            continue;
        }

        // Join the connected components that these two
        // oriented reads belong to.
        disjointSets.union_set(
            orientedReadId0.getValue(),
            orientedReadId1.getValue());
        orientedReadId0.flipStrand();
        orientedReadId1.flipStrand();
        disjointSets.union_set(
            orientedReadId0.getValue(),
            orientedReadId1.getValue());
    }

    // Store the component that each oriented read belongs to.
    overlapGraphComponent.createNew(largeDataName("OverlapGraphComponent"), largeDataPageSize);
    overlapGraphComponent.resize(orientedReadCount);
    for(OrientedReadId::Int orientedReadId=0; orientedReadId<orientedReadCount; orientedReadId++) {
        overlapGraphComponent[orientedReadId] = disjointSets.find_set(orientedReadId);
    }

    // Gather the vertices (oriented read ids) of each component.
    // The vertices of each component are sorted by construction.
    std::map<OrientedReadId::Int, vector<OrientedReadId::Int> > componentMap;
    for(OrientedReadId::Int orientedReadId=0; orientedReadId<orientedReadCount; orientedReadId++) {
        componentMap[overlapGraphComponent[orientedReadId]].push_back(orientedReadId);
    }

    // Sort the components by decreasing size.
    // We only keep components that are large enough.
    vector< pair<OrientedReadId::Int, OrientedReadId::Int> > componentTable;
    for(const auto& p: componentMap) {
        const auto componentId = p.first;
        const auto& component = p.second;
        const auto componentSize = component.size();
        CZI_ASSERT(componentSize > 0);
        if(componentSize >= minComponentSize) {
            // Keep this component.
            componentTable.push_back(make_pair(componentId, componentSize));

        }
    }
    sort(componentTable.begin(), componentTable.end(),
        OrderPairsBySecondOnlyGreater<ReadId, ReadId>());

    // Renumber the components.
    std::map<ReadId, ReadId> componentNumberMap;
    for(ReadId newComponentId=0; newComponentId<componentTable.size(); newComponentId++) {
        const auto& p = componentTable[newComponentId];
        const auto oldComponentId = p.first;
        componentNumberMap.insert(make_pair(oldComponentId, newComponentId));
    }
    for(ReadId i=0; i<orientedReadCount; i++) {
        ReadId& readComponentId = overlapGraphComponent[i];
        const auto it = componentNumberMap.find(readComponentId);
        if(it == componentNumberMap.end()) {
            readComponentId = std::numeric_limits<ReadId>::max();
        } else {
            readComponentId = it->second;
        }
    }

    // Permanently store the renumbered components.
    overlapGraphComponents.createNew(largeDataName("OverlapGraphComponents"), largeDataPageSize);
    for(ReadId newComponentId=0; newComponentId<componentTable.size(); newComponentId++) {
        const auto& p = componentTable[newComponentId];
        const auto oldComponentId = p.first;
        const auto& component = componentMap[oldComponentId];
        CZI_ASSERT(component.size() == p.second);
        overlapGraphComponents.appendVector();
        for(const auto& orientedRead: component) {
            overlapGraphComponents.append(OrientedReadId(orientedRead));
        }
    }

    // Write out the size of each component we kept.
    for(ReadId newComponentId=0; newComponentId<overlapGraphComponents.size(); newComponentId++) {
        cout << "Component " << newComponentId << " has size ";
        cout << overlapGraphComponents.size(newComponentId);
        cout << "." << endl;

    }

}

// Create a local read graph starting from a given oriented read
// and walking out a given distance on the global read graph.
void Assembler::createLocalReadGraph(
    ReadId readId,
    Strand strand,
    size_t minAlignedMarkerCount,   // Minimum number of alignment markers to generate an edge.
    size_t maxTrim,                 // Maximum left/right trim to generate an edge.
    uint32_t distance                 // How far to go from starting oriented read.
)
{
    createLocalReadGraph(OrientedReadId(readId, strand),
        minAlignedMarkerCount, maxTrim,
        distance);
}
void Assembler::createLocalReadGraph(
    OrientedReadId orientedReadId,
    size_t minAlignedMarkerCount,   // Minimum number of alignment markers to generate an edge.
    size_t maxTrim,                 // Maximum left/right trim to generate an edge.
    uint32_t maxDistance              // How far to go from starting oriented read.
)
{
    // Check that we have what we need.
    checkReadsAreOpen();
    checkReadNamesAreOpen();
    checkKmersAreOpen();
    checkMarkersAreOpen();
    checkAlignmentDataAreOpen();

    // Create the LocalReadGraph.
    LocalReadGraph graph;
    createLocalReadGraph(orientedReadId,
        minAlignedMarkerCount, maxTrim, maxDistance, 0.,
        graph);

    cout << "The local read graph has " << num_vertices(graph);
    cout << " vertices and " << num_edges(graph) << " edges." << endl;
    graph.write("LocalReadGraph.dot", maxDistance);
    writeLocalReadGraphToFasta(graph, "LocalReadGraph.fasta");

}

// Create a local read graph starting from a given oriented read
// and walking out a given distance on the global read graph.
bool Assembler::createLocalReadGraph(
    OrientedReadId orientedReadIdStart,
    size_t minAlignedMarkerCount,   // Minimum number of alignment markers to generate an edge.
    size_t maxTrim,                 // Maximum left/right trim to generate an edge.
    uint32_t maxDistance,           // How far to go from starting oriented read.
    double timeout,                 // Or 0 for no timeout.
    LocalReadGraph& graph)
{
    const auto startTime = steady_clock::now();

    // Add the starting vertex.
    graph.addVertex(orientedReadIdStart,
        uint32_t(reads[orientedReadIdStart.getReadId()].baseCount), 0);

    // Initialize a BFS starting at the start vertex.
    std::queue<OrientedReadId> q;
    q.push(orientedReadIdStart);

    // Do the BFS.
    while(!q.empty()) {

        // See if we exceeded the timeout.
        if(seconds(steady_clock::now() - startTime) > timeout) {
            graph.clear();
            return false;
        }

        // Dequeue a vertex.
        const OrientedReadId orientedReadId0 = q.front();
        // cout << "Dequeued " << orientedReadId0;
        // cout << " with " << alignmentTable.size(orientedReadId0.getValue()) << " overlaps." << endl;
        q.pop();
        const uint32_t distance0 = graph.getDistance(orientedReadId0);
        const uint32_t distance1 = distance0 + 1;

        // Loop over overlaps/alignments involving this vertex.
        for(const uint64_t i: alignmentTable[orientedReadId0.getValue()]) {
            CZI_ASSERT(i < alignmentData.size());
            const AlignmentData& ad = alignmentData[i];

            // If the alignment involves too few markers, skip.
            if(ad.info.markerCount < minAlignedMarkerCount) {
                continue;
            }

            // To compute the trim, keep into account the fact
            // that the stored AlignmentInfo was computed for
            // the ReadId's stored in the Overlap, with the first one on strand 0.
            const OrientedReadId overlapOrientedReadId0(ad.readIds[0], 0);
            const OrientedReadId overlapOrientedReadId1(ad.readIds[1], ad.isSameStrand ? 0 : 1);
            uint32_t leftTrim;
            uint32_t rightTrim;
            tie(leftTrim, rightTrim) = computeTrim(
                overlapOrientedReadId0,
                overlapOrientedReadId1,
                ad.info);
            if(leftTrim>maxTrim || rightTrim>maxTrim) {
                continue;
            }

            // The overlap and the alignment satisfy our criteria.
            // Get the other oriented read involved in this overlap.
            const OrientedReadId orientedReadId1 = ad.getOther(orientedReadId0);

            // Update our BFS.
            // Note that we are pushing to the queue vertices at maxDistance,
            // so we can find all of their edges to other vertices at maxDistance.
            if(distance0 < maxDistance) {
                if(!graph.vertexExists(orientedReadId1)) {
                    graph.addVertex(orientedReadId1,
                        uint32_t(reads[orientedReadId1.getReadId()].baseCount), distance1);
                    q.push(orientedReadId1);
                }
                graph.addEdge(orientedReadId0, orientedReadId1,
                    ad.info);
            } else {
                CZI_ASSERT(distance0 == maxDistance);
                if(graph.vertexExists(orientedReadId1)) {
                    graph.addEdge(orientedReadId0, orientedReadId1,
                        ad.info);
                }
            }

#if 0
            // THE OLD CODE DOES NOT CREATE EDGES
            // BETWEEN VERTICES AT MAXDISTANCE.
            // Add the vertex for orientedReadId1, if necessary.
            // Also add orientedReadId1 to the queue, unless
            // we already reached the maximum distance.
            if(!graph.vertexExists(orientedReadId1)) {
                graph.addVertex(orientedReadId1,
                    uint32_t(reads[orientedReadId1.getReadId()].baseCount), distance1);
                if(distance1 < maxDistance) {
                    q.push(orientedReadId1);
                    // cout << "Enqueued " << orientedReadId1 << endl;
                }
            }

            // Add the edge.
            graph.addEdge(orientedReadId0, orientedReadId1,
                ad.info);
#endif
        }

    }

    return true;
}

// Write in fasta format the sequences of the vertices of a local read graph.
void Assembler::writeLocalReadGraphToFasta(
    const LocalReadGraph& graph,
    const string& fileName)
{
    ofstream fasta(fileName);
    BGL_FORALL_VERTICES(v, graph, LocalReadGraph) {
        const OrientedReadId orientedReadId(graph[v].orientedReadId);
        writeOrientedRead(orientedReadId, fasta);
    }
}


#ifndef OSRAM_UTIL_STATIC_RTREE
#define OSRAM_UTIL_STATIC_RTREE

#include <algorithm>
#include <cassert>
#include <climits>
#include <cmath>
#include <cstdint>
#include <execution>
#include <iterator>
#include <limits>
#include <numeric>
#include <queue>

#include <boost/iostreams/device/mapped_file.hpp>
#include <range/v3/view/iota.hpp>

#include <osram/storage/shared_memory_ownership.hxx>

#include <osram/util/coordinate.hxx>
#include <osram/util/coordinate_calculation.hxx>
#include <osram/util/hilbert_value.hxx>
#include <osram/util/mmap_file.hxx>
#include <osram/util/rectangle.hxx>
#include <osram/util/vector_view.hxx>
#include <osram/util/web_mercator.hxx>

namespace osram {
namespace util {
template <class E, storage::Ownership O = storage::Ownership::Container,
          std::uint32_t BRANCHING_FACTOR = 64,
          std::uint32_t LEAF_PAGE_SIZE = 4096>
class StaticRTree {
  template <typename T> using Vector = ViewOrVector<T, O>;

public:
  struct TreeIndex;
  struct TreeNode;

private:
  Vector<TreeNode> m_search_tree;
  util::vector_view<const Coordinate> m_coordinate_list;
  Vector<std::uint64_t> m_tree_level_starts;
  boost::iostreams::mapped_file_source m_objects_region;
  util::vector_view<const E> m_objects;

  struct WrappedInputElement {
    std::uint64_t hilbert_value;
    std::uint32_t original_index;

    explicit WrappedInputElement(const std::uint64_t hilbert_value,
                                 const std::uint32_t original_index)
        : hilbert_value(hilbert_value), original_index(original_index) {}

    WrappedInputElement() : hilbert_value(0), original_index(UINT_MAX) {}

    inline bool operator<(const WrappedInputElement &other) const {
      return hilbert_value < other.hilbert_value;
    }
  };

  struct QueryCandidate {
    std::uint64_t squared_min_dist;
    TreeIndex tree_index;
    std::uint32_t segment_index;
    Coordinate fixed_projected_coordinate;

    QueryCandidate(std::uint64_t squared_min_dist, TreeIndex tree_index)
        : squared_min_dist(squared_min_dist), tree_index(tree_index),
          segment_index(std::numeric_limits<std::uint32_t>::max()) {}

    QueryCandidate(std::uint64_t squared_min_dist, TreeIndex tree_index,
                   const std::uint32_t segment_index,
                   const Coordinate &fixed_projected_coordinate)
        : squared_min_dist(squared_min_dist), tree_index(tree_index),
          segment_index(segment_index),
          fixed_projected_coordinate(fixed_projected_coordinate) {}

    inline bool is_segment() const {
      return segment_index != std::numeric_limits<std::uint32_t>::max();
    }

    inline bool operator<(const QueryCandidate &other) const {
      return other.squared_min_dist < squared_min_dist;
    }
  };

  template <typename Q>
  void explore_leaf_node(const TreeIndex &leaf_id,
                         const Coordinate &projected_input_coordinate_fixed,
                         const FloatCoordinate &projected_input_coordinate,
                         Q &traversal_queue) const {
    assert(is_leaf(leaf_id));

    for (auto const index : child_indices(leaf_id)) {
      const auto &current_edge = m_objects[index];
      const auto projected_u =
          web_mercator::from_wgs84(m_coordinate_list[current_edge.u]);
      const auto projected_v =
          web_mercator::from_wgs84(m_coordinate_list[current_edge.v]);

      FloatCoordinate projected_nearest;
      std::tie(std::ignore, projected_nearest) =
          coordinate_calculation::project_point_on_segment(
              projected_u, projected_v, projected_input_coordinate);

      const auto squared_distance =
          coordinate_calculation::squared_euclidean_distance(
              projected_input_coordinate_fixed, projected_nearest);

      assert(0. <= squared_distance);
      assert(index < std::numeric_limits<std::uint32_t>::max());
      traversal_queue.push(QueryCandidate{squared_distance, leaf_id,
                                          static_cast<std::uint32_t>(index),
                                          Coordinate{projected_nearest}});
    }
  }

  template <class Q>
  void explore_tree_node(const TreeIndex &parent,
                         const Coordinate &fixed_projected_input_coordinate,
                         Q &traversal_queue) const {
    assert(!is_leaf(parent));

    for (const auto child_index : child_indices(parent)) {
      const auto &child = m_search_tree[child_index];
      const auto squared_lower_bound_to_element =
          child.minimum_bounding_rectangle.get_min_squared_dist(
              fixed_projected_input_coordinate);
      traversal_queue.push(QueryCandidate{
          squared_lower_bound_to_element,
          TreeIndex(parent.level + 1,
                    child_index - m_tree_level_starts[parent.level + 1])});
    }
  }

  std::uint64_t get_level_size(const std::size_t level) const {
    assert(m_tree_level_starts.size() > level + 1);
    assert(m_tree_level_starts[level + 1] >= m_tree_level_starts[level]);
    return m_tree_level_starts[level + 1] - m_tree_level_starts[level];
  }

  ranges::iota_view<unsigned long, unsigned long>
  child_indices(const TreeIndex &parent) const {
    if (is_leaf(parent)) {
      const std::uint64_t first_child_index = parent.offset * LEAF_NODE_SIZE;
      const std::uint64_t end_child_index =
          std::min(first_child_index + LEAF_NODE_SIZE,
                   static_cast<std::uint64_t>(m_objects.size()));
      assert(first_child_index < std::numeric_limits<std::uint32_t>::max());
      assert(end_child_index < std::numeric_limits<std::uint32_t>::max());
      assert(end_child_index <= m_objects.size());

      return ranges::views::iota(first_child_index, end_child_index);
    }
    const std::uint64_t first_child_index =
        m_tree_level_starts[parent.level + 1] +
        parent.offset * BRANCHING_FACTOR;
    const std::uint64_t end_child_index =
        std::min(first_child_index + BRANCHING_FACTOR,
                 m_tree_level_starts[parent.level + 1] +
                     get_level_size(parent.level + 1));

    assert(first_child_index < std::numeric_limits<std::uint32_t>::max());
    assert(end_child_index < std::numeric_limits<std::uint32_t>::max());
    assert(end_child_index <= m_search_tree.size());
    assert(end_child_index <= m_tree_level_starts[parent.level + 1] +
                                  get_level_size(parent.level + 1));

    return ranges::views::iota(first_child_index, end_child_index);
  }

  bool is_leaf(const TreeIndex &tree_index) const {
    assert(m_tree_level_starts.size() > 1);
    return tree_index.level == m_tree_level_starts.size() - 2;
  }

public:
  static_assert(LEAF_PAGE_SIZE >= sizeof(E), "page_size is too small");
  static_assert(((LEAF_PAGE_SIZE - 1) & LEAF_PAGE_SIZE) == 0,
                "Page size is not a power of 2");
  static constexpr std::uint32_t LEAF_NODE_SIZE = (LEAF_PAGE_SIZE / sizeof(E));

  struct CandidateSegment {
    Coordinate fixed_projected_coordinate;
    E data;
  };

  struct TreeIndex {
    std::uint32_t level, offset;

    TreeIndex() : level(0), offset(0) {}

    TreeIndex(std::uint32_t level, std::uint32_t offset)
        : level(level), offset(offset) {}
  };

  struct TreeNode {
    RectangleInt2D minimum_bounding_rectangle;
  };

public:
  StaticRTree() = default;

  StaticRTree(const StaticRTree &) = delete;

  StaticRTree &operator=(const StaticRTree &) = delete;

  StaticRTree(StaticRTree &&) = default;

  StaticRTree &operator=(StaticRTree &&) = default;

  explicit StaticRTree(const std::vector<E> &input_data_vector,
                       const Vector<Coordinate> &coordinate_list,
                       const std::filesystem::path &on_disk_filename)
      : m_coordinate_list(coordinate_list.data(), coordinate_list.size()) {
    const auto elem_count = input_data_vector.size();
    std::vector<WrappedInputElement> input_wrapper_vector(elem_count);

    std::for_each(
        std::execution::par, ranges::views::iota(0, (int)elem_count),
        [&input_data_vector, &input_wrapper_vector, this](int index) {
          WrappedInputElement &current_wrapper = input_wrapper_vector[index];
          current_wrapper.original_index = index;

          E const &current_element = input_data_vector[index];
          assert(current_element.u < m_coordinate_list.size());
          assert(current_element.v < m_coordinate_list.size());

          Coordinate current_centroid = coordinate_calculation::centroid(
              m_coordinate_list[current_element.u],
              m_coordinate_list[current_element.v]);
          current_centroid.latitude = FixedLatitude{static_cast<std::int32_t>(
              COORDINATE_PRECISION *
              web_mercator::lat_to_y(to_floating(current_centroid.latitude)))};
          current_wrapper.hilbert_value = get_hilbert_code(current_centroid);
        });

    std::sort(std::execution::par, input_wrapper_vector.begin(),
              input_wrapper_vector.end());

    {
      boost::iostreams::mapped_file out_objects_region;
      auto out_objects = mmap_file<E>(on_disk_filename, out_objects_region,
                                      input_data_vector.size() * sizeof(E));

      std::size_t wrapped_elem_index = 0;
      auto objects_iter = out_objects.begin();

      while (wrapped_elem_index < elem_count) {
        TreeNode current_node;

        for (std::uint32_t object_index = 0;
             object_index < LEAF_NODE_SIZE and wrapped_elem_index < elem_count;
             ++object_index, ++wrapped_elem_index) {
          const std::uint32_t input_object_index =
              input_wrapper_vector[wrapped_elem_index].original_index;
          const E &object = input_data_vector[input_object_index];

          *objects_iter++ = object;

          Coordinate projected_u{web_mercator::from_wgs84(
              Coordinate{m_coordinate_list[object.u]})};
          Coordinate projected_v{web_mercator::from_wgs84(
              Coordinate(m_coordinate_list[object.v]))};

          assert(std::abs(to_floating(projected_u.longitude)) <= 180);
          assert(std::abs(to_floating(projected_u.latitude)) <= 180);
          assert(std::abs(to_floating(projected_v.longitude)) <= 180);
          assert(std::abs(to_floating(projected_v.latitude)) <= 180);

          RectangleInt2D rectangle;
          rectangle.min_lon =
              std::min(rectangle.min_lon,
                       std::min(projected_u.longitude, projected_v.longitude));
          rectangle.max_lon =
              std::max(rectangle.max_lon,
                       std::max(projected_u.longitude, projected_v.longitude));
          rectangle.min_lat =
              std::min(rectangle.min_lat,
                       std::min(projected_u.latitude, projected_v.latitude));
          rectangle.max_lat =
              std::max(rectangle.max_lat,
                       std::max(projected_u.latitude, projected_v.latitude));

          assert(rectangle.is_valid());
          current_node.minimum_bounding_rectangle = rectangle;
        }
        m_search_tree.emplace_back(current_node);
      }
    }

    m_objects = mmap_file<E>(on_disk_filename, m_objects_region);
    std::uint32_t nodes_in_previous_level = m_search_tree.size();
    std::vector<std::uint64_t> tree_level_sizes;
    tree_level_sizes.push_back(nodes_in_previous_level);

    while (nodes_in_previous_level > 1) {
      auto previous_level_start_pos =
          m_search_tree.size() - nodes_in_previous_level;
      std::uint32_t nodes_in_current_level = std::ceil(
          static_cast<double>(nodes_in_previous_level) / BRANCHING_FACTOR);

      for (auto current_node_index :
           ranges::views::iota(0, nodes_in_current_level)) {
        TreeNode parent_node;
        auto first_child_index =
            current_node_index * BRANCHING_FACTOR + previous_level_start_pos;
        auto last_child_index =
            first_child_index +
            std::min<std::size_t>(BRANCHING_FACTOR,
                                  nodes_in_previous_level -
                                      current_node_index * BRANCHING_FACTOR);

        for (auto child_node_index :
             ranges::views::iota(first_child_index, last_child_index)) {
          parent_node.minimum_bounding_rectangle.merge_bounding_boxes(
              m_search_tree[child_node_index].minimum_bounding_rectangle);
        }
        m_search_tree.emplace_back(parent_node);
      }
      nodes_in_previous_level = nodes_in_current_level;
      tree_level_sizes.push_back(nodes_in_previous_level);
    }

    std::reverse(m_search_tree.begin(), m_search_tree.end());
    std::reverse(tree_level_sizes.begin(), tree_level_sizes.end());

    m_tree_level_starts = {0};
    std::partial_sum(tree_level_sizes.begin(), tree_level_sizes.end(),
                     std::back_inserter(m_tree_level_starts));
    assert(m_tree_level_starts.size() >= 2);

    for (auto index : ranges::views::iota(0, tree_level_sizes.size())) {
      std::reverse(m_search_tree.begin() + m_tree_level_starts[index],
                   m_search_tree.begin() + m_tree_level_starts[index] +
                       tree_level_sizes[index]);
    }
  }

  template <typename std::enable_if<O == storage::Ownership::Container>>
  explicit StaticRTree(const std::filesystem::path &on_disk_filename,
                       const Vector<Coordinate> &coordinate_list)
      : m_coordinate_list(coordinate_list.data(), coordinate_list.size()) {
    m_objects = mmap_file<E>(on_disk_filename, m_objects_region);
  }

  /*explicit StaticRTree(Vector<TreeNode> search_tree,
                       Vector<std::uint64_t> tree_level_starts,
                       const std::filesystem::path &on_disk_filename,
                       const Vector<Coordinate> &coordinate_list)
      : m_search_tree(std::move(search_tree)),
        m_coordinate_list(coordinate_list.data(), coordinate_list.size()),
        m_tree_level_starts(std::move(tree_level_starts)) {

    assert(m_tree_level_starts.size() > 1);
    m_objects = mmap_file<E>(on_disk_filename, m_objects_region);
  }*/

  std::vector<E> search_in_box(const RectangleInt2D &search_rectangle) const {
    const RectangleInt2D projected_rectangle{
        search_rectangle.min_lon, search_rectangle.max_lon,
        to_fixed(FloatLatitude{web_mercator::lat_to_y(
            to_floating(FixedLatitude(search_rectangle.min_lat)))}),
        to_fixed(FloatLatitude{web_mercator::lat_to_y(
            to_floating(FixedLatitude(search_rectangle.max_lat)))})};
    std::vector<E> results;
    std::queue<TreeIndex> traversal_queue;
    traversal_queue.push(TreeIndex{});

    while (!traversal_queue.empty()) {
      auto const current_tree_index = traversal_queue.front();
      traversal_queue.pop();

      if (is_leaf(current_tree_index)) {
        for (const auto current_child_index :
             child_indices(current_tree_index)) {
          const auto &current_edge = m_objects[current_child_index];

          const RectangleInt2D bounding_box{
              std::min(m_coordinate_list[current_edge.u].longitude,
                       m_coordinate_list[current_edge.v].longitude),
              std::max(m_coordinate_list[current_edge.u].longitude,
                       m_coordinate_list[current_edge.v].longitude),
              std::min(m_coordinate_list[current_edge.u].latitude,
                       m_coordinate_list[current_edge.v].latitude),
              std::max(m_coordinate_list[current_edge.u].latitude,
                       m_coordinate_list[current_edge.v].latitude),
          };

          if (bounding_box.intersects(search_rectangle)) {
            results.push_back(current_edge);
          }
        }
      } else {
        assert(current_tree_index + 1 < m_tree_level_starts.size());

        for (const auto child_index : child_indices(current_tree_index)) {
          const auto &child_rectangle =
              m_search_tree[child_index].minimum_bounding_rectangle;

          if (child_rectangle.intersects(projected_rectangle)) {
            traversal_queue.push(TreeIndex(
                current_tree_index.level + 1,
                child_index -
                    m_tree_level_starts[current_tree_index.level + 1]));
          }
        }
      }
    }
    return results;
  }

  std::vector<E> nearest(const Coordinate input_coordinate,
                         const std::size_t &max_results) const {
    return nearest(
        input_coordinate,
        [](const CandidateSegment &) { return std::make_pair(true, true); },
        [max_results](const std::size_t num_results, const CandidateSegment &) {
          return num_results >= max_results;
        });
  }

  template <typename F, typename M>
  std::vector<E> nearest(const Coordinate input_coordinate, const F filter,
                         const M terminate) const {
    std::vector<E> results;
    auto projected_coordinate = web_mercator::from_wgs84(input_coordinate);
    Coordinate fixed_projected_coordinate{projected_coordinate};
    std::priority_queue<QueryCandidate> traversal_queue;
    traversal_queue.push(QueryCandidate{0, TreeIndex{}});

    while (!traversal_queue.empty()) {
      QueryCandidate current_query_node = traversal_queue.top();
      traversal_queue.pop();

      const TreeIndex &current_tree_index = current_query_node.tree_index;

      if (!current_query_node.is_segment()) {
        if (is_leaf(current_tree_index))
          explore_leaf_node(current_tree_index, fixed_projected_coordinate,
                            projected_coordinate, traversal_queue);
        else
          explore_tree_node(current_tree_index, fixed_projected_coordinate,
                            traversal_queue);
      } else {
        auto edge_data = m_objects[current_query_node.segment_index];
        const auto &current_candidate = CandidateSegment{
            current_query_node.fixed_projected_coordinate, edge_data};

        if (terminate(results.size(), current_candidate))
          break;

        auto use_segment = filter(current_candidate);

        if (!use_segment.first and !use_segment.second)
          continue;

        edge_data.forward_segment_id.enabled &= use_segment.first;
        edge_data.forward_segment_id.enabled &= use_segment.second;
        results.push_back(std::move(edge_data));
      }
    }
    return results;
  }
};
} // namespace util
} // namespace osram
#endif


//---------------------------------------------------------------------------//
// Copyright (c) 2018-2021 Mikhail Komarov <nemo@nil.foundation>
//
// MIT License
//
// 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 CRYPTO3_MAKE_UINT_T_HPP
#define CRYPTO3_MAKE_UINT_T_HPP

#include <tuple>

#include <boost/integer.hpp>

namespace nil {
    namespace crypto3 {
        namespace detail {
            template<std::size_t Size, typename Integer>
            static inline typename boost::uint_t<Size>::exact extract_uint_t(Integer v, std::size_t position) {
                return static_cast<typename boost::uint_t<Size>::exact>(v >>
                                                                        (((~position) & (sizeof(Integer) - 1)) << 3));
            }

            template<std::size_t Size, typename T>
            static inline typename boost::uint_t<Size>::exact make_uint_t(const std::initializer_list<T> &args) {
                typedef typename std::initializer_list<T>::value_type value_type;
                typename boost::uint_t<Size>::exact result = 0;

                for (const value_type &itr : args) {
                    result = static_cast<typename boost::uint_t<Size>::exact>(
                        (result << std::numeric_limits<value_type>::digits) | itr);
                }

                return result;
            }

            template<std::size_t Size, typename... Args>
            static inline typename boost::uint_t<Size>::exact make_uint_t(Args... args) {
                return make_uint_t<Size, typename std::tuple_element<0, std::tuple<Args...>>::type>({args...});
            }
        }    // namespace detail
    }        // namespace crypto3
}    // namespace nil

#endif    // CRYPTO3_MAKE_UINT_T_HPP


#include <boost/python.hpp>
#include <boost/python/module.hpp>
#include <opengm/python/opengmpython.hxx>
#include <opengm/python/converter.hxx>
#include <opengm/python/numpyview.hxx>
#include <opengm/learning/rws.hxx>

#define DefaultErrorFn DefaultErrorFn_TrwsExternalRws
#include "helper.hxx"

namespace bp = boost::python;
namespace op = opengm::python;
namespace ol = opengm::learning;

namespace opengm{

    template<class PARAM>
    PARAM * pyRwsParamConstructor(
    ){
        PARAM * p  = new PARAM();
        return p;
    }

    template<class L >
    L * pyRwsConstructor(
        typename L::DatasetType & dataset,
        const typename L::Parameter & param
    ){
        L * l  = new L(dataset, param);
        return l;
    }

    template<class DATASET>
    void export_rws_learner(const std::string & clsName){
        typedef learning::Rws<DATASET> PyLearner;
        typedef typename PyLearner::Parameter PyLearnerParam;

        const std::string paramClsName = clsName + std::string("Parameter");

        // learner param
        bp::class_<PyLearnerParam>(paramClsName.c_str(), bp::init<>())
            .def("__init__", make_constructor(&pyRwsParamConstructor<PyLearnerParam> ,boost::python::default_call_policies()))
            .def_readwrite("eps",  &PyLearnerParam::eps_)
            .def_readwrite("maxIterations", &PyLearnerParam::maxIterations_)
            .def_readwrite("stopLoss", &PyLearnerParam::stopLoss_)
            .def_readwrite("learningRate", &PyLearnerParam::learningRate_)
            .def_readwrite("C", &PyLearnerParam::C_)
            .def_readwrite("p", &PyLearnerParam::p_)
            .def_readwrite("sigma", &PyLearnerParam::sigma_)
        ;

        // learner
        bp::class_<PyLearner>( clsName.c_str(), bp::no_init )
        .def("__init__", make_constructor(&pyRwsConstructor<PyLearner> ,boost::python::default_call_policies()))
        .def(LearnerInferenceSuite<PyLearner>())
        ;
    }

    // template void
    // export_subgradient_ssvm_learner<op::GmAdderHammingLossDataset> (const std::string& className);

    // template void
    // export_subgradient_ssvm_learner<op::GmAdderGeneralizedHammingLossDataset> (const std::string& className);

    template void
    export_rws_learner<op::GmAdderFlexibleLossDataset> (const std::string& className);
}


#include <gtest/gtest.h>
#include <state_estimation_ros/ros_conversions.h>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <algorithm>
#include <numeric>

using namespace state_estimation;

// Test fixture which contains an assortment of data vectors to feed through the conversion
// functions
class StateEstimationRosConversions : public ::testing::Test {
  protected:
    void SetUp() override {
        // Provide some dummy values for each data vector
        xyz_vec.resize(3);
        xyz_vec << 1.1, 2.2, 3.3;

        rpy_vec.resize(3);
        rpy_vec << 0.111, 0.222, 0.333;

        // Make the quaternion vector from the roll pitch yaw angles
        Eigen::Quaterniond quat =
            (Eigen::Quaterniond)Eigen::AngleAxisd(rpy_vec(2), Eigen::Vector3d::UnitZ()) *
            Eigen::AngleAxisd(rpy_vec(1), Eigen::Vector3d::UnitY()) *
            Eigen::AngleAxisd(rpy_vec(0), Eigen::Vector3d::UnitX());

        quat_vec.resize(4);
        quat_vec << quat.x(), quat.y(), quat.z(), quat.w();

        pose_vec.resize(7);
        pose_vec << xyz_vec, quat_vec;

        pose_rpy_vec.resize(6);
        pose_rpy_vec << xyz_vec, rpy_vec;

        twist_vec.resize(6);
        twist_vec << 11.1, 22.2, 33.3, 44.4, 55.5, 66.6;

        imu_no_orient_vec.resize(6);
        imu_no_orient_vec << 0.1, 0.2, 0.3, 0.4, 0.5, 0.6;

        // Populate our sample covariance data. This will model something like a linear
        // acceleration covariance message (like in an IMU message). We'll make our internal
        // covariance matrix have more states though.
        //
        // We'll assume the covariance vector is for the state variables [Ax, Ay, Az], and we'll
        // have a state vector of [Vx, Ax, Vy, Ay]
        //
        // We also want to populate some of the cross terms, so we know if those get modified.
        // Make all the common terms <1 so we can identify them.
        cov_map = {1, 3, -1};

        cov_mat.resize(4, 4);
        // clang-format off
        cov_mat <<
            1.0, 1.1, 1.2, 1.3,
            1.1, 0.4, 1.5, 0.6,
            1.2, 1.5, 1.7, 1.8,
            1.3, 0.6, 1.8, 0.9;

        cov_vec = {
            cov_mat(1, 1), cov_mat(1, 3),   2.1,
            cov_mat(1, 3), cov_mat(3, 3),   2.2,
                2.3,            2.4,        2.5
        };
        // clang-format on
    }

    // Data vectors for each type of ROS message being converted
    Eigen::VectorXd xyz_vec;
    Eigen::VectorXd rpy_vec;
    Eigen::VectorXd quat_vec;
    Eigen::VectorXd pose_vec;
    Eigen::VectorXd pose_rpy_vec;
    Eigen::VectorXd twist_vec;
    Eigen::VectorXd imu_no_orient_vec;

    // Sample covariance entries
    Eigen::MatrixXd cov_mat;
    boost::array<double, 9> cov_vec;
    std::array<int, 3> cov_map;
};

// randomizeVector
//
// Pads a data vector with two extra values and generates a new map for the indices.
//
// Example: Given x=[1.1, 2.2, 3.3], it will be shuffled to be [PAD, 3.3, PAD, 2.2, 1.1], with
// a new map of {4, 3, 1}, where PAD is a random number inserted.
//
// @param x: Data vector to randomize
// @param x_random: Shuffled version of x with two extra elements
// @param map: Map with indices for x_random for the values from x
template <uint32_t N>
void randomizeVector(const Eigen::VectorXd& x, Eigen::VectorXd* x_random, std::array<int, N>* map) {
    // Make a map with entries {0, 1, ..., N + 1}, shuffle it, then copy over the first N elements
    // so we have a map containing any values from 0 to N + 1
    std::array<int, N + 2> map_padded;
    std::iota(map_padded.begin(), map_padded.end(), 0);
    std::random_shuffle(map_padded.begin(), map_padded.end());
    for (int i = 0; i < N; ++i) {
        (*map)[i] = map_padded[i];
    }

    // Fill out randomized vector with the padded value, then copy over the original data using the
    // naw map indices
    x_random->resize(N + 2);
    double pad_number = 9999.987654321;
    for (int i = 0; i < x_random->size(); ++i) {
        (*x_random)(i) = pad_number;
    }
    for (int i = 0; i < N; ++i) {
        (*x_random)((*map)[i]) = x(i);
    }
}

// msgCheck
//
// Helper function to convert a data vector to a message and back, and check that the values match.
// This will only compare the entries defined in the map
template <typename MsgT, uint32_t N>
void msgCheck(const Eigen::VectorXd& x, const std::array<int, N>& map) {
    Eigen::VectorXd output(x.size());
    MsgT msg;

    toMsg(x, map, &msg);
    fromMsg(msg, map, &output);

    bool all_match = true;
    for (int index : map) {
        all_match &= fabs(x(index) - output(index)) < 1e-6;
    }
    EXPECT_TRUE(all_match) << "Target: " << x.transpose() << ", Actual: " << output.transpose();
}

// randomizedMsgCheck
//
// Version of msgCheck() that will randomize the data vector and map.
template <typename MsgT, uint32_t N>
void randomizedMsgCheck(const Eigen::VectorXd& x, const std::array<int, N>& map) {
    Eigen::VectorXd x_random;
    std::array<int, N> map_random;
    randomizeVector<N>(x, &x_random, &map_random);

    msgCheck<MsgT, N>(x_random, map_random);
}

TEST_F(StateEstimationRosConversions, Point) {
    msgCheck<geometry_msgs::Point, 3>(xyz_vec, {0, 1, 2});
}

TEST_F(StateEstimationRosConversions, PointMixedSizeAndOrder) {
    randomizedMsgCheck<geometry_msgs::Point, 3>(xyz_vec, {0, 1, 2});
}

TEST_F(StateEstimationRosConversions, Vector3) {
    msgCheck<geometry_msgs::Vector3, 3>(xyz_vec, {0, 1, 2});
}

TEST_F(StateEstimationRosConversions, Vector3MixedSizeAndOrder) {
    randomizedMsgCheck<geometry_msgs::Vector3, 3>(xyz_vec, {0, 1, 2});
}

TEST_F(StateEstimationRosConversions, Quaternion) {
    msgCheck<geometry_msgs::Quaternion, 4>(quat_vec, {0, 1, 2, 3});
}

TEST_F(StateEstimationRosConversions, QuaternionMixedSizeAndOrder) {
    randomizedMsgCheck<geometry_msgs::Quaternion, 4>(quat_vec, {0, 1, 2, 3});
}

TEST_F(StateEstimationRosConversions, QuaternionRPY) {
    msgCheck<geometry_msgs::Quaternion, 3>(rpy_vec, {0, 1, 2});
}

TEST_F(StateEstimationRosConversions, QuaternionRPYMixedSizeAndOrder) {
    randomizedMsgCheck<geometry_msgs::Quaternion, 3>(rpy_vec, {0, 1, 2});
}

TEST_F(StateEstimationRosConversions, Pose) {
    msgCheck<geometry_msgs::Pose, 7>(pose_vec, {0, 1, 2, 3, 4, 5, 6});
}

TEST_F(StateEstimationRosConversions, PoseMixedSizeAndOrder) {
    randomizedMsgCheck<geometry_msgs::Pose, 7>(pose_vec, {0, 1, 2, 3, 4, 5, 6});
}

TEST_F(StateEstimationRosConversions, PoseRPY) {
    msgCheck<geometry_msgs::Pose, 6>(pose_rpy_vec, {0, 1, 2, 3, 4, 5});
}

TEST_F(StateEstimationRosConversions, PoseRPYMixedSizeAndOrder) {
    randomizedMsgCheck<geometry_msgs::Pose, 6>(pose_rpy_vec, {0, 1, 2, 3, 4, 5});
}

TEST_F(StateEstimationRosConversions, Twist) {
    msgCheck<geometry_msgs::Twist, 6>(twist_vec, {0, 1, 2, 3, 4, 5});
}

TEST_F(StateEstimationRosConversions, TwistMixedSizeAndOrder) {
    randomizedMsgCheck<geometry_msgs::Twist, 6>(twist_vec, {0, 1, 2, 3, 4, 5});
}

TEST_F(StateEstimationRosConversions, ImuNoOrientation) {
    msgCheck<sensor_msgs::Imu, 6>(imu_no_orient_vec, {0, 1, 2, 3, 4, 5});
}

TEST_F(StateEstimationRosConversions, ImuNoOrientationMixedSizeAndOrder) {
    randomizedMsgCheck<sensor_msgs::Imu, 6>(imu_no_orient_vec, {0, 1, 2, 3, 4, 5});
}

TEST_F(StateEstimationRosConversions, Float64MultiArraySizeCorrect) {
    double timestamp = 12345.6789;
    Eigen::VectorXd x(2);
    Eigen::MatrixXd cov(2, 2);

    std_msgs::Float64MultiArray msg;
    toMsg(timestamp, x, cov, &msg);
    ASSERT_EQ(1, msg.layout.dim.size());
    EXPECT_EQ(7, msg.layout.dim[0].size);
    EXPECT_EQ(7, msg.data.size());
}

TEST_F(StateEstimationRosConversions, Float64MultiArrayContentsCorrect) {
    double timestamp = 12345.6789;

    Eigen::VectorXd x(2);
    x << 1.1, 2.2;

    Eigen::MatrixXd cov(2, 2);
    cov << 1, 2, 3, 4;

    std_msgs::Float64MultiArray msg;
    toMsg(timestamp, x, cov, &msg);

    ASSERT_EQ(7, msg.data.size());
    EXPECT_EQ(timestamp, msg.data[0]);
    EXPECT_EQ(x(0), msg.data[1]);
    EXPECT_EQ(x(1), msg.data[2]);
    EXPECT_EQ(cov(0, 0), msg.data[3]);
    EXPECT_EQ(cov(0, 1), msg.data[4]);
    EXPECT_EQ(cov(1, 0), msg.data[5]);
    EXPECT_EQ(cov(1, 1), msg.data[6]);
}

TEST_F(StateEstimationRosConversions, CovarianceMatrixToVector) {
    boost::array<double, 9> cov_converted;
    covarianceMatrixToVector<3>(cov_mat, cov_map, &cov_converted);

    // Only check the common terms on the converted covariance, so any entries in the ground truth
    // that are <1
    for (int i = 0; i < cov_vec.size(); ++i) {
        if (cov_vec[i] < 1) {
            EXPECT_EQ(cov_vec[i], cov_converted[i]) << "Index " << i;
        }
    }
}

TEST_F(StateEstimationRosConversions, CovarianceMatrixToVectorDoesNotAlterOtherTerms) {
    boost::array<double, 9> cov_converted = cov_vec;
    covarianceMatrixToVector<3>(cov_mat, cov_map, &cov_converted);

    // Check that the non common terms were left unchanged, so any entries in the ground truth that
    // are >= 1
    for (int i = 0; i < cov_vec.size(); ++i) {
        if (cov_vec[i] >= 1) {
            EXPECT_EQ(cov_vec[i], cov_converted[i]) << "Index " << i;
        }
    }
}

TEST_F(StateEstimationRosConversions, CovarianceVectorToMatrix) {
    Eigen::MatrixXd cov_converted(cov_mat.rows(), cov_mat.cols());
    covarianceVectorToMatrix<3>(cov_vec, cov_map, &cov_converted);

    // Only check the common terms on the converted covariance, so any entries in the ground truth
    // that are <1
    for (int i = 0; i < cov_converted.rows(); ++i) {
        for (int j = 0; j < cov_converted.cols(); ++j) {
            if (cov_mat(i, j) < 1) {
                EXPECT_EQ(cov_mat(i, j), cov_converted(i, j)) << "Index [" << i << ", " << j << "]";
            }
        }
    }
}

TEST_F(StateEstimationRosConversions, CovarianceVectorToMatrixDoesNotAlterOtherTerms) {
    Eigen::MatrixXd cov_converted = cov_mat;
    covarianceVectorToMatrix<3>(cov_vec, cov_map, &cov_converted);

    // Only check the common terms on the converted covariance, so any entries in the ground truth
    // that are <1
    for (int i = 0; i < cov_converted.rows(); ++i) {
        for (int j = 0; j < cov_converted.cols(); ++j) {
            if (cov_mat(i, j) >= 1) {
                EXPECT_EQ(cov_mat(i, j), cov_converted(i, j)) << "Index [" << i << ", " << j << "]";
            }
        }
    }
}