bool CanReceive(const Socket* sock, long timeout_microseconds)

#include "wiPlatform.h"

#ifdef PLATFORM_LINUX
#include "wiNetwork.h"
#include "wiBackLog.h"
#include "wiTimer.h"

#include <unistd.h>
#include <errno.h>

#include <sys/socket.h>
#include <netinet/in.h>

namespace wiNetwork
{
	//For easy address conversion
	struct in_addr_union {
		union {
			struct { uint8_t s_b1,s_b2,s_b3,s_b4; } S_un_b;
			struct { uint16_t s_w1,s_w2; } S_un_w;
			uint32_t S_addr;
		};
	};

	struct SocketInternal{
		int handle;
		~SocketInternal(){
			int result = close(handle);
			if(result < 0){
				assert(0);
			}
		}
	};

	SocketInternal* to_internal(const Socket* param)
	{
		return static_cast<SocketInternal*>(param->internal_state.get());
	}

	void Initialize()
	{
		wiTimer timer;

		int result;

		wiBackLog::post("wiNetwork_Linux Initialized (" + std::to_string((int)std::round(timer.elapsed())) + " ms)");
	}

	bool CreateSocket(Socket* sock)
	{
		std::shared_ptr<SocketInternal> socketinternal = std::make_shared<SocketInternal>();
		sock->internal_state = socketinternal;

		socketinternal->handle = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP);

		if(socketinternal->handle == -1){
			std::stringstream ss;
			ss << "wiNetwork_Linux error in CreateSocket: Could not create socket";
			wiBackLog::post(ss.str().c_str());
			return false;
		}

		return true;
	}

	bool Send(const Socket* sock, const Connection* connection, const void* data, size_t dataSize)
	{
		if (sock->IsValid()){
			sockaddr_in target;
			target.sin_family = AF_INET;
			target.sin_port = htons(connection->port);
			in_addr_union address;
			address.S_un_b.s_b1 = connection->ipaddress[0];
			address.S_un_b.s_b2 = connection->ipaddress[1];
			address.S_un_b.s_b3 = connection->ipaddress[2];
			address.S_un_b.s_b4 = connection->ipaddress[3];
			target.sin_addr.s_addr = address.S_addr;

			auto socketinternal = to_internal(sock);
			int result = sendto(socketinternal->handle, (const char*)data, (int)dataSize, 0, (const sockaddr*)&target, sizeof(target));
			if (result < 0)
			{
				std::stringstream ss;
				ss << "wiNetwork_Linux error in Send: (Error Code: " + std::to_string(result) + ") " + strerror(result);
				wiBackLog::post(ss.str().c_str());
				return false;
			}

			return true;
		}
		return false;
	}

	bool ListenPort(const Socket* sock, uint16_t port)
	{
		if (sock->IsValid()){
			sockaddr_in target;
			target.sin_family = AF_INET;
			target.sin_port = htons(port);
			target.sin_addr.s_addr = htonl(INADDR_ANY);

			auto socketinternal = to_internal(sock);

			int result = bind(socketinternal->handle, (struct sockaddr *)&target , sizeof(target));
			if (result < 0)
			{
				std::stringstream ss;
				ss << "wiNetwork_Linux error in Send: (Error Code: " + std::to_string(result) + ") " + strerror(result);
				wiBackLog::post(ss.str().c_str());
				return false;
			}

			return true;
		}
		return false;
	}

{
		if (sock->IsValid()){
			auto socketinternal = to_internal(sock);

			fd_set readfds;
			FD_ZERO(&readfds);
			FD_SET(socketinternal->handle, &readfds);
			timeval timeout;
			timeout.tv_sec = 0;
			timeout.tv_usec = timeout_microseconds;

			int result = select(0, &readfds, NULL, NULL, &timeout);
			if (result < 0)
			{
				std::stringstream ss;
				ss << "wiNetwork_Linux error in Send: (Error Code: " + std::to_string(result) + ") " + strerror(result);
				wiBackLog::post(ss.str().c_str());
				assert(0);
				return false;
			}

			return FD_ISSET(socketinternal->handle, &readfds);
		}
		return false;
	}

	bool Receive(const Socket* sock, Connection* connection, void* data, size_t dataSize)
	{
		if (sock->IsValid()){
			auto socketinternal = to_internal(sock);

			sockaddr_in sender;
			int targetsize = sizeof(sender);
			int result = recvfrom(socketinternal->handle, (char*)data, (int)dataSize, 0, (sockaddr*)& sender, (socklen_t*)&targetsize);
			if (result < 0)
			{
				std::stringstream ss;
				ss << "wiNetwork_Linux error in Send: (Error Code: " + std::to_string(result) + ") " + strerror(result);
				wiBackLog::post(ss.str().c_str());
				assert(0);
				return false;
			}

			connection->port = htons(sender.sin_port); // reverse byte order from network to host
			in_addr_union address;
			address.S_addr = sender.sin_addr.s_addr;
			connection->ipaddress[0] = address.S_un_b.s_b1;
			connection->ipaddress[1] = address.S_un_b.s_b2;
			connection->ipaddress[2] = address.S_un_b.s_b3;
			connection->ipaddress[3] = address.S_un_b.s_b4;

			return true;
		}
		return false;
	}
}

#endif // LINUX


{
		return static_cast<SocketInternal*>(param->internal_state.get());
	}

#include "wiPlatform.h"

#ifdef PLATFORM_LINUX
#include "wiNetwork.h"
#include "wiBackLog.h"
#include "wiTimer.h"

#include <unistd.h>
#include <errno.h>

#include <sys/socket.h>
#include <netinet/in.h>

namespace wiNetwork
{
	//For easy address conversion
	struct in_addr_union {
		union {
			struct { uint8_t s_b1,s_b2,s_b3,s_b4; } S_un_b;
			struct { uint16_t s_w1,s_w2; } S_un_w;
			uint32_t S_addr;
		};
	};

	struct SocketInternal{
		int handle;
		~SocketInternal(){
			int result = close(handle);
			if(result < 0){
				assert(0);
			}
		}
	};

	SocketInternal* to_internal(const Socket* param)

	void Initialize()
	{
		wiTimer timer;

		int result;

		wiBackLog::post("wiNetwork_Linux Initialized (" + std::to_string((int)std::round(timer.elapsed())) + " ms)");
	}

	bool CreateSocket(Socket* sock)
	{
		std::shared_ptr<SocketInternal> socketinternal = std::make_shared<SocketInternal>();
		sock->internal_state = socketinternal;

		socketinternal->handle = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP);

		if(socketinternal->handle == -1){
			std::stringstream ss;
			ss << "wiNetwork_Linux error in CreateSocket: Could not create socket";
			wiBackLog::post(ss.str().c_str());
			return false;
		}

		return true;
	}

	bool Send(const Socket* sock, const Connection* connection, const void* data, size_t dataSize)
	{
		if (sock->IsValid()){
			sockaddr_in target;
			target.sin_family = AF_INET;
			target.sin_port = htons(connection->port);
			in_addr_union address;
			address.S_un_b.s_b1 = connection->ipaddress[0];
			address.S_un_b.s_b2 = connection->ipaddress[1];
			address.S_un_b.s_b3 = connection->ipaddress[2];
			address.S_un_b.s_b4 = connection->ipaddress[3];
			target.sin_addr.s_addr = address.S_addr;

			auto socketinternal = to_internal(sock);
			int result = sendto(socketinternal->handle, (const char*)data, (int)dataSize, 0, (const sockaddr*)&target, sizeof(target));
			if (result < 0)
			{
				std::stringstream ss;
				ss << "wiNetwork_Linux error in Send: (Error Code: " + std::to_string(result) + ") " + strerror(result);
				wiBackLog::post(ss.str().c_str());
				return false;
			}

			return true;
		}
		return false;
	}

	bool ListenPort(const Socket* sock, uint16_t port)
	{
		if (sock->IsValid()){
			sockaddr_in target;
			target.sin_family = AF_INET;
			target.sin_port = htons(port);
			target.sin_addr.s_addr = htonl(INADDR_ANY);

			auto socketinternal = to_internal(sock);

			int result = bind(socketinternal->handle, (struct sockaddr *)&target , sizeof(target));
			if (result < 0)
			{
				std::stringstream ss;
				ss << "wiNetwork_Linux error in Send: (Error Code: " + std::to_string(result) + ") " + strerror(result);
				wiBackLog::post(ss.str().c_str());
				return false;
			}

			return true;
		}
		return false;
	}

	bool CanReceive(const Socket* sock, long timeout_microseconds)
	{
		if (sock->IsValid()){
			auto socketinternal = to_internal(sock);

			fd_set readfds;
			FD_ZERO(&readfds);
			FD_SET(socketinternal->handle, &readfds);
			timeval timeout;
			timeout.tv_sec = 0;
			timeout.tv_usec = timeout_microseconds;

			int result = select(0, &readfds, NULL, NULL, &timeout);
			if (result < 0)
			{
				std::stringstream ss;
				ss << "wiNetwork_Linux error in Send: (Error Code: " + std::to_string(result) + ") " + strerror(result);
				wiBackLog::post(ss.str().c_str());
				assert(0);
				return false;
			}

			return FD_ISSET(socketinternal->handle, &readfds);
		}
		return false;
	}

	bool Receive(const Socket* sock, Connection* connection, void* data, size_t dataSize)
	{
		if (sock->IsValid()){
			auto socketinternal = to_internal(sock);

			sockaddr_in sender;
			int targetsize = sizeof(sender);
			int result = recvfrom(socketinternal->handle, (char*)data, (int)dataSize, 0, (sockaddr*)& sender, (socklen_t*)&targetsize);
			if (result < 0)
			{
				std::stringstream ss;
				ss << "wiNetwork_Linux error in Send: (Error Code: " + std::to_string(result) + ") " + strerror(result);
				wiBackLog::post(ss.str().c_str());
				assert(0);
				return false;
			}

			connection->port = htons(sender.sin_port); // reverse byte order from network to host
			in_addr_union address;
			address.S_addr = sender.sin_addr.s_addr;
			connection->ipaddress[0] = address.S_un_b.s_b1;
			connection->ipaddress[1] = address.S_un_b.s_b2;
			connection->ipaddress[2] = address.S_un_b.s_b3;
			connection->ipaddress[3] = address.S_un_b.s_b4;

			return true;
		}
		return false;
	}
}

#endif // LINUX


#include "wiPlatform.h"

#ifdef PLATFORM_LINUX
#include "wiNetwork.h"
#include "wiBackLog.h"
#include "wiTimer.h"

#include <unistd.h>
#include <errno.h>

#include <sys/socket.h>
#include <netinet/in.h>

namespace wiNetwork
{
	//For easy address conversion
	struct in_addr_union {
		union {
			struct { uint8_t s_b1,s_b2,s_b3,s_b4; } S_un_b;
			struct { uint16_t s_w1,s_w2; } S_un_w;
			uint32_t S_addr;
		};
	};

	struct SocketInternal{
		int handle;
		~SocketInternal(){
			int result = close(handle);
			if(result < 0){
				assert(0);
			}
		}
	};

	SocketInternal* to_internal(const Socket* param)
	{
		return static_cast<SocketInternal*>(param->internal_state.get());
	}

	void Initialize()
	{
		wiTimer timer;

		int result;

		wiBackLog::post("wiNetwork_Linux Initialized (" + std::to_string((int)std::round(timer.elapsed())) + " ms)");
	}

	bool CreateSocket(Socket* sock)
	{
		std::shared_ptr<SocketInternal> socketinternal = std::make_shared<SocketInternal>();
		sock->internal_state = socketinternal;

		socketinternal->handle = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP);

		if(socketinternal->handle == -1){
			std::stringstream ss;
			ss << "wiNetwork_Linux error in CreateSocket: Could not create socket";
			wiBackLog::post(ss.str().c_str());
			return false;
		}

		return true;
	}

	bool Send(const Socket* sock, const Connection* connection, const void* data, size_t dataSize)
	{
		if (sock->IsValid()){
			sockaddr_in target;
			target.sin_family = AF_INET;
			target.sin_port = htons(connection->port);
			in_addr_union address;
			address.S_un_b.s_b1 = connection->ipaddress[0];
			address.S_un_b.s_b2 = connection->ipaddress[1];
			address.S_un_b.s_b3 = connection->ipaddress[2];
			address.S_un_b.s_b4 = connection->ipaddress[3];
			target.sin_addr.s_addr = address.S_addr;

			auto socketinternal = to_internal(sock);
			int result = sendto(socketinternal->handle, (const char*)data, (int)dataSize, 0, (const sockaddr*)&target, sizeof(target));
			if (result < 0)
			{
				std::stringstream ss;
				ss << "wiNetwork_Linux error in Send: (Error Code: " + std::to_string(result) + ") " + strerror(result);
				wiBackLog::post(ss.str().c_str());
				return false;
			}

			return true;
		}
		return false;
	}

	bool ListenPort(const Socket* sock, uint16_t port)
	{
		if (sock->IsValid()){
			sockaddr_in target;
			target.sin_family = AF_INET;
			target.sin_port = htons(port);
			target.sin_addr.s_addr = htonl(INADDR_ANY);

			auto socketinternal = to_internal(sock);

			int result = bind(socketinternal->handle, (struct sockaddr *)&target , sizeof(target));
			if (result < 0)
			{
				std::stringstream ss;
				ss << "wiNetwork_Linux error in Send: (Error Code: " + std::to_string(result) + ") " + strerror(result);
				wiBackLog::post(ss.str().c_str());
				return false;
			}

			return true;
		}
		return false;
	}

	bool CanReceive(const Socket* sock, long timeout_microseconds)
	{
		if (sock->IsValid()){
			auto socketinternal = to_internal(sock);

			fd_set readfds;
			FD_ZERO(&readfds);
			FD_SET(socketinternal->handle, &readfds);
			timeval timeout;
			timeout.tv_sec = 0;
			timeout.tv_usec = timeout_microseconds;

			int result = select(0, &readfds, NULL, NULL, &timeout);
			if (result < 0)
			{
				std::stringstream ss;
				ss << "wiNetwork_Linux error in Send: (Error Code: " + std::to_string(result) + ") " + strerror(result);
				wiBackLog::post(ss.str().c_str());
				assert(0);
				return false;
			}

			return FD_ISSET(socketinternal->handle, &readfds);
		}
		return false;
	}

	bool Receive(const Socket* sock, Connection* connection, void* data, size_t dataSize)
	{
		if (sock->IsValid()){
			auto socketinternal = to_internal(sock);

			sockaddr_in sender;
			int targetsize = sizeof(sender);
			int result = recvfrom(socketinternal->handle, (char*)data, (int)dataSize, 0, (sockaddr*)& sender, (socklen_t*)&targetsize);
			if (result < 0)
			{
				std::stringstream ss;
				ss << "wiNetwork_Linux error in Send: (Error Code: " + std::to_string(result) + ") " + strerror(result);
				wiBackLog::post(ss.str().c_str());
				assert(0);
				return false;
			}

			connection->port = htons(sender.sin_port); // reverse byte order from network to host
			in_addr_union address;
			address.S_addr = sender.sin_addr.s_addr;
			connection->ipaddress[0] = address.S_un_b.s_b1;
			connection->ipaddress[1] = address.S_un_b.s_b2;
			connection->ipaddress[2] = address.S_un_b.s_b3;
			connection->ipaddress[3] = address.S_un_b.s_b4;

			return true;
		}
		return f


{
    namespace phx = boost::phoenix;
    using boost::phoenix::arg_names::arg1;
    using boost::phoenix::arg_names::arg2;
    using namespace phx;

    list<int> l0;
    list<int> l1 = list_with<>()(1,2,3,4,5);
    list<int> l2 = all_but_last(l1)();

    BOOST_TEST(null(l0)());
    BOOST_TEST(head(l1)()       == 1);
    BOOST_TEST(head(tail(l1))() == 2);
    BOOST_TEST(last(l1)()       == 5);
    BOOST_TEST(last(l2)()       == 4);
    BOOST_TEST(head(drop(2,l2))() == 3);

    return boost::report_errors();
}


// lazy_list2_tests.cpp
// more tests on list<T>
/*=============================================================================

==============================================================================*/

#include <boost/phoenix/core/limits.hpp>

#include <boost/detail/lightweight_test.hpp>
#include <boost/phoenix/core.hpp>
#include <boost/phoenix/function/lazy_prelude.hpp>

int main()
{
    namespace phx = boost::phoenix;
    using boost::phoenix::arg_names::arg1;
    using boost::phoenix::arg_names::arg2;
    using namespace phx;

    list<int> l0;
    list<int> l1 = list_with<>()(1,2,3,4,5);
    list<int> l2 = all_but_last(l1)();

    BOOST_TEST(null(l0)());
    BOOST_TEST(head(l1)()       == 1);
    BOOST_TEST(head(tail(l1))() == 2);
    BOOST_TEST(l

    BOOST_TEST(last(l2)()       == 4);
    BOOST_TEST(head(drop(2,l2))() == 3);

    return boost::report_errors();
}


/*
 *
 */

#include "experimental/graphite/src/Image_Graphite.h"

#if SK_SUPPORT_GPU
// TODO: move onAsFragmentProcessor off of SkImage_Base and remove this include
#include "src/gpu/GrFragmentProcessor.h"
#endif

namespace skgpu {

Image_Graphite::Image_Graphite(const SkImageInfo& ii)
    : SkImage_Base(ii, kNeedNewImageUniqueID) {
}

{}

#if SK_SUPPORT_GPU
std::unique_ptr<GrFragmentProcessor> Image_Graphite::onAsFragmentProcessor(
        GrRecordingContext*,
        SkSamplingOptions,
        const SkTileMode[2],
        const SkMatrix&,
        const SkRect* subset,
        const SkRect* domain) const {
    return nullptr;
}
#endif

} // namespace skgpu


/*
 *
 */

#include "experimental/graphite/src/Image_Graphite.h"

#if SK_SUPPORT_GPU
// TODO: move onAsFragmentProcessor off of SkImage_Base and remove this include
#include "src/gpu/GrFragmentProcessor.h"
#endif

namespace skgpu {

Image_Graphite::Image_Graphite(const SkImageInfo& ii)
    : SkImage_Base(ii, kNeedNewImageUniqueID) {
}

Image_Graphite::~Image_Graphite()

#if SK_SUPPORT_GPU
std::unique_ptr<GrFragmentProcessor> Image_Graphite::onAsFragmentProcessor(
        GrRecordingContext*,
        SkSamplingOptions,
        const SkTileMode[2],
        const SkMatrix&,
        const SkRect* subset,
        const SkRect* domain) const {
    return nullptr;
}
#endif

} // namespace skgpu


/*
 *
 */

#include "experimental/graphite/src/Image_Graphite.h"

#if SK_SUPPORT_GPU
// TODO: move onAsFragmentProcessor off of SkImage_Base and remove this include
#include "src/gpu/GrFragmentProcessor.h"
#endif

namespace skgpu {

Image_Graphite::Image_Graphite(const SkImageInfo& ii)
    : SkImage_Base(ii, kNeedNewImageUniqueID) {
}

Image_Graphite::~Image_Graphite() {}

#if SK_SUPPORT_GPU
std::unique_ptr<GrFragmentProcessor> Image_Graphite::onAsFragmentProcessor(
        GrRecordingContext*,
        SkSamplingOptions,
        const SkTileMode[2],
        const SkMatrix&,
        const SkRect* subset,
        const SkRect* domain) const {
    return nul


/*
 *
 */
#include <stratosphere.hpp>

#include "impl/gpio_driver_core.hpp"

namespace ams::gpio::driver {

    bool Pad::IsAnySessionBoundToInterrupt() const {
        /* Check to see if any session has an interrupt bound. */
        bool bound = false;
        this->ForEachSession([&](const ddsf::ISession &session) -> bool {
           const auto &impl = session.SafeCastTo<impl::PadSessionImpl>();
           if (impl.IsInterruptBound()) {
               bound = true;
               return false;
           }
           return true;
        });

        return bound;
    }

{
        /* Signal relevant sessions. */
        this->ForEachSession([&](ddsf::ISession &session) -> bool {
           auto &impl = session.SafeCastTo<impl::PadSessionImpl>();
           if (impl.IsInterruptBound()) {
               impl.SignalInterruptBoundEvent();
           }
           return true;
        });
    }

}


{
        /* Signal relevant sessions. */
        this->ForEachSession([&](ddsf::ISession &session) -> bool {
           auto &impl = session.SafeCastTo<impl::PadSessionImpl>();
           if (impl.IsInterruptBound()) {
               impl.SignalInterruptBoundEvent();
           }
           return true;
        });
    }

/*
 *
 */
#include <stratosphere.hpp>

#include "impl/gpio_driver_core.hpp"

namespace ams::gpio::driver {

    bool Pad::IsAnySessionBoundToInterrupt() const {
        /* Check to see if any session has an interrupt bound. */
        bool bound = false;
        this->ForEachSession([&](const ddsf::ISession &session) -> bool {
           const auto &impl = session.SafeCastTo<impl::PadSessionImpl>();
           if (impl.IsInterruptBound()) {
               bound = true;
               return false;
           }
           return true;
        });

        return bound;
    }

    void Pad::SignalInterruptBoundEvent()


/*
 *
 */
#include <stratosphere.hpp>

#include "impl/gpio_driver_core.hpp"

namespace ams::gpio::driver {

    bool Pad::IsAnySessionBoundToInterrupt() const {
        /* Check to see if any session has an interrupt bound. */
        bool bound = fals

        this->ForEachSession([&](const ddsf::ISession &session) -> bool {
           const auto &impl = session.SafeCastTo<impl::PadSessionImpl>();
           if (impl.IsInterruptBound()) {
               bound = true;
               return false;
           }
           return true;
        });

        return bound;
    }

    void Pad::SignalInterruptBoundEvent() {
        /* Signal relevant sessions. */
        this->ForEachSession([&](ddsf::ISession &session) -> bool {
           auto &impl = session.SafeCastTo<impl::PadSessionImpl>();
           if (impl.IsInterruptBound()) {
               impl.SignalInterruptBoundEvent();
           }
           return true;
        });
    }

}


inline bool lineIntersect( E e, E f, P *r = NULL )

/**
 *  ///////////////////////////////
 *  // LINE SEGMENT INTERSECTION //
 *  ///////////////////////////////
 *
 * MAIN FUNCTION: lineSegIntersect( E, E, E* )
 *      Takes two line segments in the plane and returns either
 *      true or false, depending on whether they intersect or not. If
 *      they do, also returns the line segment that is the intersection
 *      of the two given line segments.
 * TYPES:
 *      - P: point in 2D
 *      - E: edge (line segment)
 * WARNING:
 *      This code is too long, too slow and too ugly, but it seems
 * FIELD TESTING:
 *      - UVa 10709: Intersection is not that Easy
 *
 * LAST MODIFIED:
 *      October 24, 2005
 *
 * This file is part of my library of algorithms found here:
 *      http://example.com
 * LICENSE:
 *      http://example.com
 **/
#include <math.h>

struct P { double x, y; };
struct E
{
    P a, b;
    E(){}
    E( P x, P y ) { a = x; b = y; }
};

#define EPS 1e-7

inline double dist2( P p, P q )
{
    return ( p.x - q.x ) * ( p.x - q.x ) + ( p.y - q.y ) * ( p.y - q.y );
}

inline double dist( P p, P q )
{
    return sqrt( dist2( p, q ) );
}

inline bool lineIntersect( P a, P b, P c, P d, P &r )
{
    P n; n.x = d.y - c.y; n.y = c.x - d.x;
    double denom = n.x * ( b.x - a.x ) + n.y * ( b.y - a.y );
    if( fabs( denom ) < EPS ) return false;
    double num = n.x * ( a.x - c.x ) + n.y * ( a.y - c.y );
    double t = -num / denom;
    r.x = a.x + t * ( b.x - a.x );
    r.y = a.y + t * ( b.y - a.y );
    return true;
}

{
    return lineIntersect( e.a, e.b, f.a, f.b, *r );
}

inline double cross( P a, P b, P c )
{
    return ( b.x - a.x ) * ( c.y - b.y ) - ( b.y - a.y ) * ( c.x - b.x );
}

bool lineSegIntersect( E e, E f, E *r = NULL )
{
    E blah; if( !r ) r = &blah;

    double c1 = cross( e.a, e.b, f.a );
    double c2 = cross( e.b, e.a, f.b );
    double c3 = cross( f.a, f.b, e.a );
    double c4 = cross( f.b, f.a, e.b );
    if( c1 * c2 < -EPS || c3 * c4 < -EPS ) return false;
    if( fabs( c1 ) > EPS || fabs( c2 ) > EPS || fabs( c3 ) > EPS || fabs( c4 ) > EPS )
    {
        lineIntersect( e, f, &r->a );
        r->b = r->a;
        return dist( e.a, r->b ) + dist( r->b, e.b ) - dist( e.a, e.b ) <= EPS
            && dist( f.a, r->b ) + dist( r->b, f.b ) - dist( f.a, f.b ) <= EPS;
    }
    else
    {
        // degenerate line segments?
        if( dist2( e.a, e.b ) <= EPS && dist2( f.a, f.b ) <= EPS && dist2( f.a, e.a ) > EPS )
            return false;

        bool fa = dist( e.a, e.b ) >= dist( e.a, f.a ) + dist( e.b, f.a ) - EPS;
        bool fb = dist( e.a, e.b ) >= dist( e.a, f.b ) + dist( e.b, f.b ) - EPS;
        switch( ( fa ? 10 : 0 ) + ( fb ? 1 : 0 ) )
        {
            case 11:        // f is inside e
                r->a = f.a;
                r->b = f.b;
                return true;

            case 10:        // only f.a is inside e
                r->a = ( dist2( e.a, f.b ) <= dist2( e.b, f.b ) ? e.a : e.b );
                r->b = f.a;
                return true;

            case 1:         // only f.b is inside e
                r->a = ( dist2( e.a, f.a ) <= dist2( e.b, f.a ) ? e.a : e.b );
                r->b = f.b;
                return true;

            case 0:         // both outside of e
                if( dist( f.a, e.a ) + dist( e.a, f.b ) <= dist( f.a, f.b ) + EPS &&
                    dist( f.a, e.b ) + dist( e.b, f.b ) <= dist( f.a, f.b ) + EPS )
                {
                    // e is inside f
                    r->a = e.a;
                    r->b = e.b;
                    return true;
                }
                return false;
        }
    }
    return false;       // Will never get here
}


/**
 *  ///////////////////////////////
 *  // LINE SEGMENT INTERSECTION //
 *  ///////////////////////////////
 *
 * MAIN FUNCTION: lineSegIntersect( E, E, E* )
 *      Takes two line segments in the plane and returns either
 *      true or false, depending on whether they intersect or not. If
 *      they do, also returns the line segment that is the intersection
 *      of the two given line segments.
 * TYPES:
 *      - P: point in 2D
 *      - E: edge (line segment)
 * WARNING:
 *      This code is too long, too slow and too ugly, but it seems
 * FIELD TESTING:
 *      - UVa 10709: Intersection is not that Easy
 *
 * LAST MODIFIED:
 *      October 24, 2005
 *
 * This file is part of my library of algorithms found here:
 *      http://example.com
 * LICENSE:
 *      http://example.com
 **/
#include <math.h>

struct P { double x, y; };
struct E
{
    P a, b;
    E()

    E( P x, P y ) { a = x; b = y; }
};

#define EPS 1e-7

inline double dist2( P p, P q )
{
    return ( p.x - q.x ) * ( p.x - q.x ) + ( p.y - q.y ) * ( p.y - q.y );
}

inline double dist( P p, P q )
{
    return sqrt( dist2( p, q ) );
}

inline bool lineIntersect( P a, P b, P c, P d, P &r )
{
    P n; n.x = d.y - c.y; n.y = c.x - d.x;
    double denom = n.x * ( b.x - a.x ) + n.y * ( b.y - a.y );
    if( fabs( denom ) < EPS ) return false;
    double num = n.x * ( a.x - c.x ) + n.y * ( a.y - c.y );
    double t = -num / denom;
    r.x = a.x + t * ( b.x - a.x );
    r.y = a.y + t * ( b.y - a.y );
    return true;
}

inline bool lineIntersect( E e, E f, P *r = NULL )
{
    return lineIntersect( e.a, e.b, f.a, f.b, *r );
}

inline double cross( P a, P b, P c )
{
    return ( b.x - a.x ) * ( c.y - b.y ) - ( b.y - a.y ) * ( c.x - b.x );
}

bool lineSegIntersect( E e, E f, E *r = NULL )
{
    E blah; if( !r ) r = &blah;

    double c1 = cross( e.a, e.b, f.a );
    double c2 = cross( e.b, e.a, f.b );
    double c3 = cross( f.a, f.b, e.a );
    double c4 = cross( f.b, f.a, e.b );
    if( c1 * c2 < -EPS || c3 * c4 < -EPS ) return false;
    if( fabs( c1 ) > EPS || fabs( c2 ) > EPS || fabs( c3 ) > EPS || fabs( c4 ) > EPS )
    {
        lineIntersect( e, f, &r->a );
        r->b = r->a;
        return dist( e.a, r->b ) + dist( r->b, e.b ) - dist( e.a, e.b ) <= EPS
            && dist( f.a, r->b ) + dist( r->b, f.b ) - dist( f.a, f.b ) <= EPS;
    }
    else
    {
        // degenerate line segments?
        if( dist2( e.a, e.b ) <= EPS && dist2( f.a, f.b ) <= EPS && dist2( f.a, e.a ) > EPS )
            return false;

        bool fa = dist( e.a, e.b ) >= dist( e.a, f.a ) + dist( e.b, f.a ) - EPS;
        bool fb = dist( e.a, e.b ) >= dist( e.a, f.b ) + dist( e.b, f.b ) - EPS;
        switch( ( fa ? 10 : 0 ) + ( fb ? 1 : 0 ) )
        {
            case 11:        // f is inside e
                r->a = f.a;
                r->b = f.b;
                return true;

            case 10:        // only f.a is inside e
                r->a = ( dist2( e.a, f.b ) <= dist2( e.b, f.b ) ? e.a : e.b );
                r->b = f.a;
                return true;

            case 1:         // only f.b is inside e
                r->a = ( dist2( e.a, f.a ) <= dist2( e.b, f.a ) ? e.a : e.b );
                r->b = f.b;
                return true;

            case 0:         // both outside of e
                if( dist( f.a, e.a ) + dist( e.a, f.b ) <= dist( f.a, f.b ) + EPS &&
                    dist( f.a, e.b ) + dist( e.b, f.b ) <= dist( f.a, f.b ) + EPS )
                {
                    // e is inside f
                    r->a = e.a;
                    r->b = e.b;
                    return true;
                }
                return false;
        }
    }
    return false;       // Will never get here
}


/**
 *  ///////////////////////////////
 *  // LINE SEGMENT INTERSECTION //
 *  ///////////////////////////////
 *
 * MAIN FUNCTION: lineSegIntersect( E, E, E* )
 *      Takes two line segments in the plane and returns either
 *      true or false, depending on whether they intersect or not. If
 *      they do, also returns the line segment that is the intersection
 *      of the two given line segments.
 * TYPES:
 *      - P: point in 2D
 *      - E: edge (line segment)
 * WARNING:
 *      This code is too long, too slow and too ugly, but it seems
 * FIELD TESTING:
 *      - UVa 10709: Intersection is not that Easy
 *
 * LAST MODIFIED:
 *      October 24, 2005
 *
 * This file is part of my library of algorithms found here:
 *      http://example.com
 * LICENSE:
 *      http://example.com
 **/
#include <math.h>

struct P { double x, y; };
struct E
{
    P a, b;
    E(){}
    E( P x, P y ) { a = x; b = y; }
};

#define EPS 1e-7

inline double dist2( P p, P q )
{
    return ( p.x - q.x ) * ( p.x - q.x ) + ( p.y - q.y ) * ( p.y - q.y );
}

inline double dist( P p, P q )
{
    return sqrt( dist2( p, q ) );
}

inline bool lineIntersect( P a, P b, P c, P d, P &r )
{
    P n; n.x = d.y - c.y; n.y = c.x - d.x;
    double denom = n.x * ( b.x - a.x ) + n.y * ( b.y - a.y );
    if( fabs( denom ) < EPS ) return false;
    double num = n.x * ( a.x - c.x ) + n.y * ( a.y - c.y );
    double t = -num / denom;
    r.x = a.x + t * ( b.x - a.x );
    r.y = a.y + t * ( b.y - a.y );
    return true;
}

inline bool lineIntersect( E e, E f, P *r = NULL )
{
    return lineIntersect( e.a, e.b, f.a, f.b, *r );
}

inline double cross( P a, P b, P c )
{
    return ( b.x - a.x ) * ( c.y - b.y ) - ( b.y - a.y ) * ( c.x - b.x );
}

bool lineSegIntersect( E e, E f, E *r = NULL )
{
    E blah; if( !r ) r = &blah;

    double c1 = cross( e.a, e.b, f.a );
    double c2 = cross( e.b, e.a, f.b );
    double c3 = cross( f.a, f.b, e

    double c4 = cross( f.b, f.a, e.b );
    if( c1 * c2 < -EPS || c3 * c4 < -EPS ) return false;
    if( fabs( c1 ) > EPS || fabs( c2 ) > EPS || fabs( c3 ) > EPS || fabs( c4 ) > EPS )
    {
        lineIntersect( e, f, &r->a );
        r->b = r->a;
        return dist( e.a, r->b ) + dist( r->b, e.b ) - dist( e.a, e.b ) <= EPS
            && dist( f.a, r->b ) + dist( r->b, f.b ) - dist( f.a, f.b ) <= EPS;
    }
    else
    {
        // degenerate line segments?
        if( dist2( e.a, e.b ) <= EPS && dist2( f.a, f.b ) <= EPS && dist2( f.a, e.a ) > EPS )
            return false;

        bool fa = dist( e.a, e.b ) >= dist( e.a, f.a ) + dist( e.b, f.a ) - EPS;
        bool fb = dist( e.a, e.b ) >= dist( e.a, f.b ) + dist( e.b, f.b ) - EPS;
        switch( ( fa ? 10 : 0 ) + ( fb ? 1 : 0 ) )
        {
            case 11:        // f is inside e
                r->a = f.a;
                r->b = f.b;
                return true;

            case 10:        // only f.a is inside e
                r->a = ( dist2( e.a, f.b ) <= dist2( e.b, f.b ) ? e.a : e.b );
                r->b = f.a;
                return true;

            case 1:         // only f.b is inside e
                r->a = ( dist2( e.a, f.a ) <= dist2( e.b, f.a ) ? e.a : e.b );
                r->b = f.b;
                return true;

            case 0:         // both outside of e
                if( dist( f.a, e.a ) + dist( e.a, f.b ) <= dist( f.a, f.b ) + EPS &&
                    dist( f.a, e.b ) + dist( e.b, f.b ) <= dist( f.a, f.b ) + EPS )
                {
                    // e is inside f
                    r->a = e.a;
                    r->b = e.b;
                    return true;
                }
                return false;
        }
    }
    return false;       // Will never get here
}


BulletImpl::BulletImpl(int _index)
    : self( &(BWAPI::BWAPIClient.data->bullets[_index]) )
    , index(_index)

#include <BWAPI.h>
#include <BWAPI/Client/GameImpl.h>
#include <BWAPI/Client/BulletImpl.h>

namespace BWAPI
{


#include <BWAPI.h>
#include <BWAPI/Client/GameImpl.h>
#include <BWAPI/Client/BulletImpl.h>

namespace BWAPI
{
  BulletImpl::BulletImpl(int _index)
    : self( &(BWAPI::BWAPIClient.data->bullets[_index]) )
    , index(_index)


/*
Description: Binary search using recursion

Approach: To use recursion, for updating start and end variables.

Time Complexity: O(log n)
*/

#include <iostream>
using namespace std;

//function starts
int search(int arr[], int n, int start, int end, int target)
{
    //if start is greater than end, it means we haven't found our target element
    if (start > end)
    {
        return -1;
    }
    //calculating mind element
    int mid = start + (end - start) / 2;

    if (arr[mid] == target)
    {
        return mid;
    }
    if (arr[mid] > target)
    {
        return search(arr, n, start, mid - 1, target);
    }
    else
    {
        return search(arr, n, mid + 1, end, target);
    }
}

//main starts

{
    // Write C++ code here
    int arr[] = {1, 2, 3, 4, 5, 6};
    int n = sizeof(arr) / sizeof(arr[0]);
    int target = 6;
    int start = 0;
    int end = n - 1;
    int ind = search(arr, n, start, end, target);
    cout << "Element found at index: " << ind;
    return 0;
}

/*
Sample input:
arr=[1,2,3,4,5,6]
target=6;

Output:
Element found at index: 5
*/


{
    //if start is greater than end, it means we haven't found our target element
    if (start > end)
    {
        return -1;
    }
    //calculating mind element
    int mid = start + (end - start) / 2;

    if (arr[mid] == target)
    {
        return mid;
    }
    if (arr[mid] > target)
    {
        return search(arr, n, start, mid - 1, target);
    }
    else
    {
        return search(arr, n, mid + 1, end, target);
    }
}

/*
Description: Binary search using recursion

Approach: To use recursion, for updating start and end variables.

Time Complexity: O(log n)
*/

#include <iostream>
using namespace std;

//function starts
int search(int arr[], int n, int start, int end, int target)

//main starts
int main()
{
    // Write C++ code here
    int arr[] = {1, 2, 3, 4, 5, 6};
    int n = sizeof(arr) / sizeof(arr[0]);
    int target = 6;
    int start = 0;
    int end = n - 1;
    int ind = search(arr, n, start, end, target);
    cout << "Element found at index: " << ind;
    return 0;
}

/*
Sample input:
arr=[1,2,3,4,5,6]
target=6;

Output:
Element found at index: 5
*/


/*
Description: Binary search using recursion

Approach: To use recursion, for updating start and end variables.

Time Complexity: O(log n)
*/

#include <iostream>
using namespace std;

//function starts
int search(int arr[], int n, int start, int end, int target)
{
    //if start is greater than end, it means we haven't found our target element
    if (start > end)
    {
        return -1;
    }
    //calculating mind element
    int mid = start + (end - start) / 2;

    if (arr[mid] == target)
    {
        return mid;
    }
    if (arr[mid] > target)
    {
        return search(arr, n, start, mid - 1, target);
    }
    else
    {
        return search(arr, n, mid + 1, end, target);
    }
}

//main starts
int main()
{
    // Write C++ code here
    int arr[] = {1, 2, 3, 4, 5, 6};
    int n = sizeof(arr) / sizeof(arr[0]);
    int target = 6;
    int start = 0;
    in

    int ind = search(arr, n, start, end, target);
    cout << "Element found at index: " << ind;
    return 0;
}

/*
Sample input:
arr=[1,2,3,4,5,6]
target=6;

Output:
Element found at index: 5
*/


int search(int arr[], int n, int start, int end, int target)
{
    //if start is greater than end, it means we haven't found our target element
    if (start > end)
    {
        return -1;
    }
    //calculating mind element
    int mid = start + (end - start) / 2;

    if (arr[mid] == target)
    {
        return mid;
    }
    if (arr[mid] > target)
    {
        return search(arr, n, start, mid - 1, target);
    }
    else
    {
        return search(arr, n, mid + 1, end, target);
    }
}

/*
Description: Binary search using recursion

Approach: To use recursion, for updating start and end variables.

Time Complexity: O(log n)
*/

#include <iostream>
using namespace std;

//function starts


void special_keys(int a_keys, int x, int y)

#include <GL/glut.h>   // The GL Utility Toolkit (GLUT) Header

bool g_gamemode;       // GLUT GameMode ON/OFF
bool g_fullscreen;     // Fullscreen Mode ON/OFF (When g_gamemode Is OFF)

// Our GL Specific Initializations
bool init(void)
{
	glShadeModel(GL_SMOOTH);							// Enable Smooth Shading
	glClearColor(0.0f, 0.0f, 0.0f, 0.5f);				// Black Background
	glClearDepth(1.0f);									// Depth Buffer Setup
	glEnable(GL_DEPTH_TEST);							// Enables Depth Testing
	glDepthFunc(GL_LEQUAL);								// The Type Of Depth Testing To Do
	glHint(GL_PERSPECTIVE_CORRECTION_HINT, GL_NICEST);	// Really Nice Perspective Calculations
	return true;
}

// Our Rendering Is Done Here
void render(void)
{
    glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);	// Clear Screen And Depth Buffer
    glLoadIdentity();									// Reset The Current Modelview Matrix
glTranslatef(-1.5f,0.0f,-6.0f);						// Move Left 1.5 Units And Into The Screen 6.0
	glBegin(GL_TRIANGLES);								// Drawing Using Triangles
		glColor3f(1.0f,0.0f,0.0f);						// Set The Color To Red
		glVertex3f( 0.0f, 1.0f, 0.0f);					// Top
		glColor3f(0.0f,1.0f,0.0f);						// Set The Color To Green
		glVertex3f(-1.0f,-1.0f, 0.0f);					// Bottom Left
		glColor3f(0.0f,0.0f,1.0f);						// Set The Color To Blue
		glVertex3f( 1.0f,-1.0f, 0.0f);					// Bottom Right
	glEnd();											// Finished Drawing The Triangle
	glTranslatef(3.0f,0.0f,0.0f);						// Move Right 3 Units
	glColor3f(0.5f,0.5f,1.0f);							// Set The Color To Blue One Time Only
	glBegin(GL_QUADS);									// Draw A Quad
		glVertex3f(-1.0f, 1.0f, 0.0f);					// Top Left
		glVertex3f( 1.0f, 1.0f, 0.0f);					// Top Right
		glVertex3f( 1.0f,-1.0f, 0.0f);					// Bottom Right
		glVertex3f(-1.0f,-1.0f, 0.0f);					// Bottom Left
	glEnd();											// Done Drawing The Quad
    // Swap The Buffers To Become Our Rendering Visible
    glutSwapBuffers ( );
}

// Our Reshaping Handler (Required Even In Fullscreen-Only Modes)
void reshape(int w, int h)
{
	glViewport(0, 0, w, h);
	glMatrixMode(GL_PROJECTION);     // Select The Projection Matrix
	glLoadIdentity();                // Reset The Projection Matrix
	// Calculate The Aspect Ratio And Set The Clipping Volume
	if (h == 0) h = 1;
	gluPerspective(80, (float)w/(float)h, 1.0, 5000.0);
	glMatrixMode(GL_MODELVIEW);      // Select The Modelview Matrix
	glLoadIdentity();                // Reset The Modelview Matrix
}

// Our Keyboard Handler (Normal Keys)
void keyboard(unsigned char key, int x, int y)
{
	switch (key) {
		case 27:        // When Escape Is Pressed...
			exit(0);    // Exit The Program
		break;          // Ready For Next Case
		default:        // Now Wrap It Up
		break;
	}
}

// Our Keyboard Handler For Special Keys (Like Arrow Keys And Function Keys)

{
	switch (a_keys) {
		case GLUT_KEY_F1:
			// We Can Switch Between Windowed Mode And Fullscreen Mode Only
			if (!g_gamemode) {
				g_fullscreen = !g_fullscreen;       // Toggle g_fullscreen Flag
				if (g_fullscreen) glutFullScreen(); // We Went In Fullscreen Mode
				else glutReshapeWindow(500, 500);   // We Went In Windowed Mode
			}
		break;
		default:
		break;
	}
}

// Main Function For Bringing It All Together.
int main(int argc, char** argv)
{

	glutInit(&argc, argv);                           // GLUT Initializtion
	glutInitDisplayMode(GLUT_RGB | GLUT_DOUBLE);     // Display Mode (Rgb And Double Buffered)
	glutInitWindowSize(500, 500);                // Window Size If We Start In Windowed Mode
	glutCreateWindow("NeHe's OpenGL Framework"); // Window Title

	init();                                          // Our Initialization
	glutDisplayFunc(render);                         // Register The Display Function
	glutReshapeFunc(reshape);                        // Register The Reshape Handler
	glutKeyboardFunc(keyboard);                      // Register The Keyboard Handler
	glutSpecialFunc(special_keys);                   // Register Special Keys Handler
	glutMainLoop();                                  // Go To GLUT Main Loop
	return 0;
}


{
	glViewport(0, 0, w, h);
	glMatrixMode(GL_PROJECTION);     // Select The Projection Matrix
	glLoadIdentity();                // Reset The Projection Matrix
	// Calculate The Aspect Ratio And Set The Clipping Volume
	if (h == 0) h = 1;
	gluPerspective(80, (float)w/(float)h, 1.0, 5000.0);
	glMatrixMode(GL_MODELVIEW);      // Select The Modelview Matrix
	glLoadIdentity();                // Reset The Modelview Matrix
}

#include <GL/glut.h>   // The GL Utility Toolkit (GLUT) Header

bool g_gamemode;       // GLUT GameMode ON/OFF
bool g_fullscreen;     // Fullscreen Mode ON/OFF (When g_gamemode Is OFF)

// Our GL Specific Initializations
bool init(void)
{
	glShadeModel(GL_SMOOTH);							// Enable Smooth Shading
	glClearColor(0.0f, 0.0f, 0.0f, 0.5f);				// Black Background
	glClearDepth(1.0f);									// Depth Buffer Setup
	glEnable(GL_DEPTH_TEST);							// Enables Depth Testing
	glDepthFunc(GL_LEQUAL);								// The Type Of Depth Testing To Do
	glHint(GL_PERSPECTIVE_CORRECTION_HINT, GL_NICEST);	// Really Nice Perspective Calculations
	return true;
}

// Our Rendering Is Done Here
void render(void)
{
    glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);	// Clear Screen And Depth Buffer
    glLoadIdentity();									// Reset The Current Modelview Matrix
glTranslatef(-1.5f,0.0f,-6.0f);						// Move Left 1.5 Units And Into The Screen 6.0
	glBegin(GL_TRIANGLES);								// Drawing Using Triangles
		glColor3f(1.0f,0.0f,0.0f);						// Set The Color To Red
		glVertex3f( 0.0f, 1.0f, 0.0f);					// Top
		glColor3f(0.0f,1.0f,0.0f);						// Set The Color To Green
		glVertex3f(-1.0f,-1.0f, 0.0f);					// Bottom Left
		glColor3f(0.0f,0.0f,1.0f);						// Set The Color To Blue
		glVertex3f( 1.0f,-1.0f, 0.0f);					// Bottom Right
	glEnd();											// Finished Drawing The Triangle
	glTranslatef(3.0f,0.0f,0.0f);						// Move Right 3 Units
	glColor3f(0.5f,0.5f,1.0f);							// Set The Color To Blue One Time Only
	glBegin(GL_QUADS);									// Draw A Quad
		glVertex3f(-1.0f, 1.0f, 0.0f);					// Top Left
		glVertex3f( 1.0f, 1.0f, 0.0f);					// Top Right
		glVertex3f( 1.0f,-1.0f, 0.0f);					// Bottom Right
		glVertex3f(-1.0f,-1.0f, 0.0f);					// Bottom Left
	glEnd();											// Done Drawing The Quad
    // Swap The Buffers To Become Our Rendering Visible
    glutSwapBuffers ( );
}

// Our Reshaping Handler (Required Even In Fullscreen-Only Modes)
void reshape(int w, int h)

// Our Keyboard Handler (Normal Keys)
void keyboard(unsigned char key, int x, int y)
{
	switch (key) {
		case 27:        // When Escape Is Pressed...
			exit(0);    // Exit The Program
		break;          // Ready For Next Case
		default:        // Now Wrap It Up
		break;
	}
}

// Our Keyboard Handler For Special Keys (Like Arrow Keys And Function Keys)
void special_keys(int a_keys, int x, int y)
{
	switch (a_keys) {
		case GLUT_KEY_F1:
			// We Can Switch Between Windowed Mode And Fullscreen Mode Only
			if (!g_gamemode) {
				g_fullscreen = !g_fullscreen;       // Toggle g_fullscreen Flag
				if (g_fullscreen) glutFullScreen(); // We Went In Fullscreen Mode
				else glutReshapeWindow(500, 500);   // We Went In Windowed Mode
			}
		break;
		default:
		break;
	}
}

// Main Function For Bringing It All Together.
int main(int argc, char** argv)
{

	glutInit(&argc, argv);                           // GLUT Initializtion
	glutInitDisplayMode(GLUT_RGB | GLUT_DOUBLE);     // Display Mode (Rgb And Double Buffered)
	glutInitWindowSize(500, 500);                // Window Size If We Start In Windowed Mode
	glutCreateWindow("NeHe's OpenGL Framework"); // Window Title

	init();                                          // Our Initialization
	glutDisplayFunc(render);                         // Register The Display Function
	glutReshapeFunc(reshape);                        // Register The Reshape Handler
	glutKeyboardFunc(keyboard);                      // Register The Keyboard Handler
	glutSpecialFunc(special_keys);                   // Register Special Keys Handler
	glutMainLoop();                                  // Go To GLUT Main Loop
	return 0;
}


#include <GL/glut.h>   // The GL Utility Toolkit (GLUT) Header

bool g_gamemode;       // GLUT GameMode ON/OFF
bool g_fullscreen;     // Fullscreen Mode ON/OFF (When g_gamemode Is OFF)

// Our GL Specific Initializations
bool init(void)
{
	glShadeModel(GL_SMOOTH);							// Enable Smooth Shading
	glClearColor(0.0f, 0.0f, 0.0f, 0.5f);				// Black Background
	glClearDepth(1.0f);									// Depth Buffer Setup
	glEnable(GL_DEPTH_TEST);							// Enables Depth Testing
	glDepthFunc(GL_LEQUAL);								// The Type Of Depth Testing To Do
	glHint(GL_PERSPECTIVE_CORRECTION_HINT, GL_NICEST);	// Really Nice Perspective Calculations
	return true;
}

// Our Rendering Is Done Here
void render(void)
{
    glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);	// Clear Screen And Depth Buffer
    glLoadIdentity();									// Reset The Current Modelview Matrix
glTranslatef(-1.5f,0.0f,-6.0f);						// Move Left 1.5 Units And Into The Screen 6.0
	glBegin(GL_TRIANGLES);								// Drawing Using Triangles
		glColor3f(1.0f,0.0f,0.0f);						// Set The Color To Red
		glVertex3f( 0.0f, 1.0f, 0.0f);					// Top
		glColor3f(0.0f,1.0f,0.0f);						// Set The Color To Green
		glVertex3f(-1.0f,-1.0f, 0.0f);					// Bottom Left
		glColor3f(0.0f,0.0f,1.0f);						// Set The Color To Blue
		glVertex3f( 1.0f,-1.0f, 0.0f);					// Bottom Right
	glEnd();											// Finished Drawing The Triangle
	glTranslatef(3.0f,0.0f,0.0f);						// Move Right 3 Units
	glColor3f(0.5f,0.5f,1.0f);							// Set The Color To Blue One Time Only
	glBegin(GL_QUADS);									// Draw A Quad
		glVertex3f(-1.0f, 1.0f, 0.0f);					// Top Left
		glVertex3f( 1.0f, 1.0f, 0.0f);					// Top Right
		glVertex3f( 1.0f,-1.0f, 0.0f);					// Bottom Right
		glVertex3f(-1.0f,-1.0f, 0.0f);					// Bottom Left
	glEnd();											// Done Drawing The Quad
    // Swap The Buffers To Become Our Rendering Visible
    glutSwapBuffers ( );
}

// Our Reshaping Handler (Required Even In Fullscreen-Only Modes)
void reshape(int w, int h)
{
	glViewport(0, 0, w, h);
	glMatrixMode(GL_PROJECTION);     // Select The Projection Matrix
	glLoadIdentity();                // Reset The Projection Matrix
	// Calculate The Aspect Ratio And Set The Clipping Volume
	if (h == 0) h = 1;
	gluPerspective(80, (float)w/(float)h, 1.0, 5000.0);
	glMatrixMode(GL_MODELVIEW);      // Select The Modelview Matrix
	glLoadIdentity();                // Reset The Modelview Matrix
}

// Our Keyboard Handler (Normal Keys)
void keyboard(unsigned char key, int x, int y)
{
	switch (key) {
		case 27:        // When Escape Is Pressed...
			exit(0);    // Exit The Program
		break;          // Ready For Next Case
		default:        // Now Wrap It Up
		break;
	}
}

// Our Keyboard Handler For Special Keys (Like Arrow Keys And Function Keys)
void special_keys(int a_keys, int x, int y)
{
	switch (a_keys) {
		case GLUT_KEY_F1:
			// We Can Switch Between Windowed Mode And Fullscreen Mode Only
			if (!g_gamemode) {
				g_fullscreen = !g_fullscreen;       // Toggle g_fullscreen Flag
				if (g_fullscreen) glutFullScreen(); // We Went In Fullscreen Mode
				else glutReshapeWindow(500, 500);   // We Went In Windowed Mode
			}
		break;
		default:
		break;
	}
}

// Main Function For Bringing It All Together.
int main(int argc, char** argv)
{

	glutInit(&argc, argv);                           // GLUT Initializtion
	glutInitDisplayMode(GLUT_RGB | GLUT_DOUBLE);     // Display Mode (Rgb And Double Buffered)
	glutInitWindowSize(500, 500);                // Window Size If We Start In Windowed Mode
	glutCreateWindow("NeHe's OpenGL Framework"); // Window Title

	init();                                          // Our Initialization
	glutDisplayFunc(render);                         // Register The Display Function
	glutReshape

                        // Register The Reshape Handler
	glutKeyboardFunc(keyboard);                      // Register The Keyboard Handler
	glutSpecialFunc(special_keys);                   // Register Special Keys Handler
	glutMainLoop();                                  // Go To GLUT Main Loop
	return 0;
}


bool init(void)
{
	glShadeModel(GL_SMOOTH);							// Enable Smooth Shading
	glClearColor(0.0f, 0.0f, 0.0f, 0.5f);				// Black Background
	glClearDepth(1.0f);									// Depth Buffer Setup
	glEnable(GL_DEPTH_TEST);							// Enables Depth Testing
	glDepthFunc(GL_LEQUAL);								// The Type Of Depth Testing To Do
	glHint(GL_PERSPECTIVE_CORRECTION_HINT, GL_NICEST);	// Really Nice Perspective Calculations
	return true;
}

#include <GL/glut.h>   // The GL Utility Toolkit (GLUT) Header

bool g_gamemode;       // GLUT GameMode ON/OFF
bool g_fullscreen;     // Fullscreen Mode ON/OFF (When g_gamemode Is OFF)

// Our GL Specific Initializations

// Our Rendering Is Done Here
void render(void)
{
    glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);	// Clear Screen And Depth Buffer
    glLoadIdentity();									// Reset The Current Modelview Matrix
glTranslatef(-1.5f,0.0f,-6.0f);						// Move Left 1.5 Units And Into The Screen 6.0
	glBegin(GL_TRIANGLES);								// Drawing Using Triangles
		glColor3f(1.0f,0.0f,0.0f);						// Set The Color To Red
		glVertex3f( 0.0f, 1.0f, 0.0f);					// Top
		glColor3f(0.0f,1.0f,0.0f);						// Set The Color To Green
		glVertex3f(-1.0f,-1.0f, 0.0f);					// Bottom Left
		glColor3f(0.0f,0.0f,1.0f);						// Set The Color To Blue
		glVertex3f( 1.0f,-1.0f, 0.0f);					// Bottom Right
	glEnd();											// Finished Drawing The Triangle
	glTranslatef(3.0f,0.0f,0.0f);						// Move Right 3 Units
	glColor3f(0.5f,0.5f,1.0f);							// Set The Color To Blue One Time Only
	glBegin(GL_QUADS);									// Draw A Quad
		glVertex3f(-1.0f, 1.0f, 0.0f);					// Top Left
		glVertex3f( 1.0f, 1.0f, 0.0f);					// Top Right
		glVertex3f( 1.0f,-1.0f, 0.0f);					// Bottom Right
		glVertex3f(-1.0f,-1.0f, 0.0f);					// Bottom Left
	glEnd();											// Done Drawing The Quad
    // Swap The Buffers To Become Our Rendering Visible
    glutSwapBuffers ( );
}

// Our Reshaping Handler (Required Even In Fullscreen-Only Modes)
void reshape(int w, int h)
{
	glViewport(0, 0, w, h);
	glMatrixMode(GL_PROJECTION);     // Select The Projection Matrix
	glLoadIdentity();                // Reset The Projection Matrix
	// Calculate The Aspect Ratio And Set The Clipping Volume
	if (h == 0) h = 1;
	gluPerspective(80, (float)w/(float)h, 1.0, 5000.0);
	glMatrixMode(GL_MODELVIEW);      // Select The Modelview Matrix
	glLoadIdentity();                // Reset The Modelview Matrix
}

// Our Keyboard Handler (Normal Keys)
void keyboard(unsigned char key, int x, int y)
{
	switch (key) {
		case 27:        // When Escape Is Pressed...
			exit(0);    // Exit The Program
		break;          // Ready For Next Case
		default:        // Now Wrap It Up
		break;
	}
}

// Our Keyboard Handler For Special Keys (Like Arrow Keys And Function Keys)
void special_keys(int a_keys, int x, int y)
{
	switch (a_keys) {
		case GLUT_KEY_F1:
			// We Can Switch Between Windowed Mode And Fullscreen Mode Only
			if (!g_gamemode) {
				g_fullscreen = !g_fullscreen;       // Toggle g_fullscreen Flag
				if (g_fullscreen) glutFullScreen(); // We Went In Fullscreen Mode
				else glutReshapeWindow(500, 500);   // We Went In Windowed Mode
			}
		break;
		default:
		break;
	}
}

// Main Function For Bringing It All Together.
int main(int argc, char** argv)
{

	glutInit(&argc, argv);                           // GLUT Initializtion
	glutInitDisplayMode(GLUT_RGB | GLUT_DOUBLE);     // Display Mode (Rgb And Double Buffered)
	glutInitWindowSize(500, 500);                // Window Size If We Start In Windowed Mode
	glutCreateWindow("NeHe's OpenGL Framework"); // Window Title

	init();                                          // Our Initialization
	glutDisplayFunc(render);                         // Register The Display Function
	glutReshapeFunc(reshape);                        // Register The Reshape Handler
	glutKeyboardFunc(keyboard);                      // Register The Keyboard Handler
	glutSpecialFunc(special_keys);                   // Register Special Keys Handler
	glutMainLoop();                                  // Go To GLUT Main Loop
	return 0;
}


switch (key) {
		case 27:        // When Escape Is Pressed...
			exit(0);    // Exit The Program
		break;          // Ready For Next Case
		default:        // Now Wrap It Up
		break;
	}

#include <GL/glut.h>   // The GL Utility Toolkit (GLUT) Header

bool g_gamemode;       // GLUT GameMode ON/OFF
bool g_fullscreen;     // Fullscreen Mode ON/OFF (When g_gamemode Is OFF)

// Our GL Specific Initializations
bool init(void)
{
	glShadeModel(GL_SMOOTH);							// Enable Smooth Shading
	glClearColor(0.0f, 0.0f, 0.0f, 0.5f);				// Black Background
	glClearDepth(1.0f);									// Depth Buffer Setup
	glEnable(GL_DEPTH_TEST);							// Enables Depth Testing
	glDepthFunc(GL_LEQUAL);								// The Type Of Depth Testing To Do
	glHint(GL_PERSPECTIVE_CORRECTION_HINT, GL_NICEST);	// Really Nice Perspective Calculations
	return true;
}

// Our Rendering Is Done Here
void render(void)
{
    glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);	// Clear Screen And Depth Buffer
    glLoadIdentity();									// Reset The Current Modelview Matrix
glTranslatef(-1.5f,0.0f,-6.0f);						// Move Left 1.5 Units And Into The Screen 6.0
	glBegin(GL_TRIANGLES);								// Drawing Using Triangles
		glColor3f(1.0f,0.0f,0.0f);						// Set The Color To Red
		glVertex3f( 0.0f, 1.0f, 0.0f);					// Top
		glColor3f(0.0f,1.0f,0.0f);						// Set The Color To Green
		glVertex3f(-1.0f,-1.0f, 0.0f);					// Bottom Left
		glColor3f(0.0f,0.0f,1.0f);						// Set The Color To Blue
		glVertex3f( 1.0f,-1.0f, 0.0f);					// Bottom Right
	glEnd();											// Finished Drawing The Triangle
	glTranslatef(3.0f,0.0f,0.0f);						// Move Right 3 Units
	glColor3f(0.5f,0.5f,1.0f);							// Set The Color To Blue One Time Only
	glBegin(GL_QUADS);									// Draw A Quad
		glVertex3f(-1.0f, 1.0f, 0.0f);					// Top Left
		glVertex3f( 1.0f, 1.0f, 0.0f);					// Top Right
		glVertex3f( 1.0f,-1.0f, 0.0f);					// Bottom Right
		glVertex3f(-1.0f,-1.0f, 0.0f);					// Bottom Left
	glEnd();											// Done Drawing The Quad
    // Swap The Buffers To Become Our Rendering Visible
    glutSwapBuffers ( );
}

// Our Reshaping Handler (Required Even In Fullscreen-Only Modes)
void reshape(int w, int h)
{
	glViewport(0, 0, w, h);
	glMatrixMode(GL_PROJECTION);     // Select The Projection Matrix
	glLoadIdentity();                // Reset The Projection Matrix
	// Calculate The Aspect Ratio And Set The Clipping Volume
	if (h == 0) h = 1;
	gluPerspective(80, (float)w/(float)h, 1.0, 5000.0);
	glMatrixMode(GL_MODELVIEW);      // Select The Modelview Matrix
	glLoadIdentity();                // Reset The Modelview Matrix
}

// Our Keyboard Handler (Normal Keys)
void keyboard(unsigned char key, int x, int y)
{

}

// Our Keyboard Handler For Special Keys (Like Arrow Keys And Function Keys)
void special_keys(int a_keys, int x, int y)
{
	switch (a_keys) {
		case GLUT_KEY_F1:
			// We Can Switch Between Windowed Mode And Fullscreen Mode Only
			if (!g_gamemode) {
				g_fullscreen = !g_fullscreen;       // Toggle g_fullscreen Flag
				if (g_fullscreen) glutFullScreen(); // We Went In Fullscreen Mode
				else glutReshapeWindow(500, 500);   // We Went In Windowed Mode
			}
		break;
		default:
		break;
	}
}

// Main Function For Bringing It All Together.
int main(int argc, char** argv)
{

	glutInit(&argc, argv);                           // GLUT Initializtion
	glutInitDisplayMode(GLUT_RGB | GLUT_DOUBLE);     // Display Mode (Rgb And Double Buffered)
	glutInitWindowSize(500, 500);                // Window Size If We Start In Windowed Mode
	glutCreateWindow("NeHe's OpenGL Framework"); // Window Title

	init();                                          // Our Initialization
	glutDisplayFunc(render);                         // Register The Display Function
	glutReshapeFunc(reshape);                        // Register The Reshape Handler
	glutKeyboardFunc(keyboard);                      // Register The Keyboard Handler
	glutSpecialFunc(special_keys);                   // Register Special Keys Handler
	glutMainLoop();                                  // Go To GLUT Main Loop
	return 0;
}


vector<vector<int>> floodFill(vector<vector<int>>& image, int sr, int sc, int newColor)

//Problem statement
// An image is represented by a 2-D array of integers,
// each integer representing the pixel value of the image (from 0 to 65535).
// Given a coordinate (sr, sc) representing the starting pixel
// (row and column) of the flood fill, and a pixel value newColor, "flood fill" the image.
// To perform a "flood fill", consider the starting pixel,
// plus any pixels connected 4-directionally to the starting pixel of the same
//  color as the starting pixel, plus any pixels connected 4-directionally to those
//  pixels (also with the same color as the starting pixel), and so on. Replace the
//  color of all of the aforementioned pixels with the newColor.
// At the end, return the modified image.

#include<bits/stdc++.h>
using namespace std;

{

        if(image[sr][sc] == newColor)
            return image;

        queue<int> qx;
        queue<int> qy;

        int rw = image.size();
        int clm = image[0].size();

        qx.push(sr);
        qy.push(sc);

        int curr = image[sr][sc];
        image[sr][sc] = newColor;

        while( !qx.empty() ) {

            int x = qx.front();
            int y = qy.front();

            qx.pop();
            qy.pop();

            int arrx[] = {-1, 0, 1, 0};
            int arry[] = {0, 1, 0, -1};

            for(int i=0; i<4; i++) {

                int tempx = x + arrx[i];
                int tempy = y + arry[i];

                if(tempx <0 || tempy < 0) continue;
                if(tempx >=rw || tempy >= clm) continue;

                if(image[tempx][tempy] == curr){
                    qx.push(tempx);
                    qy.push(tempy);
                    image[tempx][tempy] = newColor;
                }
            }
        }

        return image;
    }


//Problem statement
// An image is represented by a 2-D array of integers,
// each integer representing the pixel value of the image (from 0 to 65535).
// Given a coordinate (sr, sc) representing the starting pixel
// (row and column) of the flood fill, and a pixel value newColor, "flood fill" the image.
// To perform a "flood fill", consider the starting pixel,
// plus any pixels connected 4-directionally to the starting pixel of the same
//  color as the starting pixel, plus any pixels connected 4-directionally to those
//  pixels (also with the same color as the starting pixel), and so on. Replace the
//  color of all of the aforementioned pixels with the newColor.
// At the end, return the modified image.

#include<bits/stdc++.h>
using namespace std;

 vector<vector<int>> floodFill(vector<vector<int>>& image, int sr, int sc, int newColor) {

        if(image[sr][sc] == newColor)
            return image;

        q

        queue<int> qy;

        int rw = image.size();
        int clm = image[0].size();

        qx.push(sr);
        qy.push(sc);

        int curr = image[sr][sc];
        image[sr][sc] = newColor;

        while( !qx.empty() ) {

            int x = qx.front();
            int y = qy.front();

            qx.pop();
            qy.pop();

            int arrx[] = {-1, 0, 1, 0};
            int arry[] = {0, 1, 0, -1};

            for(int i=0; i<4; i++) {

                int tempx = x + arrx[i];
                int tempy = y + arry[i];

                if(tempx <0 || tempy < 0) continue;
                if(tempx >=rw || tempy >= clm) continue;

                if(image[tempx][tempy] == curr){
                    qx.push(tempx);
                    qy.push(tempy);
                    image[tempx][tempy] = newColor;
                }
            }
        }

        return image;
    }


void imu_callback(const sensor_msgs::Imu& imu_msg)

// third party
#include <nav_msgs/Path.h>
#include <pcl/console/parse.h>
#include <pcl/conversions.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl_conversions/pcl_conversions.h>
#include <ros/ros.h>
#include <sensor_msgs/PointCloud.h>
#include <sensor_msgs/PointCloud2.h>
#include <std_msgs/String.h>
#include <visualization_msgs/Marker.h>
// stl
#include <unistd.h>
#include <sstream>
// local
#include "builder/map_builder.h"
#include "common/file_utils.h"
#include "common/performance/simple_prof.h"
#include "common/simple_time.h"
#include "ros_node/kitti_reader.h"
#include "ros_node/msg_conversion.h"
#include "ros_node/playable_bag.h"
#include "ros_node/tf_bridge.h"
#include "tf/transform_broadcaster.h"

using static_map::MapBuilder;
static MapBuilder::Ptr map_builder;

using static_map::data::ImuMsg;
using static_map::data::NavSatFixMsg;
using static_map::data::OdomMsg;

namespace {

constexpr char kMapFrameId[] = "/map";

void pointcloud_callback(const sensor_msgs::PointCloud2::ConstPtr& msg) {
  pcl::PCLPointCloud2 pcl_pc2;
  pcl_conversions::toPCL(*msg, pcl_pc2);
  MapBuilder::PointCloudPtr incoming_cloud(new MapBuilder::PointCloudType);
  pcl::fromPCLPointCloud2(pcl_pc2, *incoming_cloud);

  std::vector<int> inliers;  // no use, just for the function
  pcl::removeNaNFromPointCloud(*incoming_cloud, *incoming_cloud, inliers);
  map_builder->InsertPointcloudMsg(incoming_cloud);
}

{
  ImuMsg::Ptr incomming_imu(new ImuMsg);
  *incomming_imu = static_map_ros::ToLocalImu(imu_msg);
  map_builder->InsertImuMsg(incomming_imu);
}

void odom_callback(const nav_msgs::Odometry& odom_msg) {
  OdomMsg::Ptr local_odom(new OdomMsg);
  *local_odom = static_map_ros::ToLocalOdom(odom_msg);
  map_builder->InsertOdomMsg(local_odom);
}

void gps_callback(const sensor_msgs::NavSatFix& gps_msg) {
  NavSatFixMsg::Ptr local_gps(new NavSatFixMsg);
  *local_gps = static_map_ros::ToLocalNavSatMsg(gps_msg);
  map_builder->InsertGpsMsg(local_gps);
}

}  // namespace

int main(int argc, char** argv) {
  ros::init(argc, argv, "static_mapping_node");
  ros::NodeHandle n;

  SIMPLE_PROF_USE_MS;
  SIMPLE_PROF_MERGE_OUTPUT;

  google::InitGoogleLogging(argv[0]);
  FLAGS_alsologtostderr = true;
  FLAGS_colorlogtostderr = true;
  google::InstallFailureSignalHandler();

  // parse auguements
  // point cloud
  std::string point_cloud_topic = "";
  pcl::console::parse_argument(argc, argv, "-pc", point_cloud_topic);
  if (point_cloud_topic.empty()) {
    PRINT_ERROR("point cloud topic is empty!");
    return -1;
  } else {
    PRINT_INFO_FMT("Get point cloud from ROS topic: %s",
                   point_cloud_topic.c_str());
  }
  ros::Subscriber sub_pointcloud =
      n.subscribe(point_cloud_topic, 10, pointcloud_callback);
  std::string cloud_frame_id = "base_link";
  pcl::console::parse_argument(argc, argv, "-pc_frame_id", cloud_frame_id);

  // imu
  std::string imu_topic = "";
  bool use_imu = false;
  ros::Subscriber sub_imu;
  std::string imu_frame_id = "/novatel_imu";
  pcl::console::parse_argument(argc, argv, "-imu", imu_topic);
  if (imu_topic.empty()) {
    PRINT_WARNING("No imu topic, expect no imu messages.");
  } else {
    PRINT_INFO_FMT("Get imu data from ROS topic: %s", imu_topic.c_str());
    sub_imu = n.subscribe(imu_topic, 100, imu_callback);
    pcl::console::parse_argument(argc, argv, "-imu_frame_id", imu_frame_id);
    use_imu = true;
  }

  // odom
  bool use_odom = false;
  std::string odom_topic = "";
  std::string odom_frame_id = "";
  pcl::console::parse_argument(argc, argv, "-odom", odom_topic);
  pcl::console::parse_argument(argc, argv, "-odom_frame_id", odom_frame_id);
  ros::Subscriber sub_odom;
  if (!odom_topic.empty() && !odom_frame_id.empty()) {
    sub_odom = n.subscribe(odom_topic, 100, odom_callback);
    use_odom = true;
    PRINT_INFO_FMT("Get odom data from ROS topic: %s", odom_topic.c_str());
  } else {
    PRINT_WARNING("No odom data accessed!");
  }

  // gps
  bool use_gps = false;
  std::string gps_topic = "";
  std::string gps_frame_id = "";
  pcl::console::parse_argument(argc, argv, "-gps", gps_topic);
  pcl::console::parse_argument(argc, argv, "-gps_frame_id", gps_frame_id);
  ros::Subscriber gps_subscriber;
  if (!gps_topic.empty() && !gps_frame_id.empty()) {
    gps_subscriber = n.subscribe(gps_topic, 100, gps_callback);
    use_gps = true;
    PRINT_INFO_FMT("Get gps data from ROS topic: %s", gps_topic.c_str());
  } else {
    PRINT_WARNING("No GPS data accessed!");
  }

  // config file
  std::string config_file = "";
  pcl::console::parse_argument(argc, argv, "-cfg", config_file);

  // urdf file
  std::string urdf_file = "";
  pcl::console::parse_argument(argc, argv, "-urdf", urdf_file);

  // tracking frame
  std::string tracking_frame = "base_link";
  pcl::console::parse_argument(argc, argv, "-track", tracking_frame);
  CHECK(!tracking_frame.empty())
      << "tracking frame id should be set." << std::endl;

  // ros bag
  std::string bag_file = "";
  pcl::console::parse_argument(argc, argv, "-bag", bag_file);
  if (!bag_file.empty() && !static_map::common::FileExist(bag_file)) {
    PRINT_WARNING_FMT("%s does not exist.", bag_file.c_str());
    bag_file = "";
  }

  // kitti data set
  std::string kitti_path = "";
  pcl::console::parse_argument(argc, argv, "-kitti", kitti_path);
  if (!kitti_path.empty() && !static_map::common::FileExist(kitti_path)) {
    PRINT_WARNING_FMT("%s does not exist.", kitti_path.c_str());
    kitti_path = "";
  }

  if (!kitti_path.empty() && !bag_file.empty()) {
    PRINT_ERROR("Should not read bag and kitti data set at the same time.");
    return -1;
  }

  std::string pub_topics = "";
  pcl::console::parse_argument(argc, argv, "-pubs", pub_topics);

  ros::Publisher map_publisher =
      n.advertise<sensor_msgs::PointCloud2>("/optimized_map", 1);
  ros::Publisher submap_publisher =
      n.advertise<sensor_msgs::PointCloud2>("/submap", 1);
  ros::Publisher pose_marker_pub =
      n.advertise<visualization_msgs::Marker>("/submap_pose", 1);
  ros::Publisher path_pub =
      n.advertise<nav_msgs::Path>("/static_mapping_path", 1);
  ros::Publisher edge_markers_pub =
      n.advertise<visualization_msgs::Marker>("/loop_edges", 1);
  tf::TransformBroadcaster tf_broadcaster;

  auto show_function =
      [&](const static_map::MapBuilder::PointCloudPtr& cloud) -> void {
    sensor_msgs::PointCloud2 map_pointcloud;
    pcl::toROSMsg(*cloud, map_pointcloud);
    map_pointcloud.header.frame_id = kMapFrameId;
    map_pointcloud.header.stamp = ros::Time::now();
    map_publisher.publish(map_pointcloud);
  };

  auto show_submap_function =
      [&](const static_map::MapBuilder::PointCloudPtr& cloud,
          const Eigen::Matrix4d& pose) -> void {
    tf::Transform transform;
    transform.setOrigin(tf::Vector3(pose(0, 3), pose(1, 3), pose(2, 3)));
    Eigen::Quaterniond rotation(Eigen::Matrix3d(pose.block(0, 0, 3, 3)));
    tf::Quaternion q;
    q.setW(rotation.w());
    q.setX(rotation.x());
    q.setY(rotation.y());
    q.setZ(rotation.z());
    transform.setRotation(q);
    tf_broadcaster.sendTransform(tf::StampedTransform(
        transform, ros::Time::now(), kMapFrameId, tracking_frame));

    sensor_msgs::PointCloud2 submap_pointcloud;
    pcl::toROSMsg(*cloud, submap_pointcloud);
    submap_pointcloud.header.frame_id = tracking_frame;
    submap_pointcloud.header.stamp = ros::Time::now();
    submap_publisher.publish(submap_pointcloud);
  };

  const auto publish_path = [&](const std::vector<Eigen::Matrix4d>& poses) {
    nav_msgs::Path path;
    path.header.frame_id = kMapFrameId;

    path.poses.reserve(poses.size());
    for (const Eigen::Matrix4d& pose : poses) {
      geometry_msgs::PoseStamped pose_ros;
      pose_ros.header.frame_id = kMapFrameId;
      Eigen::Quaterniond q(Eigen::Matrix3d(pose.block(0, 0, 3, 3)));
      pose_ros.pose.orientation.w = q.w();
      pose_ros.pose.orientation.x = q.x();
      pose_ros.pose.orientation.y = q.y();
      pose_ros.pose.orientation.z = q.z();
      pose_ros.pose.position.x = pose(0, 3);
      pose_ros.pose.position.y = pose(1, 3);
      pose_ros.pose.position.z = pose(2, 3);

      path.poses.push_back(pose_ros);
    }
    path_pub.publish(path);
  };

  const auto publish_edges =
      [&](const std::map<int64_t, static_map::back_end::ViewGraph::GraphItem>&
              graph) {
        visualization_msgs::Marker line_list;
        line_list.header.frame_id = kMapFrameId;
        line_list.id = 0;
        line_list.type = visualization_msgs::Marker::LINE_LIST;
        // LINE_STRIP/LINE_LIST markers use only the x component of scale, for
        // the line width
        line_list.scale.x = 0.1;
        // Line list is red
        line_list.color.b = 1.0;
        line_list.color.a = 1.0;
        // Create the vertices for the points and lines
        for (const auto& index_to_connected_item : graph) {
          if (index_to_connected_item.second.connections.size() <= 1) {
            continue;
          }
          const Eigen::Vector3d self_translation =
              index_to_connected_item.second.self_pose.block(0, 3, 3, 1);

          for (const auto& connection :
               index_to_connected_item.second.connections) {
            if (connection.first - index_to_connected_item.first == 1) {
              continue;
            }
            geometry_msgs::Point p_start;
            p_start.x = self_translation[0];
            p_start.y = self_translation[1];
            p_start.z = self_translation[2];

            line_list.points.push_back(p_start);

            geometry_msgs::Point p_end;
            const Eigen::Vector3d connected_translation =
                graph.at(connection.first).self_pose.block(0, 3, 3, 1);
            p_end.x = connected_translation[0];
            p_end.y = connected_translation[1];
            p_end.z = connected_translation[2];

            line_list.points.push_back(p_end);
          }
        }

        edge_markers_pub.publish(line_list);
      };

  map_builder = std::make_shared<MapBuilder>();

  // @todo(edward) merge these 2 situation into 1
  if (urdf_file.empty()) {
    tf::TransformListener listener;
    map_builder->SetTrackingToLidar(static_map_ros::LoopUpTransfrom(
        tracking_frame, cloud_frame_id, listener));
    if (use_imu) {
      map_builder->SetTrackingToImu(static_map_ros::LoopUpTransfrom(
          tracking_frame, imu_frame_id, listener));
    }
    if (use_odom) {
      map_builder->SetTransformOdomToLidar(static_map_ros::LoopUpTransfrom(
          odom_frame_id, cloud_frame_id, listener));
      map_builder->SetTrackingToOdom(static_map_ros::LoopUpTransfrom(
          tracking_frame, odom_frame_id, listener));
    }
    if (use_gps) {
      map_builder->SetTrackingToGps(static_map_ros::LoopUpTransfrom(
          tracking_frame, gps_frame_id, listener));
    }
  } else {
    tf2_ros::Buffer tf_buffer;
    static_map_ros::ReadStaticTransformsFromUrdf(urdf_file, &tf_buffer);
    map_builder->SetTrackingToLidar(static_map_ros::LoopUpTransfrom(
        tracking_frame, cloud_frame_id, tf_buffer));
    if (use_imu) {
      map_builder->SetTrackingToImu(static_map_ros::LoopUpTransfrom(
          tracking_frame, imu_frame_id, tf_buffer));
    }
    if (use_odom) {
      map_builder->SetTransformOdomToLidar(static_map_ros::LoopUpTransfrom(
          odom_frame_id, cloud_frame_id, tf_buffer));
      map_builder->SetTrackingToOdom(static_map_ros::LoopUpTransfrom(
          tracking_frame, odom_frame_id, tf_buffer));
    }
    if (use_gps) {
      map_builder->SetTrackingToGps(static_map_ros::LoopUpTransfrom(
          tracking_frame, gps_frame_id, tf_buffer));
    }
  }

  if (!config_file.empty()) {
    const auto options = map_builder->Initialise(config_file.c_str());
    if (options.front_end_options.imu_options.enabled && !use_imu) {
      PRINT_ERROR("You should set a imu topic if you enable using imu.");
      map_builder->FinishAllComputations();
      return 0;
    }
  } else {
    map_builder->Initialise(NULL);
  }
  map_builder->EnableUsingOdom(use_odom);
  map_builder->EnableUsingGps(use_gps);
  if (!pub_topics.empty()) {
    if (std::strstr(pub_topics.c_str(), "[map]")) {
      map_builder->SetShowMapFunction(show_function);
    }
    if (std::strstr(pub_topics.c_str(), "[submap]")) {
      map_builder->SetShowSubmapFunction(show_submap_function);
    }
    if (std::strstr(pub_topics.c_str(), "[path]")) {
      map_builder->SetShowPathFunction(publish_path);
    }
    if (std::strstr(pub_topics.c_str(), "[edge]")) {
      map_builder->SetShowEdgeFunction(publish_edges);
    }
  }

  REGISTER_FUNC;

  if (!bag_file.empty()) {
    static_map_ros::PlayableBag bag(bag_file, ros::TIME_MIN, ros::TIME_MAX,
                                    ::ros::Duration(1.0));

    const double duration_sec = bag.DurationInSeconds();
    const auto begin_time = std::get<0>(bag.GetBeginEndTime());

    int message_index = 0;
    ros::Time last_message_time = begin_time;
    while (bag.IsMessageAvailable() && ros::ok()) {
      auto message = bag.GetNextMessage();
      if (message.isType<sensor_msgs::PointCloud2>() &&
          message.getTopic() == point_cloud_topic) {
        pointcloud_callback(message.instantiate<sensor_msgs::PointCloud2>());
      } else if (message.isType<sensor_msgs::Imu>() &&
                 message.getTopic() == imu_topic) {
        if (use_imu) {
          imu_callback(*message.instantiate<sensor_msgs::Imu>());
        }
      } else if (message.isType<nav_msgs::Odometry>() &&
                 message.getTopic() == odom_topic) {
        if (use_odom) {
          odom_callback(*message.instantiate<nav_msgs::Odometry>());
        }
      } else if (message.isType<sensor_msgs::NavSatFix>() &&
                 message.getTopic() == gps_topic) {
        if (use_gps) {
          gps_callback(*message.instantiate<sensor_msgs::NavSatFix>());
        }
      }

      // read bag in twice time of the bag speed
      if (message.getTime() > last_message_time) {
        usleep((message.getTime() - last_message_time).toSec() * 0.1 * 1000000);
      }
      last_message_time = message.getTime();

      ++message_index;
      if (message_index % 10000 == 0) {
        PRINT_COLOR_FMT(BOLD, "processed %lf of %lf seconds of the bag... ",
                        (message.getTime() - begin_time).toSec(), duration_sec);
      }
    }
  } else if (!kitti_path.empty()) {
    PRINT_INFO("No bag, read kitti data.");
    static_map::KittiReader kitti_reader;
    kitti_reader.SetPointCloudDataPath(kitti_path);
    int index = 0;
    while (true && ros::ok()) {
      MapBuilder::PointCloudPtr point_cloud = kitti_reader.ReadFromBin(index);
      if (point_cloud && !point_cloud->empty()) {
        point_cloud->header.frame_id = cloud_frame_id;
        point_cloud->header.stamp =
            static_map::SimpleTime::GetCurrentTime().ToNanoSec() / 1000ull;
        map_builder->InsertPointcloudMsg(point_cloud);

        usleep(100000);
      } else {
        break;
      }
      index++;
    }
  } else {
    ::ros::spin();
  }

  map_builder->FinishAllComputations();
  return 0;
}


{
  OdomMsg::Ptr local_odom(new OdomMsg);
  *local_odom = static_map_ros::ToLocalOdom(odom_msg);
  map_builder->InsertOdomMsg(local_odom);
}

// third party
#include <nav_msgs/Path.h>
#include <pcl/console/parse.h>
#include <pcl/conversions.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl_conversions/pcl_conversions.h>
#include <ros/ros.h>
#include <sensor_msgs/PointCloud.h>
#include <sensor_msgs/PointCloud2.h>
#include <std_msgs/String.h>
#include <visualization_msgs/Marker.h>
// stl
#include <unistd.h>
#include <sstream>
// local
#include "builder/map_builder.h"
#include "common/file_utils.h"
#include "common/performance/simple_prof.h"
#include "common/simple_time.h"
#include "ros_node/kitti_reader.h"
#include "ros_node/msg_conversion.h"
#include "ros_node/playable_bag.h"
#include "ros_node/tf_bridge.h"
#include "tf/transform_broadcaster.h"

using static_map::MapBuilder;
static MapBuilder::Ptr map_builder;

using static_map::data::ImuMsg;
using static_map::data::NavSatFixMsg;
using static_map::data::OdomMsg;

namespace {

constexpr char kMapFrameId[] = "/map";

void pointcloud_callback(const sensor_msgs::PointCloud2::ConstPtr& msg) {
  pcl::PCLPointCloud2 pcl_pc2;
  pcl_conversions::toPCL(*msg, pcl_pc2);
  MapBuilder::PointCloudPtr incoming_cloud(new MapBuilder::PointCloudType);
  pcl::fromPCLPointCloud2(pcl_pc2, *incoming_cloud);

  std::vector<int> inliers;  // no use, just for the function
  pcl::removeNaNFromPointCloud(*incoming_cloud, *incoming_cloud, inliers);
  map_builder->InsertPointcloudMsg(incoming_cloud);
}

void imu_callback(const sensor_msgs::Imu& imu_msg) {
  ImuMsg::Ptr incomming_imu(new ImuMsg);
  *incomming_imu = static_map_ros::ToLocalImu(imu_msg);
  map_builder->InsertImuMsg(incomming_imu);
}

void odom_callback(const nav_msgs::Odometry& odom_msg)

void gps_callback(const sensor_msgs::NavSatFix& gps_msg) {
  NavSatFixMsg::Ptr local_gps(new NavSatFixMsg);
  *local_gps = static_map_ros::ToLocalNavSatMsg(gps_msg);
  map_builder->InsertGpsMsg(local_gps);
}

}  // namespace

int main(int argc, char** argv) {
  ros::init(argc, argv, "static_mapping_node");
  ros::NodeHandle n;

  SIMPLE_PROF_USE_MS;
  SIMPLE_PROF_MERGE_OUTPUT;

  google::InitGoogleLogging(argv[0]);
  FLAGS_alsologtostderr = true;
  FLAGS_colorlogtostderr = true;
  google::InstallFailureSignalHandler();

  // parse auguements
  // point cloud
  std::string point_cloud_topic = "";
  pcl::console::parse_argument(argc, argv, "-pc", point_cloud_topic);
  if (point_cloud_topic.empty()) {
    PRINT_ERROR("point cloud topic is empty!");
    return -1;
  } else {
    PRINT_INFO_FMT("Get point cloud from ROS topic: %s",
                   point_cloud_topic.c_str());
  }
  ros::Subscriber sub_pointcloud =
      n.subscribe(point_cloud_topic, 10, pointcloud_callback);
  std::string cloud_frame_id = "base_link";
  pcl::console::parse_argument(argc, argv, "-pc_frame_id", cloud_frame_id);

  // imu
  std::string imu_topic = "";
  bool use_imu = false;
  ros::Subscriber sub_imu;
  std::string imu_frame_id = "/novatel_imu";
  pcl::console::parse_argument(argc, argv, "-imu", imu_topic);
  if (imu_topic.empty()) {
    PRINT_WARNING("No imu topic, expect no imu messages.");
  } else {
    PRINT_INFO_FMT("Get imu data from ROS topic: %s", imu_topic.c_str());
    sub_imu = n.subscribe(imu_topic, 100, imu_callback);
    pcl::console::parse_argument(argc, argv, "-imu_frame_id", imu_frame_id);
    use_imu = true;
  }

  // odom
  bool use_odom = false;
  std::string odom_topic = "";
  std::string odom_frame_id = "";
  pcl::console::parse_argument(argc, argv, "-odom", odom_topic);
  pcl::console::parse_argument(argc, argv, "-odom_frame_id", odom_frame_id);
  ros::Subscriber sub_odom;
  if (!odom_topic.empty() && !odom_frame_id.empty()) {
    sub_odom = n.subscribe(odom_topic, 100, odom_callback);
    use_odom = true;
    PRINT_INFO_FMT("Get odom data from ROS topic: %s", odom_topic.c_str());
  } else {
    PRINT_WARNING("No odom data accessed!");
  }

  // gps
  bool use_gps = false;
  std::string gps_topic = "";
  std::string gps_frame_id = "";
  pcl::console::parse_argument(argc, argv, "-gps", gps_topic);
  pcl::console::parse_argument(argc, argv, "-gps_frame_id", gps_frame_id);
  ros::Subscriber gps_subscriber;
  if (!gps_topic.empty() && !gps_frame_id.empty()) {
    gps_subscriber = n.subscribe(gps_topic, 100, gps_callback);
    use_gps = true;
    PRINT_INFO_FMT("Get gps data from ROS topic: %s", gps_topic.c_str());
  } else {
    PRINT_WARNING("No GPS data accessed!");
  }

  // config file
  std::string config_file = "";
  pcl::console::parse_argument(argc, argv, "-cfg", config_file);

  // urdf file
  std::string urdf_file = "";
  pcl::console::parse_argument(argc, argv, "-urdf", urdf_file);

  // tracking frame
  std::string tracking_frame = "base_link";
  pcl::console::parse_argument(argc, argv, "-track", tracking_frame);
  CHECK(!tracking_frame.empty())
      << "tracking frame id should be set." << std::endl;

  // ros bag
  std::string bag_file = "";
  pcl::console::parse_argument(argc, argv, "-bag", bag_file);
  if (!bag_file.empty() && !static_map::common::FileExist(bag_file)) {
    PRINT_WARNING_FMT("%s does not exist.", bag_file.c_str());
    bag_file = "";
  }

  // kitti data set
  std::string kitti_path = "";
  pcl::console::parse_argument(argc, argv, "-kitti", kitti_path);
  if (!kitti_path.empty() && !static_map::common::FileExist(kitti_path)) {
    PRINT_WARNING_FMT("%s does not exist.", kitti_path.c_str());
    kitti_path = "";
  }

  if (!kitti_path.empty() && !bag_file.empty()) {
    PRINT_ERROR("Should not read bag and kitti data set at the same time.");
    return -1;
  }

  std::string pub_topics = "";
  pcl::console::parse_argument(argc, argv, "-pubs", pub_topics);

  ros::Publisher map_publisher =
      n.advertise<sensor_msgs::PointCloud2>("/optimized_map", 1);
  ros::Publisher submap_publisher =
      n.advertise<sensor_msgs::PointCloud2>("/submap", 1);
  ros::Publisher pose_marker_pub =
      n.advertise<visualization_msgs::Marker>("/submap_pose", 1);
  ros::Publisher path_pub =
      n.advertise<nav_msgs::Path>("/static_mapping_path", 1);
  ros::Publisher edge_markers_pub =
      n.advertise<visualization_msgs::Marker>("/loop_edges", 1);
  tf::TransformBroadcaster tf_broadcaster;

  auto show_function =
      [&](const static_map::MapBuilder::PointCloudPtr& cloud) -> void {
    sensor_msgs::PointCloud2 map_pointcloud;
    pcl::toROSMsg(*cloud, map_pointcloud);
    map_pointcloud.header.frame_id = kMapFrameId;
    map_pointcloud.header.stamp = ros::Time::now();
    map_publisher.publish(map_pointcloud);
  };

  auto show_submap_function =
      [&](const static_map::MapBuilder::PointCloudPtr& cloud,
          const Eigen::Matrix4d& pose) -> void {
    tf::Transform transform;
    transform.setOrigin(tf::Vector3(pose(0, 3), pose(1, 3), pose(2, 3)));
    Eigen::Quaterniond rotation(Eigen::Matrix3d(pose.block(0, 0, 3, 3)));
    tf::Quaternion q;
    q.setW(rotation.w());
    q.setX(rotation.x());
    q.setY(rotation.y());
    q.setZ(rotation.z());
    transform.setRotation(q);
    tf_broadcaster.sendTransform(tf::StampedTransform(
        transform, ros::Time::now(), kMapFrameId, tracking_frame));

    sensor_msgs::PointCloud2 submap_pointcloud;
    pcl::toROSMsg(*cloud, submap_pointcloud);
    submap_pointcloud.header.frame_id = tracking_frame;
    submap_pointcloud.header.stamp = ros::Time::now();
    submap_publisher.publish(submap_pointcloud);
  };

  const auto publish_path = [&](const std::vector<Eigen::Matrix4d>& poses) {
    nav_msgs::Path path;
    path.header.frame_id = kMapFrameId;

    path.poses.reserve(poses.size());
    for (const Eigen::Matrix4d& pose : poses) {
      geometry_msgs::PoseStamped pose_ros;
      pose_ros.header.frame_id = kMapFrameId;
      Eigen::Quaterniond q(Eigen::Matrix3d(pose.block(0, 0, 3, 3)));
      pose_ros.pose.orientation.w = q.w();
      pose_ros.pose.orientation.x = q.x();
      pose_ros.pose.orientation.y = q.y();
      pose_ros.pose.orientation.z = q.z();
      pose_ros.pose.position.x = pose(0, 3);
      pose_ros.pose.position.y = pose(1, 3);
      pose_ros.pose.position.z = pose(2, 3);

      path.poses.push_back(pose_ros);
    }
    path_pub.publish(path);
  };

  const auto publish_edges =
      [&](const std::map<int64_t, static_map::back_end::ViewGraph::GraphItem>&
              graph) {
        visualization_msgs::Marker line_list;
        line_list.header.frame_id = kMapFrameId;
        line_list.id = 0;
        line_list.type = visualization_msgs::Marker::LINE_LIST;
        // LINE_STRIP/LINE_LIST markers use only the x component of scale, for
        // the line width
        line_list.scale.x = 0.1;
        // Line list is red
        line_list.color.b = 1.0;
        line_list.color.a = 1.0;
        // Create the vertices for the points and lines
        for (const auto& index_to_connected_item : graph) {
          if (index_to_connected_item.second.connections.size() <= 1) {
            continue;
          }
          const Eigen::Vector3d self_translation =
              index_to_connected_item.second.self_pose.block(0, 3, 3, 1);

          for (const auto& connection :
               index_to_connected_item.second.connections) {
            if (connection.first - index_to_connected_item.first == 1) {
              continue;
            }
            geometry_msgs::Point p_start;
            p_start.x = self_translation[0];
            p_start.y = self_translation[1];
            p_start.z = self_translation[2];

            line_list.points.push_back(p_start);

            geometry_msgs::Point p_end;
            const Eigen::Vector3d connected_translation =
                graph.at(connection.first).self_pose.block(0, 3, 3, 1);
            p_end.x = connected_translation[0];
            p_end.y = connected_translation[1];
            p_end.z = connected_translation[2];

            line_list.points.push_back(p_end);
          }
        }

        edge_markers_pub.publish(line_list);
      };

  map_builder = std::make_shared<MapBuilder>();

  // @todo(edward) merge these 2 situation into 1
  if (urdf_file.empty()) {
    tf::TransformListener listener;
    map_builder->SetTrackingToLidar(static_map_ros::LoopUpTransfrom(
        tracking_frame, cloud_frame_id, listener));
    if (use_imu) {
      map_builder->SetTrackingToImu(static_map_ros::LoopUpTransfrom(
          tracking_frame, imu_frame_id, listener));
    }
    if (use_odom) {
      map_builder->SetTransformOdomToLidar(static_map_ros::LoopUpTransfrom(
          odom_frame_id, cloud_frame_id, listener));
      map_builder->SetTrackingToOdom(static_map_ros::LoopUpTransfrom(
          tracking_frame, odom_frame_id, listener));
    }
    if (use_gps) {
      map_builder->SetTrackingToGps(static_map_ros::LoopUpTransfrom(
          tracking_frame, gps_frame_id, listener));
    }
  } else {
    tf2_ros::Buffer tf_buffer;
    static_map_ros::ReadStaticTransformsFromUrdf(urdf_file, &tf_buffer);
    map_builder->SetTrackingToLidar(static_map_ros::LoopUpTransfrom(
        tracking_frame, cloud_frame_id, tf_buffer));
    if (use_imu) {
      map_builder->SetTrackingToImu(static_map_ros::LoopUpTransfrom(
          tracking_frame, imu_frame_id, tf_buffer));
    }
    if (use_odom) {
      map_builder->SetTransformOdomToLidar(static_map_ros::LoopUpTransfrom(
          odom_frame_id, cloud_frame_id, tf_buffer));
      map_builder->SetTrackingToOdom(static_map_ros::LoopUpTransfrom(
          tracking_frame, odom_frame_id, tf_buffer));
    }
    if (use_gps) {
      map_builder->SetTrackingToGps(static_map_ros::LoopUpTransfrom(
          tracking_frame, gps_frame_id, tf_buffer));
    }
  }

  if (!config_file.empty()) {
    const auto options = map_builder->Initialise(config_file.c_str());
    if (options.front_end_options.imu_options.enabled && !use_imu) {
      PRINT_ERROR("You should set a imu topic if you enable using imu.");
      map_builder->FinishAllComputations();
      return 0;
    }
  } else {
    map_builder->Initialise(NULL);
  }
  map_builder->EnableUsingOdom(use_odom);
  map_builder->EnableUsingGps(use_gps);
  if (!pub_topics.empty()) {
    if (std::strstr(pub_topics.c_str(), "[map]")) {
      map_builder->SetShowMapFunction(show_function);
    }
    if (std::strstr(pub_topics.c_str(), "[submap]")) {
      map_builder->SetShowSubmapFunction(show_submap_function);
    }
    if (std::strstr(pub_topics.c_str(), "[path]")) {
      map_builder->SetShowPathFunction(publish_path);
    }
    if (std::strstr(pub_topics.c_str(), "[edge]")) {
      map_builder->SetShowEdgeFunction(publish_edges);
    }
  }

  REGISTER_FUNC;

  if (!bag_file.empty()) {
    static_map_ros::PlayableBag bag(bag_file, ros::TIME_MIN, ros::TIME_MAX,
                                    ::ros::Duration(1.0));

    const double duration_sec = bag.DurationInSeconds();
    const auto begin_time = std::get<0>(bag.GetBeginEndTime());

    int message_index = 0;
    ros::Time last_message_time = begin_time;
    while (bag.IsMessageAvailable() && ros::ok()) {
      auto message = bag.GetNextMessage();
      if (message.isType<sensor_msgs::PointCloud2>() &&
          message.getTopic() == point_cloud_topic) {
        pointcloud_callback(message.instantiate<sensor_msgs::PointCloud2>());
      } else if (message.isType<sensor_msgs::Imu>() &&
                 message.getTopic() == imu_topic) {
        if (use_imu) {
          imu_callback(*message.instantiate<sensor_msgs::Imu>());
        }
      } else if (message.isType<nav_msgs::Odometry>() &&
                 message.getTopic() == odom_topic) {
        if (use_odom) {
          odom_callback(*message.instantiate<nav_msgs::Odometry>());
        }
      } else if (message.isType<sensor_msgs::NavSatFix>() &&
                 message.getTopic() == gps_topic) {
        if (use_gps) {
          gps_callback(*message.instantiate<sensor_msgs::NavSatFix>());
        }
      }

      // read bag in twice time of the bag speed
      if (message.getTime() > last_message_time) {
        usleep((message.getTime() - last_message_time).toSec() * 0.1 * 1000000);
      }
      last_message_time = message.getTime();

      ++message_index;
      if (message_index % 10000 == 0) {
        PRINT_COLOR_FMT(BOLD, "processed %lf of %lf seconds of the bag... ",
                        (message.getTime() - begin_time).toSec(), duration_sec);
      }
    }
  } else if (!kitti_path.empty()) {
    PRINT_INFO("No bag, read kitti data.");
    static_map::KittiReader kitti_reader;
    kitti_reader.SetPointCloudDataPath(kitti_path);
    int index = 0;
    while (true && ros::ok()) {
      MapBuilder::PointCloudPtr point_cloud = kitti_reader.ReadFromBin(index);
      if (point_cloud && !point_cloud->empty()) {
        point_cloud->header.frame_id = cloud_frame_id;
        point_cloud->header.stamp =
            static_map::SimpleTime::GetCurrentTime().ToNanoSec() / 1000ull;
        map_builder->InsertPointcloudMsg(point_cloud);

        usleep(100000);
      } else {
        break;
      }
      index++;
    }
  } else {
    ::ros::spin();
  }

  map_builder->FinishAllComputations();
  return 0;
}