/**************************************************************
 *
 * Licensed to the Apache Software Foundation (ASF) under one
 * or more contributor license agreements.  See the NOTICE file
 * distributed with this work for additional information
 * regarding copyright ownership.  The ASF licenses this file
 * to you under the Apache License, Version 2.0 (the
 * "License"); you may not use this file except in compliance
 * with the License.  You may obtain a copy of the License at
 *
 *   http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing,
 * software distributed under the License is distributed on an
 * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
 * KIND, either express or implied.  See the License for the
 * specific language governing permissions and limitations
 * under the License.
 *
 *************************************************************/

#ifndef INCLUDED_SLIDESHOW_SMILFUNCTIONPARSER_HXX
#define INCLUDED_SLIDESHOW_SMILFUNCTIONPARSER_HXX

#include "expressionnode.hxx"
#include "slideshowexceptions.hxx"

#include <basegfx/range/b2drectangle.hxx>

#include <boost/noncopyable.hpp>

namespace rtl
{
    class OUString;
}

/* Definition of SmilFunctionParser class */

namespace slideshow
{
    namespace internal
    {
        class SmilFunctionParser : private boost::noncopyable
        {
        public:
            /** Parse a string containing a SMIL value.

            	This method parses a string representing
                a fixed value (i.e. a value that does not
                change by time). Due to the dynamic view
                capabilities of the presentation engine,
                this value can sometimes only be determined
                during runtime of the animation (because
                e.g. mixed screen/view coordinates are
                involved), and is thus still returned as an
                ExpressionNode object. An example for
                such a case is the "Width+1.0" string, which
                contains the width of the shape in user
                coordinate space, and the screen width
                in device coordinate space.

                The following grammar is accepted by this method:

               identifier = 'pi'|'e'|'X'|'Y'|'Width'|'Height'

               function = 'abs'|'sqrt'|'sin'|'cos'|'tan'|'atan'|'acos'|'asin'|'exp'|'log'

               basic_expression =
               				 number |
               				 identifier |
               				 function '(' additive_expression ')' |
               				 '(' additive_expression ')'

               unary_expression =
               					'-' basic_expression |
                                basic_expression

               multiplicative_expression =
               				    unary_expression ( ( '*' unary_expression )* |
                                				   ( '/' unary_expression )* )

               additive_expression =
               					multiplicative_expression ( ( '+' multiplicative_expression )* |
               											    ( '-' multiplicative_expression )* )

                @param rSmilValue
                The string to parse

                @param rRelativeShapeBounds
                The bounds of the shape this SMIL value is to be
                evaluated for. The bounds must be relative to
                the page the shape is part of, i.e. within the [0,1]
                range. This is necessary, since the string might
                contain symbolic references to the shape bounding box.

                @throws ParseError if an invalid expression is given.

                @return the generated function object.
             */
            static ExpressionNodeSharedPtr parseSmilValue( const ::rtl::OUString& 			rSmilValue,
                                                           const ::basegfx::B2DRectangle&	rRelativeShapeBounds ); // throw ParseError

            /** Parse a string containing a SMIL function.

            	This method parses a string representing
                a possibly time-varying SMIL function.

                The following grammar is accepted by this method:

               identifier = 't'|'pi'|'e'|'X'|'Y'|'Width'|'Height'

               function = 'abs'|'sqrt'|'sin'|'cos'|'tan'|'atan'|'acos'|'asin'|'exp'|'log'

               basic_expression =
               				 number |
               				 identifier |
               				 function '(' additive_expression ')' |
               				 '(' additive_expression ')'

               unary_expression =
               					'-' basic_expression |
                                basic_expression

               multiplicative_expression =
               				    unary_expression ( ( '*' unary_expression )* |
                                				   ( '/' unary_expression )* )

               additive_expression =
               					multiplicative_expression ( ( '+' multiplicative_expression )* |
               											    ( '-' multiplicative_expression )* )

                @param rSmilFunction
                The string to parse

                @param rRelativeShapeBounds
                The bounds of the shape this SMIL value is to be
                evaluated for. The bounds must be relative to
                the page the shape is part of, i.e. within the [0,1]
                range. This is necessary, since the string might
                contain symbolic references to the shape bounding box.

                @throws ParseError if an invalid expression is given.

                @return the generated function object.
             */
            static ExpressionNodeSharedPtr parseSmilFunction( const ::rtl::OUString& 			rSmilFunction,
                                                              const ::basegfx::B2DRectangle&	rRelativeShapeBounds ); // throw ParseError

        private:
            // disabled constructor/destructor, since this is
            // supposed to be a singleton
            SmilFunctionParser();
        };
    }
}

#endif /* INCLUDED_SLIDESHOW_SMILFUNCTIONPARSER_HXX */


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

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

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

// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_GEOMETRY_ALGORITHMS_ENVELOPE_HPP
#define BOOST_GEOMETRY_ALGORITHMS_ENVELOPE_HPP

#include <vector>

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

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

#include <boost/geometry/algorithms/assign.hpp>
#include <boost/geometry/algorithms/expand.hpp>
#include <boost/geometry/algorithms/not_implemented.hpp>
#include <boost/geometry/core/cs.hpp>
#include <boost/geometry/core/exterior_ring.hpp>
#include <boost/geometry/core/point_type.hpp>
#include <boost/geometry/core/tags.hpp>

#include <boost/geometry/geometries/concepts/check.hpp>

namespace boost { namespace geometry
{

#ifndef DOXYGEN_NO_DETAIL
namespace detail { namespace envelope
{

/// Calculate envelope of an 2D or 3D segment
struct envelope_expand_one
{
    template<typename Geometry, typename Box>
    static inline void apply(Geometry const& geometry, Box& mbr)
    {
        assign_inverse(mbr);
        geometry::expand(mbr, geometry);
    }
};

/// Iterate through range (also used in multi*)
template<typename Range, typename Box>
inline void envelope_range_additional(Range const& range, Box& mbr)
{
    typedef typename boost::range_iterator<Range const>::type iterator_type;

    for (iterator_type it = boost::begin(range);
        it != boost::end(range);
        ++it)
    {
        geometry::expand(mbr, *it);
    }
}

/// Generic range dispatching struct
struct envelope_range
{
    /// Calculate envelope of range using a strategy
    template <typename Range, typename Box>
    static inline void apply(Range const& range, Box& mbr)
    {
        assign_inverse(mbr);
        envelope_range_additional(range, mbr);
    }
};

struct envelope_multi_linestring
{
    template<typename MultiLinestring, typename Box>
    static inline void apply(MultiLinestring const& mp, Box& mbr)
    {
        assign_inverse(mbr);
        for (typename boost::range_iterator<MultiLinestring const>::type
                it = mp.begin();
            it != mp.end();
            ++it)
        {
            envelope_range_additional(*it, mbr);
        }
    }
};

// version for multi_polygon: outer ring's of all polygons
struct envelope_multi_polygon
{
    template<typename MultiPolygon, typename Box>
    static inline void apply(MultiPolygon const& mp, Box& mbr)
    {
        assign_inverse(mbr);
        for (typename boost::range_const_iterator<MultiPolygon>::type
                it = mp.begin();
            it != mp.end();
            ++it)
        {
            envelope_range_additional(exterior_ring(*it), mbr);
        }
    }
};

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

#ifndef DOXYGEN_NO_DISPATCH
namespace dispatch
{

template
<
    typename Geometry,
    typename Tag = typename tag<Geometry>::type
>
struct envelope: not_implemented<Tag>
{};

template <typename Point>
struct envelope<Point, point_tag>
    : detail::envelope::envelope_expand_one
{};

template <typename Box>
struct envelope<Box, box_tag>
    : detail::envelope::envelope_expand_one
{};

template <typename Segment>
struct envelope<Segment, segment_tag>
    : detail::envelope::envelope_expand_one
{};

template <typename Linestring>
struct envelope<Linestring, linestring_tag>
    : detail::envelope::envelope_range
{};

template <typename Ring>
struct envelope<Ring, ring_tag>
    : detail::envelope::envelope_range
{};

template <typename Polygon>
struct envelope<Polygon, polygon_tag>
    : detail::envelope::envelope_range
{
    template <typename Box>
    static inline void apply(Polygon const& poly, Box& mbr)
    {
        // For polygon, inspecting outer ring is sufficient
        detail::envelope::envelope_range::apply(exterior_ring(poly), mbr);
    }

};

template <typename Multi>
struct envelope<Multi, multi_point_tag>
    : detail::envelope::envelope_range
{};

template <typename Multi>
struct envelope<Multi, multi_linestring_tag>
    : detail::envelope::envelope_multi_linestring
{};

template <typename Multi>
struct envelope<Multi, multi_polygon_tag>
    : detail::envelope::envelope_multi_polygon
{};

} // namespace dispatch
#endif

namespace resolve_variant {

template <typename Geometry>
struct envelope
{
    template <typename Box>
    static inline void apply(Geometry const& geometry, Box& box)
    {
        concept::check<Geometry const>();
        concept::check<Box>();

        dispatch::envelope<Geometry>::apply(geometry, box);
    }
};

template <BOOST_VARIANT_ENUM_PARAMS(typename T)>
struct envelope<boost::variant<BOOST_VARIANT_ENUM_PARAMS(T)> >
{
    template <typename Box>
    struct visitor: boost::static_visitor<void>
    {
        Box& m_box;

        visitor(Box& box): m_box(box) {}

        template <typename Geometry>
        void operator()(Geometry const& geometry) const
        {
            envelope<Geometry>::apply(geometry, m_box);
        }
    };

    template <typename Box>
    static inline void
    apply(boost::variant<BOOST_VARIANT_ENUM_PARAMS(T)> const& geometry,
          Box& box)
    {
        boost::apply_visitor(visitor<Box>(box), geometry);
    }
};

} // namespace resolve_variant

/*!
\brief \brief_calc{envelope}
\ingroup envelope
\details \details_calc{envelope,\det_envelope}.
\tparam Geometry \tparam_geometry
\tparam Box \tparam_box
\param geometry \param_geometry
\param mbr \param_box \param_set{envelope}

\qbk{[include reference/algorithms/envelope.qbk]}
\qbk{
[heading Example]
[envelope] [envelope_output]
}
*/
template<typename Geometry, typename Box>
inline void envelope(Geometry const& geometry, Box& mbr)
{
    resolve_variant::envelope<Geometry>::apply(geometry, mbr);
}

/*!
\brief \brief_calc{envelope}
\ingroup envelope
\details \details_calc{return_envelope,\det_envelope}. \details_return{envelope}
\tparam Box \tparam_box
\tparam Geometry \tparam_geometry
\param geometry \param_geometry
\return \return_calc{envelope}

\qbk{[include reference/algorithms/envelope.qbk]}
\qbk{
[heading Example]
[return_envelope] [return_envelope_output]
}
*/
template<typename Box, typename Geometry>
inline Box return_envelope(Geometry const& geometry)
{
    Box mbr;
    resolve_variant::envelope<Geometry>::apply(geometry, mbr);
    return mbr;
}

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_ALGORITHMS_ENVELOPE_HPP


///
/// @file
/// @copyright Copyright (C) 2017-2018, Jonathan Bryan Schmalhofer
///
/// @brief Class with Utils for handling KITTI DataSet easier
///
#ifndef KITTI_UTILS_HPP_
#define KITTI_UTILS_HPP_

#include <cstdint>
#include <string>
#include <vector>
#include <fstream>      // std::ifstream
#include <iostream>
#include <cstddef>      // std::size_t - see: https://stackoverflow.com/questions/36594569/which-header-should-i-include-for-size-t
#include <cmath>        // cos

#include <boost/date_time.hpp>
#include <boost/filesystem/path.hpp>
#include <boost/filesystem.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/math/constants/constants.hpp>   // pi - see: https://stackoverflow.com/questions/21867617/best-platform-independent-pi-constant
#include <boost/numeric/ublas/matrix_proxy.hpp>

#include <Eigen/Dense>    // for Eigen::MatrixXd
#include <Eigen/Geometry> // for eulerAngles()

#include "invert_matrix.hpp"

#include "oxts_type.hpp"

namespace kitti_utils
{

// Source for GetFileList(): https://gist.github.com/vivithemage/9517678
std::vector<std::string> GetFileList(const std::string& path, const std::string extension);

class KittiUtils
{
public:
    KittiUtils();

    ~KittiUtils();

    void LoadTimestamps(std::string path_timestamp_file, std::vector<long>& time, int& counter);
    void LoadOxtsLitePaths(std::string path_gpsimu_file, std::vector<long>& time, std::vector<std::string>& filelist);
    double GetScale(std::vector<std::string> filelist);
    boost::numeric::ublas::matrix<double> GetTr0(std::vector<std::string> filelist, double scale);
    boost::numeric::ublas::matrix<double> GetTransformation(oxts_data datapoint, double scale);
    oxts_data GetOxtsData(std::vector<std::string> filelist, std::vector<long> timestamps, std::size_t idx);
    boost::numeric::ublas::matrix<double> ConvertOxtsToPose(oxts_data datapoint, double scale, boost::numeric::ublas::matrix<double> Tr_0_inv);
    void GetEulerAngles(boost::numeric::ublas::matrix<double> pose, double &roll, double &pitch, double &yaw);
    void GetPosition(boost::numeric::ublas::matrix<double> pose, double &x, double &y, double &z);

private:

};
}  // namespace kitti_utils

#endif  // KITTI_UTILS_HPP_


/*
 *  The core class (implementation) for roadmap building
 *
 *  Created on: Jan 30, 2015
 *  Author: Jingjin Yu
 */

#include "roadmap.h"
#include "helper_functions.h"

#include <utility>
#include <algorithm>
#include <vector>

#include <CGAL/Qt/Converter.h>
#include <CGAL/Boolean_set_operations_2.h>

#include <QGraphicsSimpleTextItem>

#include <boost/graph/graph_traits.hpp>
#include <boost/graph/lookup_edge.hpp>

/*------------------------------------------------------------------------------------------
			see resources/lattice-indexing.pptx for the processing logic
--------------------------------------------------------------------------------------------*/

static const QColor BASIC_QCOLORS8[] = {Qt::red, Qt::blue, Qt::green, Qt::magenta, Qt::cyan, Qt::gray, Qt::black, Qt::yellow};
static int colorCounter = 0;

void Roadmap::buildRoadmap(Polygon2_list* pObsList, Polygon_2 *pBoundingRect, double radius){
	// Populate some internal variables for use across calls
	m_radius = radius;
	m_edgeLength = radius/0.43;
	m_obstaclePolyList = *pObsList;
	m_pBoundingRect = pBoundingRect;
	if(m_pVisibilityGraph != 0){
		delete m_pVisibilityGraph;
		m_pVisibilityGraph = 0;
	}
	if(m_pEnvironment != 0){
		delete m_pEnvironment;
		m_pEnvironment = 0;
	}

	// Some basic setup
	ICPoint_2 bottomLeft = K_ICK_converter((*m_pBoundingRect)[0]);
	ICPoint_2 topRight = K_ICK_converter((*m_pBoundingRect)[2]);

	bottomLeftX = bottomLeft.x();
	bottomLeftY = bottomLeft.y();
	width = topRight.x() - bottomLeft.x();
	height = topRight.y() - bottomLeft.y();;
	sqrt3 = sqrt(3.0);

	// Compute number of columns and rows
	n_w = (int)(ceil(width/(m_edgeLength*3/2))) + 3;
	n_h = (int)(ceil(height/(m_edgeLength*sqrt3))) + 3;

	// The lattice start x, y
	xs = bottomLeftX - (3/2)*m_edgeLength*1.35;
	ys = bottomLeftY - sqrt3*m_edgeLength*1.4;

	// Clean up from previous build
	m_pointList.clear();
	m_vidPointMap.clear();
	m_pointVidMap.clear();
	m_graph.clear();
	m_finalGraph.clear();
	m_vidFGMap.clear();
	m_vidGFMap.clear();
	m_edgeToBeRemovedSet.clear();
	m_vertexToBeRemovedSet.clear();
	m_boundaryBoundingCycle.clear();
	m_connectingPathMap.clear();
	for(std::vector<Graph*>::iterator git = m_obsBoundingCycleVec.begin(); git != m_obsBoundingCycleVec.end(); git++){
		delete (*git);
	}
	m_obsBoundingCycleVec.clear();

	/*
	// Build the roadmap, first obtain a lattice that cover the outer boundary
	buildHexgaonLattice();

	// Remove extra edges, at the same time, find smallest cycles enclosing the obstacles
	removeExcessEdges();

	// Preserve connectivity
	checkAndFixConnectivity();
*/
}

void Roadmap::removeExcessEdges(){
	// We do this in several steps. First, we go through each polygon obstacle boundary
	// and delete all edges of the lattice that intersect with these boundaries. Then,
	// we find the connected components of the remaining lattice graph. For each component
	// we only need to test one vertex to know whether it belongs to the configuration space
	// or not. We keep all components that belong to the configuration space

	// =====================================================================================
	// Compute the set of edges that falls on obstacle boundaries, at the same time also
	// compute the smallest cycle in the full lattice that encloses the obstacle. First do
	// it for the bounding polygon
	getIntersectingEdges(*m_pBoundingRect, m_boundaryBoundingCycle, true);

	// Then for all obstacles
	for(Polygon2_list::iterator obsit = m_obstaclePolyList.begin(); obsit != m_obstaclePolyList.end(); obsit++){
		Graph* pg = new Graph();
		m_obsBoundingCycleVec.push_back(pg);
		getIntersectingEdges(*obsit, *pg);
	}

	// =====================================================================================
	// Remove edges that do not belong to the graph
	Graph g;
	std::set<std::pair<int, int> > edgeSet = m_graph.getEdgeSet();
	for(std::set<std::pair<int, int> >::iterator eit = edgeSet.begin(); eit != edgeSet.end(); eit++){
		int fv = (*eit).first;
		int sv = (*eit).second;
		if(!edgeInSet(fv, sv, m_edgeToBeRemovedSet)){
			g.addEdge(fv, sv);
		}
	}
	m_graph = g;

	// =====================================================================================
	// Go through all vertices and find all vertices and edges inside the configuration
	// space. To do so, iterate over all lattices points (the full lattice minus the edges that
	// crosses obstacle boundaries) and for each point, if it has not been checked, see whether
	// the point is inside the configuration space. Then we do a BFS from the point and visit
	// all points/edges connected to the point. We add the edges to our final graph if and only
	// if the starting vertex is in the c-space. This way, we need to do c-space membership
	// check geometrically only very limited number of times, usually around the number of
	// obstacles in the c-space.

	g.clear();
	std::set<int> visitedVertices;
	for(std::map<int, Point_2>::iterator vit = m_vidPointMap.begin(); vit != m_vidPointMap.end(); vit++){
		int vid = vit->first;
		if(visitedVertices.find(vid) == visitedVertices.end()){
			visitedVertices.insert(vid);
			// Test whether the vertex is inside the configuration space
			bool inCSpace = isPointInCSpace(vit->second);

			// Do BFS
			std::list<int> tempQueue;
			tempQueue.push_back(vid);
			while(tempQueue.size() > 0){
				int current = tempQueue.front();
				tempQueue.pop_front();

				std::set<int> neighborSet = m_graph.getNeighborSet(current);

				for(std::set<int>::iterator vit = neighborSet.begin(); vit != neighborSet.end(); vit++){
					// Retrieve first and second vertices
					int et = *vit;
					if(visitedVertices.find(et) == visitedVertices.end()){
						visitedVertices.insert(et);
						tempQueue.push_back(et);
					}
					// Add edge as needed
					if(inCSpace) g.addEdge(current, et);
				}
			}
		}
	}
	m_graph.clear();
	m_graph = g;

	// Remove isolated vertices
	for(std::set<int>::iterator vit = m_vertexToBeRemovedSet.begin(); vit != m_vertexToBeRemovedSet.end(); vit++){
		// std::cout << "Removing vertex with id: " << *vit << std::endl;
		m_graph.removeVertex(*vit);
	}

	// Remove single degree edges
	std::set<int> vSetCopy = m_graph.getVertexSet();
	for(std::set<int>::iterator vit = vSetCopy.begin(); vit != vSetCopy.end(); vit++){
		if(m_graph.hasVertex(*vit)){
			if(m_graph.getNeighborSet(*vit).size() == 0){
				m_graph.removeVertex(*vit);
				// std::cout << "Removing vertex with id: " << *vit << std::endl;
			}
		}
	}

	buildFinalGraph();
}

void Roadmap::buildFinalGraph(){
	// Construct m_finalGraph
	std::set<std::pair<int, int>>& eSet = m_graph.getEdgeSet();
	int vIDCount = 0;
	for(std::set<std::pair<int, int> >::iterator eit = eSet.begin(); eit != eSet.end(); eit++){
		int fv = eit->first;
		int sv = eit->second;

		// Check whether fv was added
		if(m_vidGFMap.find(fv) == m_vidGFMap.end()){
			m_vidGFMap[fv] = vIDCount;
			m_vidFGMap[vIDCount] = fv;
			fv = vIDCount++;
		}
		else{
			fv = m_vidGFMap[fv];
		}

		// Check whether sv was added
		if(m_vidGFMap.find(sv) == m_vidGFMap.end()){
			m_vidGFMap[sv] = vIDCount;
			m_vidFGMap[vIDCount] =sv;
			sv = vIDCount++;
		}
		else{
			sv = m_vidGFMap[sv];
		}
		m_finalGraph.addEdge(fv, sv);
	}
}

void Roadmap::drawBoundingCycle(QGraphicsScene& scene){
	// Draw the cycles
	QPen regularPen = QPen(QColor(0, 255, 0, 127), 0.5, Qt::SolidLine, Qt::RoundCap, Qt::RoundJoin);
	QPen illegalPen = QPen(QColor(255, 0, 0, 127), 0.5, Qt::SolidLine, Qt::RoundCap, Qt::RoundJoin);
	for(std::vector<Graph*>::iterator git = m_obsBoundingCycleVec.begin(); git != m_obsBoundingCycleVec.end(); git++){
		Graph& g = *(*git);
		std::set<std::pair<int, int> > edgeSet = g.getEdgeSet();
		for(std::set<std::pair<int, int> >::iterator eit = edgeSet.begin(); eit != edgeSet.end(); eit++){
			int v1 = (*eit).first;
			int v2 = (*eit).second;
			ICPoint_2 p1 = K_ICK_converter(m_vidPointMap[v1]);
			ICPoint_2 p2 = K_ICK_converter(m_vidPointMap[v2]);
			if(m_graph.hasEdge(v1, v2)){
				scene.addLine(p1.x(), p1.y(), p2.x(), p2.y(), regularPen);
			}
			else{
				scene.addLine(p1.x(), p1.y(), p2.x(), p2.y(), illegalPen);
			}
		}
	}
}

bool Roadmap::isPointInCSpace(Point_2 &p){
	// Check whether the point is inside the bounding rect
	if(m_pBoundingRect->bounded_side(p) == CGAL::ON_UNBOUNDED_SIDE){
		return false;
	}

    for(Polygon2_list::iterator pli = m_obstaclePolyList.begin(); pli != m_obstaclePolyList.end(); pli++){
		Polygon_2 &tp = *(pli);
		if(tp.bounded_side(p) == CGAL::ON_BOUNDED_SIDE){
			return false;
		}
	}
	return true;
}

void Roadmap::drawVertexIds(QGraphicsScene& scene){
	// Draw the id text of the vretex
	QFont font;
	QPainterPath path;
	font.setPointSizeF(m_radius/1.5);
	font.setBold(false);
	for(std::set<int>::iterator vit = m_graph.getVertexSet().begin(); vit != m_graph.getVertexSet().end(); vit ++){
		ICPoint_2 p = K_ICK_converter(m_vidPointMap[*vit]);
		QGraphicsSimpleTextItem *ti = scene.addSimpleText(QString::number(m_vidGFMap[*vit]), font);
		ti->setPos(p.x() + m_radius/2, p.y() - m_radius/2);
		ti->setPen(QPen(QColor(Qt::green), 0.03*m_radius, Qt::SolidLine, Qt::RoundCap,Qt::RoundJoin));
		ti->setZValue(2);
	}
}

void Roadmap::buildVisibilityGraph(){
	// To build the visibility graph, we use the package VisiLibity, which is not precise arithematic,
	// but good for our purpose since we do not use visibility graph as part of our main logic.

	// Only build once per roadmap
	if(m_pVisibilityGraph != 0) return;

	// Vector to hold all polygon for contructing VisiLibity object
	vector<Polygon> polyVec;

	// We assume that the infated obstacles do not intersect each other but may intersect the boundary.
	// Therefore, we test such intersection and obtain an updated boundary (the inside)
	ECPolygon_2 boundary = convertToExactPolygon(*m_pBoundingRect);
	if(boundary.is_clockwise_oriented()) boundary.reverse_orientation();
	for(Polygon2_list::iterator pli = m_obstaclePolyList.begin(); pli != m_obstaclePolyList.end(); pli++){
		ECPolygon_2 ecPoly = convertToExactPolygon(*pli);
		if(boundaryInterset(boundary, ecPoly)){
			vector<ECPolygon_with_holes_2> outVec;

			// Remove the obstacle "from" the boundary polygon
			CGAL::difference(boundary, ecPoly, back_inserter(outVec));

			// Update boundary
			boundary = outVec[0].outer_boundary();
		}
		else{
			// Add all non intersecting obstacles
			Polygon tempPoly;
			converFromCGALtoVisilibity(ecPoly, tempPoly, false);
			polyVec.push_back(tempPoly);
		}
	}

	// Insert the boundary
	Polygon boundaryPoly;
	converFromCGALtoVisilibity(boundary, boundaryPoly, true);
	polyVec.insert(polyVec.begin(), boundaryPoly);

	// Build environment and visibility graph
	m_pEnvironment = new Environment(polyVec);
	m_pVisibilityGraph = new Visibility_Graph(*m_pEnvironment, m_epsilon);
}

double Roadmap::computeShortestPath(Point_2& p1, Point_2& p2, std::list<Point_2> & path){
	ICPoint_2 p1x = K_ICK_converter(p1);
	ICPoint_2 p2x = K_ICK_converter(p2);
	return computeShortestPath(p1x.x(), p1x.y(), p2x.x(), p2x.y(), path);
}

double Roadmap::computeShortestPath(double x1, double y1, double x2, double y2, std::list<Point_2> & path){
	Polyline pl = m_pEnvironment->shortest_path(Point(x1, y1), Point(x2, y2), *m_pVisibilityGraph, m_epsilon);
	for(int i = 0; i < pl.size(); i++){
		path.push_back(Point_2(pl[i].x(), pl[i].y()));
	}
	return getPathLength(path);
}

double Roadmap::computeShortestPath(double x1, double y1, double x2, double y2, vector<pair<double, double> >& path){
	// Compute shortest path
	std::list<Point_2> shortestPath;
	double length = computeShortestPath(x1, y1, x2, y2, shortestPath);

	// Convert the path
	for(std::list<Point_2>::iterator vi = shortestPath.begin(); vi != shortestPath.end(); vi++){
		ICPoint_2 p = K_ICK_converter(*vi);
		path.push_back(pair<double, double>(p.x(), p.y()));
	}
	return length;
}

void Roadmap::addToScene(QGraphicsScene& scene, bool drawEdge, QPen edgePen, bool drawVertex, QPen vertexPen){
	// Paint the edges
	if(drawEdge){
		std::set<std::pair<int, int> > edgeSet = m_graph.getEdgeSet();
		for(std::set<std::pair<int, int> >::iterator eit = edgeSet.begin(); eit != edgeSet.end(); eit++){
			int v1 = (*eit).first;
			int v2 = (*eit).second;
			ICPoint_2 p1 = K_ICK_converter(m_vidPointMap[v1]);
			ICPoint_2 p2 = K_ICK_converter(m_vidPointMap[v2]);
			scene.addLine(p1.x(), p1.y(), p2.x(), p2.y(), edgePen);
			/*}*/

			if(m_boundaryBoundingCycle.hasEdge(v1, v2)){
				scene.addLine(p1.x(), p1.y(), p2.x(), p2.y(), QPen(Qt::blue, 0.05, Qt::SolidLine, Qt::RoundCap, Qt::RoundJoin));
			}
			else{
				for(std::vector<Graph*>::iterator git = m_obsBoundingCycleVec.begin(); git != m_obsBoundingCycleVec.end(); git++){
					if((*git)->hasEdge(v1, v2)){
						scene.addLine(p1.x(), p1.y(), p2.x(), p2.y(), QPen(Qt::yellow, 0.05, Qt::SolidLine, Qt::RoundCap, Qt::RoundJoin));
					}
				}
			}
		}
	}

	// Paint vertices if needed and obtain the fill area
	if(!drawVertex){
		for(std::list<Point_2>::iterator it = m_pointList.begin(); it != m_pointList.end(); it ++){
			ICPoint_2 p = K_ICK_converter(*it);
			scene.addEllipse(p.x() - 0.025, p.y() - 0.025, 0.05, 0.05, vertexPen);
		}
	}

	for(Path_Map::iterator pathIt = m_connectingPathMap.begin(); pathIt != m_connectingPathMap.end(); pathIt ++){
		std::list<Point_2>& shortestPath = pathIt->second;
		std::list<Point_2>::iterator vit = shortestPath.begin();
		if(vit != shortestPath.end()){
			ICPoint_2 p = K_ICK_converter(*vit);
			vit++;
			for(; vit != shortestPath.end(); vit++){
				ICPoint_2 p2 = K_ICK_converter(*vit);
				scene.addLine(p.x(), p.y(), p2.x(), p2.y(), QPen(Qt::magenta, 0.025, Qt::SolidLine, Qt::RoundCap, Qt::RoundJoin));
				p = p2;
			}
		}
	}

	// std::cout << shortestPath.size() << std::endl;

}

void Roadmap::getIntersectingEdges(Polygon_2 & poly, Graph& boundingCycle, bool outerBoundary){
	// Detecting all edges of the lattice graph that intersects the boundary of poly

	// =====================================================================================
	// Get a vertex of the poly and find the hexgaon of the lattice that contains the vetex.
	// For each col and row combo, there are three possible hexgaons the point may fall into

	// Locate the hexgon (relative to some arbitrary base choice)
	ICPoint_2 p0 = K_ICK_converter(poly.vertex(0));
	std::pair<int, int> crp = locateHexagonForPoint(p0);
	int col = crp.first;
	int row = crp.second;

	// Compute the current hexagon
	Point_2 p[6];
	Segment_2 e[6];
	Polygon_2 hex;
	int pIndex[6];
	populateHexagon(col, row, p, e, hex, pIndex);

	// =====================================================================================
	// With a point of the obstacle and a hexgon containing the point, we iteratively check
	// intersections between the segments of the obstacle and the hexagon. We assume that
	// the obstacle would have at least three vertices. Note that a obstacle may be smaller than
	// a single hexagon.

	std::set<std::pair<int, int> > tempEdgeToRemoveSet;
	int numVertices = poly.size();		//
	int currentVertexIndex = 0;
	while(currentVertexIndex < numVertices){
		// Get the current segment of polygon to be checked
		Point_2 nextVertex = poly[currentVertexIndex==numVertices-1?0:currentVertexIndex+1];

		// Check whether nextVertex is outside of the current hexagon. If we are still in the
		// same hexagon, then move to the next obstacle vertex
		if(hex.bounded_side(nextVertex) == CGAL::ON_BOUNDED_SIDE) {
			currentVertexIndex ++;
			continue;
		}

		// If we are here, then we jumped outside of a lattice hexagon. Figure out which edge is
		// being intersected. We keep doing this until we cover the entire obstacle edge
		Point_2 currentVertex = poly[currentVertexIndex];
		Segment_2 obsEdge(currentVertex, nextVertex);

		std::pair<int, int> lastPair(-10, -10);
		while(true){
			int edgeIndex = 0;
			while(edgeIndex < 6){
				if(intersection(e[edgeIndex], obsEdge)){
					std::pair<int, int> edgePair(pIndex[edgeIndex], pIndex[edgeIndex==5?0:edgeIndex+1]);
					std::pair<int, int> edgePairReverse(pIndex[edgeIndex==5?0:edgeIndex+1], pIndex[edgeIndex]);
					if(lastPair != edgePair && lastPair != edgePairReverse){
						lastPair = edgePair;
						break;
					}
				}
				edgeIndex ++;
			};

			if(edgeIndex < 6){
				// We hit an intersection, mark the edge as to be removed
				tempEdgeToRemoveSet.insert(lastPair);
				m_edgeToBeRemovedSet.insert(lastPair);

				// Figure out the next hexgon to be checked
				if(col%2 == 0){
					switch(edgeIndex){
						case 0: col--; break;
						case 1: row++; break;
						case 2: col++; break;
						case 3: col++; row--; break;
						case 4: row--; break;
						case 5: col--; row--; break;
						default: break;
					}
				}
				else{
					switch(edgeIndex){
						case 0: col--; row++; break;
						case 1: row++; break;
						case 2: col++; row++; break;
						case 3: col++; break;
						case 4: row--; break;
						case 5: col--; break;
						default: break;
					}
				}
				populateHexagon(col, row, p, e, hex, pIndex);
			}
			else{
				// No more intersections, we are dong with the current obstacle edge
				break;
			}
		}
		currentVertexIndex++;
	}

	// =====================================================================================
	// We now have all the edges of the lattice that lie on the polygonal obstacle boundary.
	// Next, we locate the bounding cycle in the full lattice graph surrounding the obstacle
	if(tempEdgeToRemoveSet.size() > 0){
		// Iterate through edges to be removed and find one with an end vertex that
		// has an associated edge in the configuraiton space
		int sIndex, t1Index, t2Index;
		for(std::set<std::pair<int,int> >::iterator eit = tempEdgeToRemoveSet.begin();
		eit != tempEdgeToRemoveSet.end(); eit++){
			std::pair<int, int> edge = *eit;
			// Try one vertex
			sIndex = edge.first;
			if((t2Index = pointBelongToEdgeOutideObstacle(poly, sIndex, tempEdgeToRemoveSet, outerBoundary))!= -1)
			{
				t1Index = edge.second;
				break;
			}

			// Try the other vertex
			sIndex = edge.second;
			if((t2Index = pointBelongToEdgeOutideObstacle(poly, sIndex, tempEdgeToRemoveSet, outerBoundary))!= -1)
			{
				t1Index = edge.first;
				break;
			}

		}

		// Now edge (sIndex, t1Index) crosses the boundary, and (sIndex, t2Index) it outside the boundary
		// We simply locate a hexagon containing these two edges to extend the cycle
		Point_2 p1 = m_vidPointMap[sIndex];
		Point_2 p2 = m_vidPointMap[t1Index];
		Point_2 p3 = m_vidPointMap[t2Index];
		ICPoint_2 midPoint = K_ICK_converter(Point_2((p2.x() + p3.x())/2, (p2.y() + p3.y())/2));
		crp = locateHexagonForPoint(midPoint);
		populateHexagon(crp.first, crp.second, p, e, hex, pIndex);

		int endIndex = sIndex;		// When we see this index again, we are done

		// Add valid edge to cycle
		addEdgeIfNotThere(sIndex, t2Index, boundingCycle);

		// With the first hexagon, we can keep going along the boundary
		while(true){
			// Get next candidate index
			int nextIndex = getNextNodeInSequence(sIndex, t2Index, pIndex);

			p1 = m_vidPointMap[sIndex];
			p2 = m_vidPointMap[nextIndex];
			p3 = m_vidPointMap[t2Index];

			// Check whether the edge (t2Index, nextIndex) crosses boundary
			if(edgeInSet(t2Index, nextIndex, tempEdgeToRemoveSet)){
				// In this case, we move to the next hexagon
				crp = getBorderHexagon(crp.first, crp.second, nextIndex, t2Index, pIndex);
				populateHexagon(crp.first, crp.second, p, e, hex, pIndex);
				sIndex = nextIndex;
				// t2Index = sIndex;
			}
			else{
				// Edge is valid, update indices
				sIndex = t2Index;
				t2Index = nextIndex;

				// Add valid edge to cycle
				addEdgeIfNotThere(sIndex, t2Index, boundingCycle);
			}

			if(endIndex == t2Index) break;
		}
	}

	// =====================================================================================
	// Process bounding cycle to remove single degree vertices
	std::set<int> vSet = boundingCycle.getVertexSet();
	std::set<int> vSingleSet;
	// Collect single degree vertices
	for(std::set<int>::iterator vit = vSet.begin(); vit != vSet.end(); vit++){
		if(boundingCycle.getNeighborSet(*vit).size() == 1){
			vSingleSet.insert(*vit);
			// Need to check the neighbor as well
			int prevNbr = *vit;
			int nbr = *(boundingCycle.getNeighborSet(*vit).begin());
			while(boundingCycle.getNeighborSet(nbr).size()==2){
				vSingleSet.insert(nbr);
				std::set<int> nbrSet = boundingCycle.getNeighborSet(nbr);
				nbrSet.erase(prevNbr);

				prevNbr = nbr;
				nbr = *(nbrSet.begin());
			}
		}
	}
	// Delete them
	for(std::set<int>::iterator vit = vSingleSet.begin(); vit != vSingleSet.end(); vit++){
		boundingCycle.removeVertex(*vit);

		// It seems good to remove these single degree vertices
		if(!outerBoundary)m_vertexToBeRemovedSet.insert(*vit);
	}
}

std::pair<int, int> Roadmap::getBorderHexagon(int col, int row, int i1, int i2, int pIndex[]){
	// First locate the index of i1
	int i1Index = 0;
	for(int i = 0; i < 6; i ++){
		if(pIndex[i] == i1){
			i1Index = i;
			break;
		}
	}

	// Figure out the edge index
	int edgeIndex = 0;
	int nextIndex = (i1Index == 5? 0 : i1Index + 1);
	if(pIndex[nextIndex] == i2){
		// Going clockwise
		edgeIndex = i1Index;
	}
	else{
		// Going counterclockwise
		edgeIndex = (i1Index + 6 - 1)%6;
	}

	// Locate the next hexagon
	if(col%2 == 0){
		switch(edgeIndex){
			case 0: col--; break;
			case 1: row++; break;
			case 2: col++; break;
			case 3: col++; row--; break;
			case 4: row--; break;
			case 5: col--; row--; break;
			default: break;
		}
	}
	else{
		switch(edgeIndex){
			case 0: col--; row++; break;
			case 1: row++; break;
			case 2: col++; row++; break;
			case 3: col++; break;
			case 4: row--; break;
			case 5: col--; break;
			default: break;
		}
	}
	return std::pair<int, int>(col, row);
}

bool Roadmap::edgeInSet(int v1, int v2, std::set<std::pair<int, int> >& edgeSet){
	std::pair<int, int> edge(v1, v2);
	std::pair<int, int> rEdge(v2, v1);
	return (edgeSet.find(edge) != edgeSet.end() || edgeSet.find(rEdge) != edgeSet.end());
}

void Roadmap::addEdgeIfNotThere(int v1, int v2, Graph & graph){
	if(!graph.hasEdge(v1, v2)){
		graph.addEdge(v1, v2);
	}
}

int Roadmap::getNextNodeInSequence(int i1, int i2, int pIndex[]){
	// First locate the index of i1
	int i1Index = 0;
	for(int i = 0; i < 6; i ++){
		if(pIndex[i] == i1){
			i1Index = i;
			break;
		}
	}
	int nextIndex = (i1Index == 5? 0 : i1Index + 1);
	if(pIndex[nextIndex] == i2){
		// Going clockwise
		return pIndex[(i1Index + 2)%6];
	}
	else{
		// Going counterclockwise
		return pIndex[(6 + i1Index - 2)%6];
	}
}

std::pair<int, int> Roadmap::locateHexagonForPoint(ICPoint_2 &p0){
	// Compute the rectangular (col, row)
	int col = (int)(floor((p0.x() - xs)/(m_edgeLength*1.5)));
	int row = (int)(floor((p0.y() - ys)/(m_edgeLength*sqrt3)));

	// Shift everything to the "origin"
	double dx = p0.x() - (xs + m_edgeLength*1.5*col);
	double dy = p0.y() - (ys + m_edgeLength*sqrt3*row);

	// For each col/row combo, which correspond to a rectangular region, there can be
	// three hexagons corrsponding to a point in that rectangle
	if(col%2 == 0){
		if(dx <= 0.5*m_edgeLength && dy <= sqrt3*m_edgeLength  - sqrt3*dx && dy >= sqrt3*dx){
			// We need to move left
			col --;
			// row ++;
		}
		else if(dy >= m_edgeLength*0.5*sqrt3){
			// Off by one row
			row ++;
		}
		else{
			// Already in the right place, do nothing
		}
	}
	else{
		if(dy >= sqrt3*0.5*m_edgeLength + sqrt3*dx){
			col--;
			row++;
		}
		else if(dy <= sqrt3*0.5*m_edgeLength - sqrt3*dx){
			col--;
		}
		else{
			// Already in the right place, do nothing
		}
	}
	return std::pair<int, int>(col, row);
}

int Roadmap::pointBelongToEdgeOutideObstacle(Polygon_2 & poly, int pIndex,
	std::set<std::pair<int,int> > &tempEdgeToRemoveSet, bool outerBoundary){
	// Grab the point and check whether it is in the c-space
	if(pointOutsideObstacle(poly, pIndex, outerBoundary)){
		// The point is not "in" the obstacle, get edges and check that they are not
		// intersecting with the boundary. Because pIndex is outside obstacle, as long
		// as one edge from pIndex is not intersecting boundary, the edge must be outside
		// the boundary
		std::set<int>& nbrSet = m_graph.getNeighborSet(pIndex);
		for(std::set<int>::iterator nit = nbrSet.begin(); nit != nbrSet.end(); nit++){
			int et = *nit;
			if(!edgeInSet(pIndex, et, tempEdgeToRemoveSet)){
				return et;
			}
		}
	}
	return -1;
}

bool Roadmap::pointOutsideObstacle(Polygon_2 & poly, int pIndex, bool outerBoundary){
	Point_2 p = m_vidPointMap[pIndex];
	int boundedStatus = poly.bounded_side(p);
	if((boundedStatus == CGAL::ON_BOUNDED_SIDE && outerBoundary) ||
		(boundedStatus == CGAL::ON_UNBOUNDED_SIDE && !outerBoundary)){
		return true;
	}
	return false;
}

void Roadmap::populateHexagon(int col, int row, Point_2* p, Segment_2* e, Polygon_2 &hex, int* pIndex){
	hex.clear();
	if(col%2 == 0){
		pIndex[0] = (col + row*n_w)*2;
		pIndex[1] = (col + row*n_w)*2 + 1;
		pIndex[2] = (col + 1 + row*n_w)*2 + 1;
		pIndex[3] = (col + 1 + row*n_w)*2;
		pIndex[4] = (col + 1 + (row-1)*n_w)*2 + 1;
		pIndex[5] = (col + (row-1)*n_w)*2 + 1;
	}
	else{
		pIndex[0] = (col + row*n_w)*2 + 1;
		pIndex[1] = (col + (row+1)*n_w)*2;
		pIndex[2] = (col + 1 + (row+1)*n_w)*2;
		pIndex[3] = (col + 1 + row*n_w)*2 + 1;
		pIndex[4] = (col + 1 + row*n_w)*2;
		pIndex[5] = (col + row*n_w)*2;
	}
	// Then we get all vertices
	for(int i = 0; i < 6; i ++){
		p[i] = m_vidPointMap[pIndex[i]];
	}

	// Then the edges and the hexgaon
	for(int i = 0; i < 6; i ++){
		e[i] = Segment_2(p[i], p[i == 5? 0 : i+1]);
		hex.push_back(p[i]);
	}
}

void Roadmap::buildHexgaonLattice(){

	// Start the lattice at bottomLeftX, bottomLeftY.
	// Assume that there are n_w and n_h hexgons inside the rectangle, then we should have
	// (1/2)*sideLength + n_w*sideLength*3/2 <= width and width < (1/2)*sideLength + (n_w + 1)*sideLength*3/2
	// (sqrt(3)/2)*sideLength + n_h*(sqrt(3)/2)*sideLength <= height and height < (sqrt(3)/2)*sideLength + (n_h + 1)*(sqrt(3)/2)*sideLength
	// From these we can compute n_w and n_h. We add a few to make sure that we cover everything.

	// Compute lattice nodes
	for(int i = 0; i < n_w; i ++){
		for(int j = 0; j < n_h; j ++){
			// Build one vertical "wave" of vertices
			Point_2 v0(xs + i*m_edgeLength*1.5 + (i%2 == 0? 0 : m_edgeLength*0.5),
				ys + j*m_edgeLength*sqrt3);
			Point_2 v1(xs + m_edgeLength/2 + i*m_edgeLength*1.5 + (i%2 == 0? 0 : - m_edgeLength*0.5),
				ys + sqrt3*m_edgeLength/2 + j*m_edgeLength*sqrt3);

			m_vidPointMap[(i + j*n_w)*2] = v0;
			m_pointVidMap[v0] = (i + j*n_w)*2;
			m_vidPointMap[(i + j*n_w)*2 + 1] = v1;
			m_pointVidMap[v1] = (i + j*n_w)*2 + 1;

			m_pointList.push_back(v0);
			m_pointList.push_back(v1);
		}
	}

	// Build adjacency, for each vertex, check whether its three neighbors are present
	for(int id = 0; id < n_w*n_h*2; id ++){
		// Compute w, h
		int w = (id/2)%n_w;
		int h = (id/2)/n_w;
		bool odd = (id%2==1);

		// If odd is true, check the vertex above, which has index (w, h + 1, 0)
		if(odd){
			if(h + 1 < n_h){
				m_graph.addEdge(id, (w + (h + 1)*n_w)*2);
			}
		}
		else{
			m_graph.addEdge(id, (w + (h)*n_w)*2 + 1);
		}
		// If odd is true and w is even, check (w + 1, h, 1)
		if(odd && w%2 == 0){
			if(w + 1 < n_w){
				m_graph.addEdge(id, (w + 1 + h*n_w)*2 + 1);
			}
		}

		// If odd is false and w is odd, check (w + 1, h, 0)
		if(odd == false && w%2 == 1){
			if(w + 1 < n_w){
				m_graph.addEdge(id, (w + 1 + h*n_w)*2);
			}
		}
	}
}

void Roadmap::drawHexagonLattice(QGraphicsScene& scene, bool drawVetex){
	QPen edgePen = QPen(Qt::gray, 0.25, Qt::DashLine, Qt::RoundCap, Qt::RoundJoin);
	QPen vertexPen = QPen(Qt::blue, 0.4, Qt::SolidLine, Qt::RoundCap, Qt::RoundJoin);
	std::set<std::pair<int, int> > edgeSet = m_graph.getEdgeSet();
	std::set<int> vSet;
	for(std::set<std::pair<int, int> >::iterator eit = edgeSet.begin(); eit != edgeSet.end(); eit++){
		int v1 = (*eit).first;
		int v2 = (*eit).second;
		vSet.insert(v1); vSet.insert(v2);
		ICPoint_2 p1 = K_ICK_converter(m_vidPointMap[v1]);
		ICPoint_2 p2 = K_ICK_converter(m_vidPointMap[v2]);
		scene.addLine(p1.x(), p1.y(), p2.x(), p2.y(), edgePen);
	}

	// Paint vertices if needed and obtain the fill area
	if(drawVetex){
		for(std::set<int>::iterator vit = vSet.begin(); vit != vSet.end(); vit ++){
			ICPoint_2 p = K_ICK_converter(m_vidPointMap[*vit]);
			scene.addEllipse(p.x() - 0.25, p.y() - 0.25, 0.5, 0.5, vertexPen);
		}
	}
}

void Roadmap::snapToGraph(vector<pair<double, double> >&coords, vector<int>&snapped){
	set<int> usedVertices;
	// For each member in coords, locate the hexagon it belongs and then locate the closest
	// graph vertex to the said coordinate.
	for(int r = 0; r < coords.size(); r ++){
		double x = coords[r].first;
		double y = coords[r].second;
		Point_2 p0 = Point_2(x, y);

		ICPoint_2 p0x = K_ICK_converter(p0);
		std::pair<int, int> crp = locateHexagonForPoint(p0x);
		int col = crp.first;
		int row = crp.second;

		// Compute the current hexagon
		Point_2 p[6];
		Segment_2 e[6];
		Polygon_2 hex;
		int pIndex[6];
		populateHexagon(col, row, p, e, hex, pIndex);

		// Find the closest vertex that is not occupied and inside the configuration space
		int bestV = -1;
		double minDist = -1;
		for(int vi = 0; vi < 6; vi ++){
			// Only work with vertices inside the configuration space and not used
			if(m_graph.hasVertex(pIndex[vi])
			   && usedVertices.find(pIndex[vi]) == usedVertices.end()){
				if(minDist < 0){
					minDist = getDistance(p0, p[vi]);
					bestV = pIndex[vi];
				}
				else{
					double dist = getDistance(p0, p[vi]);
					if(minDist > dist){
						minDist = dist;
						bestV = pIndex[vi];
					}
				}
			}
		}

		// Snap!
		if(bestV != -1){
#ifdef _DEBUG
			cout << "Snapping point (" << x << ", " << y << ") to vertex " << m_vidGFMap[bestV] << endl;
#endif
			snapped.push_back(m_vidGFMap[bestV]);
			usedVertices.insert(bestV);
		}
		else{
			throw "ERROR: Cannot find suitable vertex for snapping";
		}
	}

}

void Roadmap::createRandomStartGoalPairs(int numRobots, double spacing,  vector<pair<double, double> >& starts,
	vector<pair<double, double> >& goals){

	createRandomCoords(numRobots, spacing, starts);
	createRandomCoords(numRobots, spacing, goals);
}

void Roadmap::createRandomCoords(int numRobots, double spacing, vector<pair<double, double> >& coords){
	// Uniform sample from the free space and pick 5 start/goal pairs
	CGAL::Bbox_2 bbox = m_pBoundingRect->bbox();
	while(true){
		long trialCount = 0;
		for(int i = 0; i < numRobots; i ++){
			while(trialCount < numRobots*30000){
				trialCount ++;
#ifdef _DEBUG
				if(trialCount %1000 == 0) {cout << trialCount << endl;}
#endif
				double x = (rand()%10000)/10000.*(bbox.xmax() - bbox.xmin())  + bbox.xmin();
				double y = (rand()%10000)/10000.*(bbox.ymax() - bbox.ymin())  + bbox.ymin();
#ifdef _DEBUG
				cout << "Randomly created point with x=" << x <<", y=" << y;
#endif
				Point_2 p(x, y);
				// Check that the point is in free configuration space
				if(isPointInCSpace(p)){
					bool good = true;
					// Check that the point has good distance from existing points
					for(int vi = 0; vi < coords.size(); vi ++){
						if(getDistance(p, coords[vi].first, coords[vi].second) < (spacing > 2 ? spacing : 2)*m_radius){
							good = false;
						}
					}

					if(good){
						// No problem! We have a good set
						coords.push_back(pair<double, double>(x, y));
#ifdef _DEBUG
						cout << " - added to set for robot " << i << endl;
#endif
						break;
					}
					else{
#ifdef _DEBUG
						cout << endl;
#endif
					}

				}
				else{
#ifdef _DEBUG
					cout << endl;
#endif
				}
			}
			if(trialCount >= numRobots*30000){
				// cout << coords.size() << endl;
				coords.clear();
				break;
			}
		}
		if(coords.size() == numRobots)return;
	}
}

bool Roadmap::solveProblem(vector<pair<int, int> >& sgVec, map<int, vector<int> >& paths,
	string& fileFolder, string& fileNameExtra){
	// Solve the problem by calling external java solver

	// =====================================================================================
	// First write the problem
	string pfString = fileFolder + "\\p" + fileNameExtra + ".txt";
	string sfString = fileFolder + "\\s" + fileNameExtra + ".txt";
	ofstream ps(pfString.c_str(), ios::out);

	// Write the the number of robots
	ps << m_finalGraph.getVertexSet().size() << endl;

	// Write out all the edges
	set<pair<int, int> >& edgeSet = m_finalGraph.getEdgeSet();
	for(set<pair<int, int> >::iterator ei = edgeSet.begin(); ei != edgeSet.end(); ei ++){
		ps << ei->first << ":" << ei->second << " ";
	}
	ps << endl;

	// Write out the starts and then the goals
	for(int r = 0; r < sgVec.size(); r ++){
		ps << sgVec[r].first << " ";
	}
	ps << endl;
	for(int r = 0; r < sgVec.size(); r ++){
		ps << sgVec[r].second << " ";
	}
	ps << endl;
	ps.close();

	// =====================================================================================
	// Make system call
	string callStr = "java -cp gurobi.jar;mp.jar projects.multipath.general.algorithms.Main";
	if(fileNameExtra.length() > 0) {
		callStr.append(" ").append(pfString).append(" ").append(sfString);
	}
	int i = system(callStr.c_str());

	// =====================================================================================
	// Read in the solution, there should be sgVec.size() robots
	ifstream ss(sfString.c_str(), ios::in);
	if(ss.good()){
		for(int r = 0; r < sgVec.size(); r++){
			string line;
			getline(ss, line);
			QString s(line.c_str());
			QStringList qsl = s.split(" ");
			paths[r] = vector<int>();
			for(int t = 0; t < qsl.size(); t ++){
				paths[r].push_back(qsl[t].toInt());
			}
			paths[r].pop_back();
		}
		ss.close();
	}
	else{
		ss.close();
		return false;
	}

	// Solve the problem locally
	// ILPSolver solver(&m_finalGraph);
	// solver.solve(sgVec, paths, -1);

	improvePaths(paths);
	return true;
}

void Roadmap::improvePaths(map<int, vector<int> >& paths){
	// Remove obvious ossilation from the paths
	for(int t = 1; t < paths[0].size() - 1; t ++){
		for(int r = 0; r < paths.size(); r ++){
			// Do we have single step ossilation?
			if(paths[r][t - 1] == paths[r][t + 1] && paths[r][t - 1] != paths[r][t]){
				// Check whether any other robots goes to paths[r][t - 1] at t
				bool conflict = false;
				for(int ori = 0; ori < paths.size(); ori ++){
					if(ori == r) continue;
					if(paths[ori][t] == paths[r][t - 1]){
						conflict = true;
					}
				}

				// If not, let the robot stay
				if(!conflict){
					paths[r][t] = paths[r][t - 1];
				}
			}
			// Do a two step look ahead as well
			else if((t < paths[0].size() - 2) && paths[r][t - 1] == paths[r][t + 2] &&
				paths[r][t] == paths[r][t + 1] && paths[r][t - 1] != paths[r][t]){
				// Check whether any other robots goes to paths[r][t - 1] at t
				bool conflict = false;
				for(int ori = 0; ori < paths.size(); ori ++){
					if(ori == r) continue;
					if(paths[ori][t] == paths[r][t - 1] || paths[ori][t + 1] == paths[r][t - 1]){
						conflict = true;
					}
				}

				// If not, let the robot stay
				if(!conflict){
					paths[r][t+1] = paths[r][t] = paths[r][t - 1];
				}
			}
		}
	}
}

pair<double, double> Roadmap::getVertexLocationFromID(int vid){
	// Get vid in the original graph
	vid = m_vidFGMap[vid];

	// Locate the vid
	Point_2& p = m_vidPointMap[vid];
	ICPoint_2 pp = K_ICK_converter(p);

	return pair<double, double>(pp.x(), pp.y());
}


///////////////////////////////////////////////////////////////////////////////
// kernel::estimation::include.hpp                                           //
//                                                                           //
//  Copyright 2009 Erwann Rogard. Distributed under the Boost                //
//  Software License, Version 1.0. (See accompanying file                    //
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)         //
///////////////////////////////////////////////////////////////////////////////
#ifndef BOOST_STATISTICS_DETAIL_KERNEL_ESTIMATION_INCLUDE_H_ER_2009
#define BOOST_STATISTICS_DETAIL_KERNEL_ESTIMATION_INCLUDE_H_ER_2009

#include <boost/statistics/detail/kernel/estimation/estimator.hpp>
#include <boost/statistics/detail/kernel/estimation/nw_visitor.hpp>
#include <boost/statistics/detail/kernel/estimation/meta_nw_visitor_unary.hpp>
#include <boost/statistics/detail/kernel/estimation/rp_visitor.hpp>

#endif


/*
 Copyright (c) 2017 Paul Lagrée

 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 __oim__RandomEvaluator__
#define __oim__RandomEvaluator__

#include "common.hpp"
#include "Evaluator.hpp"

#include <sys/time.h>
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/uniform_int_distribution.hpp>

class RandomEvaluator : public Evaluator {
 private:
  std::mt19937 gen_;
  std::uniform_int_distribution<unode_int> dst_;

 public:
  RandomEvaluator() : gen_(seed_ns()) {};

  std::unordered_set<unode_int> select(
      const Graph& graph, Sampler&,
      const std::unordered_set<unode_int>&, unsigned int k) {
    dst_ = std::uniform_int_distribution<unode_int>(
        0, graph.get_number_nodes() - 1);
    std::unordered_set<unode_int> seeds;
    while (seeds.size() < k) {
      unode_int seed = dst_(gen_);
      seeds.insert(seed);
    }
    return seeds;
  }
};

#endif /* defined(__oim__RandomEvaluator__) */


#include <boost/date_time/posix_time/posix_time.hpp>
#include <boost/date_time/gregorian/gregorian.hpp>
#include <iostream>

int main()
{
  boost::posix_time::ptime pt(boost::gregorian::date(2009, 1, 5), boost::posix_time::time_duration(12, 0, 0));
  boost::gregorian::date d = pt.date();
  std::cout << d << std::endl;
  boost::posix_time::time_duration td = pt.time_of_day();
  std::cout << td << std::endl;
}

Boost/The Boost C++ Libraries/src/10.3.1/main.cpp


/* boost random/detail/signed_unsigned_compare.hpp header file
 *
 * Copyright Jens Maurer 2000-2001
 * 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)
 *
 * See http://www.boost.org for most recent version including documentation.
 *
 * Revision history
 */

#ifndef BOOST_RANDOM_DETAIL_SIGNED_UNSIGNED_COMPARE
#define BOOST_RANDOM_DETAIL_SIGNED_UNSIGNED_COMPARE

#include <boost/limits.hpp>

namespace boost {
namespace random {

/*
 * Correctly compare two numbers whose types possibly differ in signedness.
 * See boost::numeric_cast<> for the general idea.
 * Most "if" statements involve only compile-time constants, so the
 * optimizing compiler can do its job easily.
 *
 * With most compilers, the straightforward implementation produces a
 * bunch of (legitimate) warnings.  Some template magic helps, though.
 */

namespace detail {
template<bool signed1, bool signed2>
struct do_compare
{ };

template<>
struct do_compare<false, false>
{
  // cast to the larger type is automatic with built-in types
  template<class T1, class T2>
  static bool equal(T1 x, T2 y) { return x == y; }
  template<class T1, class T2>
  static bool lessthan(T1 x, T2 y) { return x < y; }
};

template<>
struct do_compare<true, true> : do_compare<false, false>
{ };

template<>
struct do_compare<true, false>
{
  template<class T1, class T2>
  static bool equal(T1 x, T2 y) { return x >= 0 && static_cast<T2>(x) == y; }
  template<class T1, class T2>
  static bool lessthan(T1 x, T2 y) { return x < 0 || static_cast<T2>(x) < y; }
};

template<>
struct do_compare<false, true>
{
  template<class T1, class T2>
  static bool equal(T1 x, T2 y) { return y >= 0 && x == static_cast<T1>(y); }
  template<class T1, class T2>
  static bool lessthan(T1 x, T2 y) { return y >= 0 && x < static_cast<T1>(y); }
};

} // namespace detail

template<class T1, class T2>
int equal_signed_unsigned(T1 x, T2 y)
{
  typedef std::numeric_limits<T1> x_traits;
  typedef std::numeric_limits<T2> y_traits;
  return detail::do_compare<x_traits::is_signed, y_traits::is_signed>::equal(x, y);
}

template<class T1, class T2>
int lessthan_signed_unsigned(T1 x, T2 y)
{
  typedef std::numeric_limits<T1> x_traits;
  typedef std::numeric_limits<T2> y_traits;
  return detail::do_compare<x_traits::is_signed, y_traits::is_signed>::lessthan(x, y);
}

} // namespace random
} // namespace boost

#endif // BOOST_RANDOM_DETAIL_SIGNED_UNSIGNED_COMPARE


#include "sv/util/dataset.h"

#include <absl/strings/match.h>
#include <absl/strings/str_join.h>
#include <absl/strings/str_replace.h>
#include <fmt/format.h>
#include <fmt/ranges.h>
#include <glog/logging.h>

#include <Eigen/Geometry>
#include <filesystem>
#include <fstream>
#include <istream>
#include <opencv2/imgcodecs.hpp>  // imread
#include <opencv2/imgproc.hpp>    // threashold

#ifndef XTENSOR_FOUND
#define XTENSOR_FOUND 0
#endif

// Conditionallly include xtensor if package found
#if XTENSOR_FOUND
#include <xtensor/xarray.hpp>
#include <xtensor/xnpy.hpp>
#endif

namespace sv {

namespace fs = std::filesystem;

using SO3d = Sophus::SO3d;
using SE3d = Sophus::SE3d;
using Matrix3d = Eigen::Matrix3d;
using Vector3d = Eigen::Vector3d;
using Quaterniond = Eigen::Quaterniond;
using RowMat44d = Eigen::Matrix<double, 4, 4, Eigen::RowMajor>;

namespace {

constexpr int kPoseDim = 7;
constexpr int kIntrinDim = 4;
constexpr int kTransformDim = 16;

template <typename T>
void VectorExtend(std::vector<T>& v1, const std::vector<T>& v2) {
  v1.reserve(v1.size() + v2.size());
  v1.insert(v1.end(), v2.begin(), v2.end());
}

void SortStrsAsInts(std::vector<std::string>& strings) {
  std::sort(strings.begin(), strings.end(), [](const auto& s1, const auto& s2) {
    if (s1.size() != s2.size()) return (s1.size() < s2.size());
    return s1 < s2;
  });
}

/// @brief Read stereo data
bool ReadStereoData(const fs::path& left_path,
                    const fs::path& right_path,
                    const std::string& ext,
                    std::vector<std::string>& files) {
  // Read left images
  const auto left_files = GetFiles(left_path, ext, true);

  if (left_files.empty()) {
    LOG(WARNING) << "No files found in right_path: " << left_path;
  } else {
    VectorExtend(files, left_files);
  }

  // Read right images
  const auto right_files = GetFiles(right_path, ext, true);

  if (right_files.empty()) {
    LOG(WARNING) << "No files found in right_path: " << right_path;
  } else {
    // Otherwise must have the same size as left files
    CHECK_EQ(left_files.size(), right_files.size());
    VectorExtend(files, right_files);
  }

  return left_files.size() == right_files.size();
}

cv::Mat MakeStereoIntrins(const cv::Mat& intrin) {
  CHECK_EQ(intrin.cols, 5);
  CHECK_EQ(intrin.type(), CV_64FC1);
  cv::Mat intrins = cv::Mat::zeros(2, 5, CV_64FC1);
  intrin.copyTo(intrins.row(0));
  intrin.copyTo(intrins.row(1));
  intrins.at<double>(1, 4) *= -1;
  return intrins;
}

}  // namespace

void MatToSE3d(const cv::Mat& mat, SE3d& tf) {
  CHECK(mat.isContinuous());
  CHECK_EQ(mat.type(), CV_64FC1);
  CHECK_EQ(mat.total(), kPoseDim);

  tf.translation() = Eigen::Map<const Vector3d>(mat.ptr<double>(0));
  // 0  1  2  3  4  5  6
  // tx ty tz qx qy qz qw
  const Quaterniond q(mat.at<double>(6),
                      mat.at<double>(3),
                      mat.at<double>(4),
                      mat.at<double>(5));
  tf.setQuaternion(q.normalized());
}

SE3d SE3dFromMat(const cv::Mat& mat) {
  if (mat.empty()) return {};

  CHECK(mat.isContinuous());
  CHECK_EQ(mat.type(), CV_64FC1);
  CHECK_EQ(mat.total(), kTransformDim);

  Eigen::Map<const RowMat44d> tf(mat.ptr<double>(0));
  return {tf.topLeftCorner<3, 3>(), tf.topRightCorner<3, 1>()};
}

std::vector<std::string> GetFiles(const std::string& dir,
                                  std::string_view ext,
                                  bool sort) {
  std::vector<fs::path> paths;
  std::vector<std::string> files;

  const fs::path pdir{dir};

  if (!fs::exists(pdir)) {
    LOG(WARNING) << fmt::format("[{}] does not exist", pdir.string());
    return files;
  }

  if (!fs::is_directory(pdir)) {
    LOG(WARNING) << fmt::format("[{}] is not a directory", pdir.string());
    return files;
  }

  for (const auto& entry : fs::directory_iterator(pdir)) {
    if (!fs::is_regular_file(entry.path())) continue;
    if (!ext.empty() && entry.path().extension() != ext) continue;
    paths.push_back(entry.path());
  }

  // sort by filename only
  if (sort) {
    std::sort(
        paths.begin(), paths.end(), [](const fs::path& p1, const fs::path& p2) {
          return p1.filename() < p2.filename();
        });
  }

  // convert to string
  files.reserve(paths.size());
  std::transform(paths.cbegin(),
                 paths.cend(),
                 std::back_inserter(files),
                 [](const fs::path& p) { return p.string(); });

  return files;
}

cv::Mat CvReadImage(const std::string& file) {
  auto image = cv::imread(file, cv::IMREAD_UNCHANGED);
  if (image.empty()) {
    LOG(WARNING) << "Failed to read color image: " << file;
  }
  return image;
}

cv::Mat CvReadDepth(const std::string& file, double div_factor) {
  auto depth = cv::imread(file, cv::IMREAD_ANYDEPTH);

  if (depth.empty()) {
    LOG(WARNING) << "Failed to read depth image: " << file;
    return depth;
  }

  if (div_factor != 1.0) {
    CHECK_GT(div_factor, 0);
    depth.convertTo(depth, CV_32FC1, 1.0 / div_factor);
  } else {
    CHECK_EQ(depth.type(), CV_32FC1);
  }

  return depth;
}

cv::Mat ThresholdDepth(const cv::Mat& depth,
                       double min_depth,
                       double max_depth) {
  // If there's nothing to threshold, just return
  if (min_depth < 0 && max_depth < 0) return depth;

  cv::Mat out = depth.clone();

  if (0 <= min_depth) {
    cv::threshold(out, out, min_depth, 0, cv::THRESH_TOZERO);
  }

  if (0 < max_depth && std::isfinite(max_depth)) {
    cv::threshold(out, out, max_depth, 0, cv::THRESH_TOZERO_INV);
  }

  return out;
}

/// ============================================================================
DatasetBase::DatasetBase(const std::string& name,
                         const std::string& data_dir,
                         const std::vector<std::string>& dtypes)
    : name_{name}, data_dir_{data_dir}, dtypes_{dtypes} {
  CHECK(fs::exists(data_dir_)) << data_dir_;
}

cv::Mat DatasetBase::Get(std::string_view dtype, int i, int cam) const {
  CHECK_GE(i, 0);
  CHECK_GE(cam, 0);
  return GetImpl(dtype, i, cam);
}

std::string DatasetBase::Repr() const {
  return fmt::format("{}(dir={}, size={}, dtypes=[{}])",
                     name_,
                     data_dir_,
                     size_,
                     absl::StrJoin(dtypes_, ", "));
}

bool DatasetBase::is_stereo() const {
  return static_cast<int>(files_.at(DataType::kImage).size()) == (2 * size());
}

/// ============================================================================
IclNuim::IclNuim(const std::string& data_dir)
    : DatasetBase{"icl_nuim", data_dir, kDtypes} {
  // Check directories exist
  const fs::path data_dir_path{data_dir_};

  // Read image files
  files_[DataType::kImage] = GetFiles(data_dir_path / "rgb", ".png", false);
  SortStrsAsInts(files_[DataType::kImage]);
  size_ = static_cast<int>(files_[DataType::kImage].size());

  // Read depth files
  files_[DataType::kDepth] = GetFiles(data_dir_path / "depth", ".png", false);
  SortStrsAsInts(files_[DataType::kDepth]);

  // Read intrinsics
  data_[DataType::kIntrin] = cv::Mat{481.20, -480.00, 319.50, 239.50, 0.0};

  // Read poses, convert to homogeneous transform
  const auto poses = ReadPoses(data_dir_path / "poses.txt");
  data_[DataType::kPose] = ConvertPoses(poses);
}

cv::Mat IclNuim::GetImpl(std::string_view dtype, int i, int /*cam*/) const {
  const auto ind = static_cast<size_t>(i);
  if (dtype == DataType::kImage) {
    const auto& files = files_.at(DataType::kImage);
    if (ind >= files.size()) return {};
    return CvReadImage(files.at(i));
  }

  if (dtype == DataType::kDepth) {
    const auto& files = files_.at(DataType::kDepth);
    if (ind >= files.size()) return {};
    return CvReadDepth(files.at(i), kDepthDivFactor);
  }

  if (dtype == DataType::kIntrin) {
    return data_.at(DataType::kIntrin).clone();
  }

  if (dtype == DataType::kPose) {
    return data_.at(DataType::kPose).row(i).clone();
  }

  return {};
}

cv::Mat IclNuim::ReadPoses(const std::string& file) const {
  std::ifstream ifs{file};
  CHECK(ifs.good()) << "Failed to open file: " << file;
  std::string header;
  std::getline(ifs, header);  // skip the first line

  int i{};
  std::vector<double> data;
  data.reserve(static_cast<size_t>(size_ * kPoseDim));

  // timestamp tx ty tz qx qy qz qw
  while (ifs >> i) {
    std::copy_n(
        std::istream_iterator<double>(ifs), kPoseDim, std::back_inserter(data));
  }

  CHECK_EQ(data.size() % kPoseDim, 0);
  return cv::Mat(data, true)
      .reshape(0, static_cast<int>(data.size()) / kPoseDim);
}

cv::Mat IclNuim::ConvertPoses(const cv::Mat& poses) const {
  CHECK_EQ(poses.cols, kPoseDim);

  SE3d tf;
  cv::Mat transforms;
  transforms.create(poses.rows, kTransformDim, CV_64FC1);

  for (int i = 0; i < poses.rows; ++i) {
    MatToSE3d(poses.row(i), tf);
    Eigen::Map<RowMat44d> tf_map(transforms.ptr<double>(i));
    tf_map = tf.matrix();
  }

  return transforms;
}

/// ============================================================================
Vkitti2::Vkitti2(const std::string& data_dir)
    : DatasetBase{"vkitti2", data_dir, kDtypes} {
  const fs::path data_path{data_dir_};
  const auto image_stereo = ReadStereoData(data_path / "frames/rgb/Camera_0",
                                           data_path / "frames/rgb/Camera_1",
                                           ".jpg",
                                           files_[DataType::kImage]);
  const auto depth_stereo = ReadStereoData(data_path / "frames/depth/Camera_0",
                                           data_path / "frames/depth/Camera_1",
                                           ".png",
                                           files_[DataType::kDepth]);
  const auto image_size = static_cast<int>(files_.at(DataType::kImage).size());
  const auto depth_size = static_cast<int>(files_.at(DataType::kDepth).size());
  size_ = image_stereo ? image_size / 2 : image_size;
  if (depth_size > 0) {
    CHECK_EQ(depth_size, depth_stereo ? size_ * 2 : size_);
  }

  // Read intrinsics
  data_[DataType::kIntrin] = ReadIntrinsics(data_path / "intrinsic.txt");
  CHECK_EQ(data_.at(DataType::kIntrin).rows, size_ * 2);

  // Read extrinsics
  const auto extrins = ReadExtrinsics(data_path / "extrinsic.txt");
  CHECK_EQ(extrins.rows, size_ * 2);
  data_[DataType::kPose] = ConvertExtrins(extrins);
}

Vkitti2 Vkitti2::Create(const std::string& base_dir,
                        int seq,
                        const std::string& var) {
  return Vkitti2{fmt::format("{}/Scene{:02d}/{}", base_dir, seq, var)};
}

cv::Mat Vkitti2::GetImpl(std::string_view dtype, int i, int cam) const {
  if (dtype == DataType::kImage) {
    const size_t ind = ToFileInd(i, cam);
    const auto& files = files_.at(DataType::kImage);
    if (ind >= files.size()) return {};
    return CvReadImage(files.at(ind));
  }

  if (dtype == DataType::kDepth) {
    const size_t ind = ToFileInd(i, cam);
    const auto& files = files_.at(DataType::kDepth);
    if (ind >= files.size()) return {};
    return CvReadDepth(files.at(ind), kDepthDivFactor);
  }

  if (dtype == DataType::kIntrin) {
    const auto ind = ToDataInd(i, cam);
    cv::Mat intrin = cv::Mat::zeros(1, 5, CV_64FC1);
    cv::Mat fxycxy = data_.at(DataType::kIntrin).row(ind);
    fxycxy.copyTo(intrin.colRange(0, 4));
    // Left camera has positive baseline while right has negative
    intrin.at<double>(4) = cam == 0 ? kBaseline : -kBaseline;
    return intrin;
  }

  if (dtype == DataType::kPose) {
    const int ind = ToDataInd(i, cam);
    return data_.at(DataType::kPose).row(ind).clone();
  }

  return {};
}

cv::Mat Vkitti2::ReadExtrinsics(const std::string& file) const {
  std::ifstream ifs{file};
  CHECK(ifs.good()) << "Failed to open file: " << file;
  std::string header;
  std::getline(ifs, header);  // skip the first line

  int i{};
  int cam{};
  std::vector<double> data;
  data.reserve(static_cast<size_t>(size_ * kTransformDim * 2));

  while (ifs >> i >> cam) {
    std::copy_n(std::istream_iterator<double>(ifs),
                kTransformDim,
                std::back_inserter(data));
  }

  CHECK_EQ(data.size() % kTransformDim, 0);
  return cv::Mat(data, true)
      .reshape(0, static_cast<int>(data.size()) / kTransformDim);
}

cv::Mat Vkitti2::ReadIntrinsics(const std::string& file) const {
  std::ifstream ifs{file};
  CHECK(ifs.good()) << "Unable to open file: " << file;
  std::string header;
  std::getline(ifs, header);  // skip the first line

  int i{};
  int cam{};
  std::vector<double> data;
  data.reserve(static_cast<size_t>(size_ * kIntrinDim * 2));

  while (ifs >> i >> cam) {
    std::copy_n(std::istream_iterator<double>(ifs),
                kIntrinDim,
                std::back_inserter(data));
  }

  CHECK_EQ(data.size() % kIntrinDim, 0);
  return cv::Mat(data, true)
      .reshape(0, static_cast<int>(data.size()) / kIntrinDim);
}

cv::Mat Vkitti2::ConvertExtrins(const cv::Mat& extrins) const {
  CHECK_EQ(extrins.cols, kTransformDim);

  cv::Mat transforms;
  transforms.create(extrins.rows, kTransformDim, CV_64FC1);

  for (int i = 0; i < extrins.rows; ++i) {
    Eigen::Map<const RowMat44d> extrin_map(extrins.ptr<double>(i));
    const SE3d extrin(SO3d::fitToSO3(extrin_map.topLeftCorner<3, 3>()),
                      extrin_map.topRightCorner<3, 1>());
    Eigen::Map<RowMat44d> tf_map(transforms.ptr<double>(i));
    tf_map = extrin.inverse().matrix();
  }

  return transforms;
}

/// ============================================================================
TartanAir::TartanAir(const std::string& data_dir)
    : DatasetBase{"tartan_air", data_dir, kDtypes} {
  const fs::path data_path{data_dir_};

  const auto image_stereo = ReadStereoData(data_path / "image_left",
                                           data_path / "image_right",
                                           ".png",
                                           files_[DataType::kImage]);

  // Allow depth to be optional
  const auto depth_stereo = ReadStereoData(data_path / "depth_left",
                                           data_path / "depth_right",
                                           ".npy",
                                           files_[DataType::kDepth]);

  const auto image_size = static_cast<int>(files_.at(DataType::kImage).size());
  const auto depth_size = static_cast<int>(files_.at(DataType::kDepth).size());
  size_ = image_stereo ? image_size / 2 : image_size;
  if (depth_size > 0) {
    CHECK_EQ(depth_size, depth_stereo ? size_ * 2 : size_);
  }

  // left poses on top, right poses bottom
  cv::Mat poses;
  cv::vconcat(ReadPoses(data_path / "pose_left.txt"),
              ReadPoses(data_path / "pose_right.txt"),
              poses);
  CHECK_EQ(poses.rows, size_ * 2);
  data_[DataType::kPose] = ConvertPoses(poses);

  // Intrins
  const cv::Mat intrin({1, 5}, {320.0, 320.0, 320.0, 240.0, 0.25});
  data_[DataType::kIntrin] = MakeStereoIntrins(intrin);
}

TartanAir TartanAir::Create(const std::string& base_dir,
                            const std::string& scene,
                            const std::string& mode,
                            int seq) {
  return TartanAir{fmt::format("{}/{}/{}/P{:03d}", base_dir, scene, mode, seq)};
}

cv::Mat TartanAir::ReadPoses(const std::string& file) const {
  constexpr int kPoseSize = 7;
  std::ifstream ifs{file};
  CHECK(ifs.good()) << "Failed to open file: " << file;

  // tx ty tz qx qy qz qw
  std::vector<double> data;
  data.reserve(static_cast<size_t>(size_ * kPoseSize));

  std::copy(std::istream_iterator<double>(ifs),
            std::istream_iterator<double>(),
            std::back_inserter(data));

  CHECK_EQ(data.size() % kPoseSize, 0);
  return cv::Mat(data, true)
      .reshape(0, static_cast<int>(data.size()) / kPoseSize);
}

cv::Mat TartanAir::GetImpl(std::string_view dtype, int i, int cam) const {
  const size_t ind = ToInd(i, cam);

  cv::Mat data;

  if (dtype == DataType::kImage) {
    const auto& files = files_.at(DataType::kImage);
    if (ind >= files.size()) return {};
    return CvReadImage(files.at(ind));
  }

  if (dtype == DataType::kDepth) {
#if XTENSOR_FOUND
    const auto& files = files_.at(DataType::kDepth);
    if (ind >= files.size()) return {};
    const auto raw = xt::load_npy<float>(files.at(ind));

    // Copy to Mat, maybe there is a faster way
    cv::Mat depth;
    depth.create(static_cast<int>(raw.shape()[0]),
                 static_cast<int>(raw.shape()[1]),
                 CV_32FC1);
    for (int r = 0; r < depth.rows; ++r) {
      for (int c = 0; c < depth.cols; ++c) {
        depth.at<float>(r, c) = raw(r, c);
      }
    }
    return depth;
#else
    LOG(WARNING) << "xtensor not found, unable to read npy, return empty depth";
    return {};
#endif
  }

  if (dtype == DataType::kIntrin) {
    return data_.at(DataType::kIntrin).row(cam).clone();
  }

  if (dtype == DataType::kPose) {
    return data_.at(DataType::kPose).row(static_cast<int>(ind)).clone();
  }

  return {};
}

cv::Mat TartanAir::ConvertPoses(const cv::Mat& poses) const {
  CHECK_EQ(poses.cols, kPoseDim);

  SE3d tf_w_ned;
  cv::Mat transforms;
  transforms.create(poses.rows, kTransformDim, CV_64FC1);

  // motion is defined in ned frame, but what we really need is motion of camera
  // T_w_c = T_w_ned @ R_ned_c
  Matrix3d R_ned_c;
  // clang-format off
  R_ned_c << 0, 0, 1,
             1, 0, 0,
             0, 1, 0;
  // clang-format on
  const SE3d tf_ned_c(R_ned_c, Vector3d::Zero());

  for (int i = 0; i < poses.rows; ++i) {
    MatToSE3d(poses.row(i), tf_w_ned);
    Eigen::Map<RowMat44d> tf_map(transforms.ptr<double>(i));
    tf_map = (tf_w_ned * tf_ned_c).matrix();
  }

  return transforms;
}

/// ============================================================================
KittiOdom::KittiOdom(const std::string& data_dir)
    : DatasetBase{"kitti", data_dir, kDtypes} {
  const fs::path data_path{data_dir_};
  const auto image_stereo = ReadStereoData(data_path / "image_0",
                                           data_path / "image_1",
                                           ".png",
                                           files_[DataType::kImage]);
  const auto image_size = static_cast<int>(files_.at(DataType::kImage).size());
  size_ = image_stereo ? image_size / 2 : image_size;

  // Intrinsics
  data_[DataType::kIntrin] = ReadIntrinsics(data_path / "calib.txt");

  // Extrinsics (GT poses)
  const cv::Mat raw_poses = ReadPoses(data_path / "../../poses" /
                                      (data_path.stem().string() + ".txt"));
  CHECK_EQ(size_, raw_poses.rows);
  data_[DataType::kPose] = ConvertPoses(raw_poses);
}

KittiOdom KittiOdom::Create(const std::string& base_dir, int seq) {
  return KittiOdom{fmt::format("{}/sequences/{:02d}", base_dir, seq)};
}

cv::Mat KittiOdom::GetImpl(std::string_view dtype, int i, int cam) const {
  if (dtype == DataType::kImage) {
    const size_t ind = ToInd(i, cam);
    const auto& files = files_.at(DataType::kImage);
    if (ind >= files.size()) return {};
    return CvReadImage(files.at(ind));
  }

  if (dtype == DataType::kIntrin) {
    cv::Mat intrin = data_.at(DataType::kIntrin).clone();
    if (cam == 1) {
      intrin.at<double>(4) *= -1;
    }
    return intrin;
  }

  if (dtype == DataType::kPose) {
    auto pose = data_.at(DataType::kPose).row(i).clone();
    if (cam == 1) {
      // Need pose of right camera
      // T_w_r = T_w_l * T_l_r
      //       = [R t] * [I b] = [R R*b + t]
      //       = [R R.col(0) * b + t]
      Eigen::Map<RowMat44d> T_w_l(pose.ptr<double>());
      T_w_l.topRightCorner<3, 1>() += T_w_l.topLeftCorner<3, 1>() * baseline_;
    }
    return pose;
  }

  return {};
}

cv::Mat KittiOdom::ReadIntrinsics(const std::string& file) {
  constexpr int kProjSize = 12;
  std::ifstream ifs{file};
  CHECK(ifs.good()) << "Unable to open file: " << file;
  std::string buf;
  // First line is left camera, but matrix is same for right camera
  // and right also contains baseline information
  std::getline(ifs, buf);

  std::vector<double> data;
  data.reserve(kProjSize);

  // Ignore first number (index) in line
  ifs >> buf;
  std::copy_n(
      std::istream_iterator<double>(ifs), kProjSize, std::back_inserter(data));

  // Projection matrix
  // [0, 1,  2,  3]
  // [4, 5,  6,  7]
  // [8, 9, 10, 11]

  baseline_ = -data[3] / data[0];
  std::vector<double> intrin{data[0], data[5], data[2], data[6], baseline_};
  return cv::Mat(intrin, true);
}

cv::Mat KittiOdom::ReadPoses(const std::string& file) const {
  constexpr int kTransSize = 12;
  std::ifstream ifs{file};
  CHECK(ifs.good()) << "Unable to open file: " << file;

  std::vector<double> data;
  data.reserve(static_cast<size_t>(size_ * kTransformDim));

  std::string line;
  while (std::getline(ifs, line)) {
    std::istringstream iss(line);
    std::copy_n(std::istream_iterator<double>(iss),
                kTransSize,
                std::back_inserter(data));
    // kitti pose file only stores 3x4 matrix, so in order to get 4x4 matrix we
    // need to push back bottom row manually
    data.push_back(0);
    data.push_back(0);
    data.push_back(0);
    data.push_back(1);
  }

  CHECK_EQ(data.size() % kTransformDim, 0);
  return cv::Mat(data, true)
      .reshape(0, static_cast<int>(data.size()) / kTransformDim);
}

cv::Mat KittiOdom::ConvertPoses(const cv::Mat& poses) const {
  CHECK_EQ(poses.cols, kTransformDim);

  cv::Mat transforms;
  transforms.create(poses.rows, kTransformDim, CV_64FC1);

  for (int i = 0; i < poses.rows; ++i) {
    Eigen::Map<const RowMat44d> extrin_map(poses.ptr<double>(i));
    const SE3d extrin(SO3d::fitToSO3(extrin_map.topLeftCorner<3, 3>()),
                      extrin_map.topRightCorner<3, 1>());
    Eigen::Map<RowMat44d> tf_map(transforms.ptr<double>(i));
    tf_map = extrin.matrix();
  }

  return transforms;
}

std::string ExtractDatasetName(const std::string& data_dir) {
  std::string name;
  // Extract name from base_dir
  if (absl::StrContains(data_dir, "vkitti")) {
    name = "vkitti";
  } else if (absl::StrContains(data_dir, "kitti")) {
    name = "kitti";
  } else if (absl::StrContains(data_dir, "tartan_air")) {
    name = "tartan_air";
  } else if (absl::StrContains(data_dir, "realsense")) {
    name = "realsense";
  }

  return name;
}

Dataset CreateDataset(const std::string& data_dir, std::string name) {
  if (name.empty()) {
    name = ExtractDatasetName(data_dir);
  }

  Dataset ds;

  if (name.empty()) {
    LOG(WARNING) << fmt::format("Could not infer dataset name from dir '{}'",
                                data_dir);
    return ds;
  }

  if (name == "vkitti") {
    ds = Vkitti2(data_dir);
  } else if (name == "kitti") {
    ds = KittiOdom(data_dir);
  } else if (name == "tartan_air") {
    ds = TartanAir(data_dir);
  } else if (name == "realsense") {
    ds = StereoFolder::Create(name, data_dir, "infra1", "infra2", "calib.txt");
  } else {
    LOG(WARNING) << fmt::format("Invalid dataset name: {}", name);
  }

  return ds;
}

StereoFolder::StereoFolder(const std::string& name,
                           const std::string& left_dir,
                           const std::string& right_dir,
                           const std::string& calib_file)
    : DatasetBase{name, left_dir, kDtypes} {
  const auto image_stereo =
      ReadStereoData(left_dir, right_dir, ".png", files_[DataType::kImage]);
  const auto image_size = static_cast<int>(files_.at(DataType::kImage).size());
  size_ = image_stereo ? image_size / 2 : image_size;

  // Intrinsics fx fy cx cy b
  data_[DataType::kIntrin] = ReadIntrinsics(calib_file);
}

StereoFolder StereoFolder::Create(const std::string& data_name,
                                  const std::string& data_dir,
                                  const std::string& left_name,
                                  const std::string& right_name,
                                  const std::string& calib_name) {
  const auto data_path = fs::path(data_dir);
  return StereoFolder(data_name,
                      data_path / left_name,
                      data_path / right_name,
                      data_path / calib_name);
}

cv::Mat StereoFolder::GetImpl(std::string_view dtype, int i, int cam) const {
  if (dtype == DataType::kImage) {
    const size_t ind = ToInd(i, cam);
    const auto& files = files_.at(DataType::kImage);
    if (ind >= files.size()) return {};
    return CvReadImage(files.at(ind));
  }

  if (dtype == DataType::kIntrin) {
    return data_.at(DataType::kIntrin).row(cam).clone();
  }

  return {};
}

cv::Mat StereoFolder::ReadIntrinsics(const std::string& file) {
  std::ifstream ifs{file};
  CHECK(ifs.good()) << "Unable to open file: " << file;
  // Get first line and parse [fx, fy, cx, cy, b]
  std::string buf;
  std::getline(ifs, buf);

  std::vector<double> data;
  std::istringstream iss(buf);
  std::copy_n(std::istream_iterator<double>(iss), 5, std::back_inserter(data));
  return MakeStereoIntrins(cv::Mat(data).reshape(0, 1));
}

}  // namespace sv