#pragma once

#include <string>

#include <boost/endian/conversion.hpp>
#include <boost/uuid/uuid.hpp>

#include <common/bits.hpp>
#include <common/buffer.hpp>
#include <common/macros.hpp>
#include <common/span.hpp>
#include <common/traits.hpp>
#include <common/types.hpp>

namespace vitamine::proxyd
{
	namespace detail
	{
		constexpr UInt VARINT32_MAX_ENCODED_SIZE = ceildiv(32, 7);

		inline
		UInt wireSizeVarInt32(Int32 value)
		{
			switch(clz(value))
			{
			case 0:
			case 1:
			case 2:
			case 3:
				return 5;

			case 4:
			case 5:
			case 6:
			case 7:
			case 8:
			case 9:
			case 10:
				return 4;

			case 11:
			case 12:
			case 13:
			case 14:
			case 15:
			case 16:
			case 17:
				return 3;

			case 18:
			case 19:
			case 20:
			case 21:
			case 22:
			case 23:
			case 24:
				return 2;

			case 25:
			case 26:
			case 27:
			case 28:
			case 29:
			case 30:
			case 31:
			case 32:
				return 1;

			default: UNREACHABLE
			}
		}

		inline
		UInt encodeVarInt32(UInt8* buf, Int32 value)
		{
			auto v = (UInt32)value;

			switch(clz(value))
			{
			case 0:
			case 1:
			case 2:
			case 3:
				buf[0] = 0x80u | v;
				buf[1] = 0x80u | v >> 7u;
				buf[2] = 0x80u | v >> 14u;
				buf[3] = 0x80u | v >> 21u;
				buf[4] =         v >> 28u;
				return 5;

			case 4:
			case 5:
			case 6:
			case 7:
			case 8:
			case 9:
			case 10:
				buf[0] = 0x80u | v;
				buf[1] = 0x80u | v >> 7u;
				buf[2] = 0x80u | v >> 14u;
				buf[3] =         v >> 21u;
				return 4;

			case 11:
			case 12:
			case 13:
			case 14:
			case 15:
			case 16:
			case 17:
				buf[0] = 0x80u | v;
				buf[1] = 0x80u | v >> 7u;
				buf[2] =         v >> 14u;
				return 3;

			case 18:
			case 19:
			case 20:
			case 21:
			case 22:
			case 23:
			case 24:
				buf[0] = 0x80u | v;
				buf[1] =         v >> 7u;
				return 2;

			case 25:
			case 26:
			case 27:
			case 28:
			case 29:
			case 30:
			case 31:
			case 32:
				buf[0] = v;
				return 1;

			default: UNREACHABLE
			}
		}
	}

	inline
	void serializeVarInt(Buffer& buffer, Int32 value)
	{
		UInt8 buf[detail::VARINT32_MAX_ENCODED_SIZE];
		auto length = detail::encodeVarInt32(buf, value);
		buffer.write(buf, length);
	}

	template <typename T>
	void serializeInt(Buffer& buffer, T value)
	{
		boost::endian::native_to_big_inplace(value);
		buffer.write(&value, sizeof value);
	}

	inline
	void serializeBool(Buffer& buffer, bool value)
	{
		serializeInt(buffer, (UInt8)value);
	}

	template <typename T>
	void serializeFloat(Buffer& buffer, T value)
	{
		typename UIntForSize<sizeof(T)>::Type intval;
		std::memcpy(&intval, &value, sizeof value);
		boost::endian::native_to_big_inplace(intval);
		buffer.write(&intval, sizeof intval);
	}

	inline
	void serializeString(Buffer& buffer, Span<Char8 const> str)
	{
		serializeVarInt(buffer, str.size());
		buffer.write(str.data(), str.size());
	}

	inline
	void serializeString(Buffer& buffer, std::string const& str)
	{
		serializeString(buffer, Span<Char8 const>(str.data(), str.size()));
	}

	inline
	void serializeBytes(Buffer& buffer, Span<UInt8 const> data)
	{
		buffer.write(data.data(), data.size());
	}

	inline
	void serializeUuid(Buffer& buffer, boost::uuids::uuid uuid)
	{
		UInt64 lo, hi;
		std::memcpy(&lo, uuid.begin(),     8);
		std::memcpy(&hi, uuid.begin() + 8, 8);
		serializeInt(buffer, hi);
		serializeInt(buffer, lo);
	}
}


#include <boost/test/unit_test.hpp>

#include <server_clib/blowfish.h>
#include <iostream>
#include <algorithm>

namespace server_clib {

BOOST_AUTO_TEST_SUITE(blowfish_tests)

BOOST_AUTO_TEST_CASE(encryption_base_author_check)
{
    uint32_t L = 1, R = 2;
    blowfish_ctx_t ctx;

    blowfish_init(&ctx, (uint8_t*)"TESTKEY", 7);
    blowfish_encrypt_chunk(&ctx, &L, &R);
    BOOST_REQUIRE(L == 0xDF333FD2L);
    BOOST_REQUIRE(R == 0x30A71BB4L);

    blowfish_decrypt_chunk(&ctx, &L, &R);
    BOOST_REQUIRE(L == 1);
    BOOST_REQUIRE(R == 2);
}

BOOST_AUTO_TEST_CASE(chunk_encryption_check)
{
    const uint32_t initial_L = 12;
    const uint32_t initial_R = 31;

    uint32_t L = initial_L, R = initial_R;
    blowfish_ctx_t ctx;

    char key[] = "password";
    char wrong_key[] = "PASSWORD";

    BOOST_REQUIRE(blowfish_init(&ctx, (uint8_t*)key, sizeof(key)));
    BOOST_REQUIRE(blowfish_encrypt_chunk(&ctx, &L, &R));
    BOOST_REQUIRE(blowfish_destroy(&ctx));
    BOOST_REQUIRE_EQUAL(ctx.P[0], 0);
    BOOST_REQUIRE_EQUAL(ctx.S[0][0], 0);

    uint32_t enc_L = L, enc_R = R;

    BOOST_REQUIRE(blowfish_init(&ctx, (uint8_t*)wrong_key, sizeof(wrong_key)));
    BOOST_REQUIRE(blowfish_decrypt_chunk(&ctx, &L, &R));

    BOOST_REQUIRE_NE(L, initial_L);
    BOOST_REQUIRE_NE(R, initial_R);

    L = enc_L;
    R = enc_R;

    BOOST_REQUIRE(blowfish_init(&ctx, (uint8_t*)key, sizeof(key)));
    BOOST_REQUIRE(blowfish_decrypt_chunk(&ctx, &L, &R));
    BOOST_REQUIRE(blowfish_destroy(&ctx));
    BOOST_REQUIRE_EQUAL(ctx.P[0], 0);
    BOOST_REQUIRE_EQUAL(ctx.S[0][0], 0);

    BOOST_REQUIRE_EQUAL(L, initial_L);
    BOOST_REQUIRE_EQUAL(R, initial_R);
}

BOOST_AUTO_TEST_CASE(data_encryption_check)
{
    const char input_data[] = "function1 function2 function3"
                              "function4 function5 function6";
    char key[] = "password";
    blowfish_ctx_t ctx;

    auto enc_sz = blowfish_get_stream_output_length(sizeof(input_data));
    BOOST_REQUIRE_GE(enc_sz, sizeof(input_data));
    BOOST_REQUIRE_EQUAL(enc_sz % blowfish_get_min_chunk_length(), 0);

    unsigned char* enc_data = (unsigned char*)alloca(enc_sz);
    BOOST_REQUIRE(blowfish_init(&ctx, (uint8_t*)key, sizeof(key)));
    BOOST_REQUIRE_EQUAL(
        blowfish_stream_encrypt(&ctx, (const unsigned char*)input_data, sizeof(input_data), enc_data, enc_sz), enc_sz);
    BOOST_REQUIRE(blowfish_destroy(&ctx));

    BOOST_REQUIRE(blowfish_init(&ctx, (uint8_t*)key, sizeof(key)));
    unsigned char* dec_data = (unsigned char*)alloca(enc_sz);
    BOOST_REQUIRE_EQUAL(blowfish_stream_decrypt(&ctx, enc_data, enc_sz, dec_data, enc_sz), enc_sz);
    BOOST_REQUIRE(blowfish_destroy(&ctx));

    BOOST_REQUIRE_EQUAL(std::string{ input_data }, std::string{ (char*)dec_data });
}

BOOST_AUTO_TEST_CASE(data_stream_encryption_check)
{
    const char input_data[] = "function1 function2 function3"
                              "function4 function5 function6";
    char key[] = "password";
    blowfish_ctx_t ctx;
    size_t chuck_sz = sizeof(input_data) / 3;

    BOOST_REQUIRE_GE(chuck_sz, blowfish_get_min_chunk_length());

    BOOST_REQUIRE(blowfish_init(&ctx, (uint8_t*)key, sizeof(key)));

    size_t rest_sz = sizeof(input_data);
    std::vector<unsigned char> encoded;
    encoded.reserve(blowfish_get_stream_output_length(rest_sz));
    std::vector<unsigned char> chunk;
    chunk.resize(chuck_sz, 0);
    const unsigned char* p_in_pos = reinterpret_cast<const unsigned char*>(input_data);
    while (rest_sz > 0)
    {
        auto chunk_actial_in = std::min(chuck_sz, rest_sz);
        auto chunk_actial_out = std::max(chunk_actial_in, static_cast<size_t>(blowfish_get_min_chunk_length()));
        auto r = blowfish_stream_encrypt(&ctx, p_in_pos, chunk_actial_in, &chunk[0], chunk_actial_out);
        BOOST_REQUIRE_GT(r, 0);
        p_in_pos += r;
        std::copy_n(begin(chunk), r, std::back_inserter(encoded));
        rest_sz -= (r <= rest_sz) ? r : rest_sz;
    }
    BOOST_REQUIRE(blowfish_destroy(&ctx));

    {
        BOOST_REQUIRE(blowfish_init(&ctx, (uint8_t*)key, sizeof(key)));
        unsigned char* dec_data = (unsigned char*)alloca(encoded.size());
        BOOST_REQUIRE_EQUAL(blowfish_stream_decrypt(&ctx, &encoded[0], encoded.size(), dec_data, encoded.size()),
                            encoded.size());
        BOOST_REQUIRE(blowfish_destroy(&ctx));

        BOOST_REQUIRE_EQUAL(std::string{ input_data }, std::string{ (char*)dec_data });
    }

    BOOST_REQUIRE(blowfish_init(&ctx, (uint8_t*)key, sizeof(key)));
    std::vector<unsigned char> decoded;
    decoded.reserve(encoded.size());
    rest_sz = encoded.size();
    size_t ci = 0;
    while (rest_sz > 0)
    {
        auto chunk_actial_in = std::min(chuck_sz, rest_sz);
        auto chunk_actial_out = std::max(chunk_actial_in, static_cast<size_t>(blowfish_get_min_chunk_length()));
        auto r = blowfish_stream_decrypt(&ctx, &encoded[ci], chunk_actial_in, &chunk[0], chunk_actial_out);
        BOOST_REQUIRE_GT(r, 0);
        ci += r;
        std::copy_n(begin(chunk), r, std::back_inserter(decoded));
        rest_sz -= (r <= rest_sz) ? r : rest_sz;
    }

    BOOST_REQUIRE_EQUAL(std::string{ input_data }, std::string{ (char*)&decoded[0] });
}

BOOST_AUTO_TEST_SUITE_END()
} // namespace server_clib


/*!
 * *****************************************************************************
 *   \file faceDepthMapFrom3D_main.cpp
 *   \author moennen
 *   \brief
 *   \date 2018-03-19
 *   *****************************************************************************/

#include "utils/gl_utils.h"
#include "utils/gl_utils.inline.h"
#include "utils/imgFileLst.h"

#include "SDL2/SDL.h"
#include "SDL2/SDL_opengl.h"

// opengl
#include <glm/glm.hpp>
#include <glm/gtc/matrix_transform.hpp>

// opencv
#include "utils/cv_utils.h"

// face detector / models
#include "externals/face/beFaceMModel.h"
#include "externals/face/beFaceModelInstance.h"

#include <boost/filesystem.hpp>

#include <memory>
#include <random>
#include <iostream>

using namespace std;
using namespace cv;
using namespace glm;
using namespace boost;

namespace
{
std::random_device rd{};
std::mt19937 rs_gen{rd()};

void init()
{
   glClearColor( 0.0, 0.0, 0.0, 0.0 );
   glMatrixMode( GL_PROJECTION );
   glLoadIdentity();
   // glOrtho( 0.0, windowSz.x, windowSz.y, 0.0, -10000.0, 10000.0 );
   // glEnable( GL_BLEND );
   glEnable( GL_DEPTH_TEST );
   // glFrontFace( GL_CW );
   glEnable( GL_TEXTURE_2D );
   glBlendFunc( GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA );
}

void drawFaceModel(
    const gl_utils::TriMeshBuffer& mdFace,
    const glm::mat4& p,
    const glm::mat4& mv,
    const glm::vec3& lightPos,
    const glm::vec3& lightCol,
    const float ambient )
{
   static gl_utils::RenderProgram faceShader;

   static GLint uniMVP = -1;
   static GLint uniMV = -1;
   static GLint uniMVN = -1;
   static GLint uniLightPos = -1;
   static GLint uniLightCol = -1;
   static GLint uniAmbient = -1;

   if ( faceShader._id == -1 )
   {
      faceShader.load(
          "/mnt/p4/avila/moennen_wkspce/sceneIllEst/faceDepthMapFrom3DMM/shaders/face_frag.glsl",
          "/mnt/p4/avila/moennen_wkspce/sceneIllEst/faceDepthMapFrom3DMM/shaders/face_vtx.glsl" );
      uniMVP = faceShader.getUniform( "mvp" );
      uniMV = faceShader.getUniform( "mv" );
      uniMVN = faceShader.getUniform( "mvn" );
      uniLightPos = faceShader.getUniform( "lightPos" );
      uniLightCol = faceShader.getUniform( "lightColor" );
      uniAmbient = faceShader.getUniform( "ambient" );
   }

   faceShader.activate();

   glUniformMatrix4fv( uniMV, 1, 0, value_ptr( mv ) );

   const mat4 mvp = p * mv;
   glUniformMatrix4fv( uniMVP, 1, 0, value_ptr( mvp ) );
   const mat4 mvt = transpose( inverse( mv ) );
   glUniformMatrix4fv( uniMVN, 1, 0, value_ptr( mvt ) );
   glUniform3fv( uniLightPos, 1, value_ptr( lightPos ) );
   glUniform3fv( uniLightCol, 1, value_ptr( lightCol ) );

   glUniform1f( uniAmbient, ambient );

   mdFace.draw();

   faceShader.deactivate();
}

void draw( GLuint tex, size_t w, size_t h )
{
   // Bind Texture
   glBindTexture( GL_TEXTURE_2D, tex );

   GLfloat Vertices[] = {(float)0,
                         (float)0,
                         0,
                         (float)0 + w,
                         (float)0,
                         0,
                         (float)0 + (float)w,
                         (float)0 + (float)h,
                         0,
                         (float)0,
                         (float)0 + (float)h,
                         0};
   GLfloat TexCoord[] = {
       0,
       0,
       1,
       0,
       1,
       1,
       0,
       1,
   };
   GLubyte indices[] = {0,
                        1,
                        2,  // first triangle (bottom left - top left - top right)
                        0,
                        2,
                        3};  // second triangle (bottom left - top right - bottom right)

   glEnableClientState( GL_VERTEX_ARRAY );
   glVertexPointer( 3, GL_FLOAT, 0, Vertices );

   glEnableClientState( GL_TEXTURE_COORD_ARRAY );
   glTexCoordPointer( 2, GL_FLOAT, 0, TexCoord );

   glDrawElements( GL_TRIANGLES, 6, GL_UNSIGNED_BYTE, indices );

   glDisableClientState( GL_TEXTURE_COORD_ARRAY );
   glDisableClientState( GL_VERTEX_ARRAY );

   glBindTexture( GL_TEXTURE_2D, 0 );
}

void getOutUVS(
    const vector<vec2>& iuvs,
    const vector<uvec3>& ivecIdx,
    const glm::uvec3* ovecIdx,
    glm::vec2* ouvs )
{
   for ( size_t f = 0; f < ivecIdx.size(); ++f )
   {
      ouvs[ovecIdx[f].x] = iuvs[ivecIdx[f].x];
      ouvs[ovecIdx[f].y] = iuvs[ivecIdx[f].y];
      ouvs[ovecIdx[f].z] = iuvs[ivecIdx[f].z];
   }
}
}
const string keys =
    "{help h usage ? |         | print this message   }"
    "{@faceModel     |         | face detection model }"
    "{@faceInstLst   |         | face model instance files list}"
    "{@outDir        |         | output directories   }";

int main( int argc, char* argv[] )
{
   CommandLineParser parser( argc, argv, keys );
   if ( parser.has( "help" ) )
   {
      parser.printMessage();
      return ( 0 );
   }

   // Load background images
   ImgNFileLst instLst(2, parser.get<string>( "@faceInstLst" ).c_str(), "" );
   if ( instLst.size() == 0 )
   {
      cerr << "Invalid face instances list : " << parser.get<string>( "@faceInstLst" ) << endl;
      return -1;
   }

   //------------------------------------------------------------------------- Load model
   //
   const string BEfaceModelBin = parser.get<string>( "@faceModel" );
   BEFaceMModel beFaceMMd( BEfaceModelBin.c_str() );
   if ( !beFaceMMd.initialized() )
   {
      std::cerr << "Cannot import model : " << BEfaceModelBin << std::endl;
      return -1;
   }

   uvec2 imgSz( 256, 256 );

   // SDL init
   SDL_Init( SDL_INIT_EVERYTHING );
   SDL_Window* window = SDL_CreateWindow(
       "renderFaceMaps",
       SDL_WINDOWPOS_CENTERED,
       SDL_WINDOWPOS_CENTERED,
       imgSz.x,
       imgSz.y,
       SDL_WINDOW_OPENGL );
   SDL_GLContext glCtx = SDL_GL_CreateContext( window );

   // GLEW init
   GLenum err = glewInit();
   if ( GLEW_OK != err )
   {
      cerr << "Error: " << glewGetErrorString( err ) << endl;
      return -1;
   }

   // Common data alloc
   vector<vec3> vecVtx( BEFaceMModel::NumVertices, vec3( 0.0f ) );
   vector<vec3> vtxCol( BEFaceMModel::NumVertices, vec3( 0.0f ) );
   vector<vec3> vtxNorm( BEFaceMModel::NumVertices, vec3( 0.0f ) );

   // Out root path
   const filesystem::path outRootPath( parser.get<string>( "@outDir" ) );

   // Constant render params
   const float ambient(1.0);
   const vec3 lightPos(0.0);
   const vec3 lightCol = vec3(0.1);

   for ( int s = 0; s < instLst.size(); ++s )
   {
      Mat instImg = cv_utils::imread32FC4( instLst.filePath(s,0) );
      if ( instImg.empty() )
      {
         std::cerr << "Cannot load instance image : " << instLst.filePath(s,0) << std::endl;
         continue;
      }
      imgSz = uvec2( instImg.cols, instImg.rows );

      BEFaceMModelInstance instModel = { instLst.filePath(s,1) };
      if ( !instModel.initialized() )
      {
         std::cerr << "Cannot load instance model : " <<  instLst.filePath(s,1) << std::endl;
         continue;
      }

      // Set the render buffers
      gl_utils::RenderTarget renderTarget( imgSz );
      gl_utils::Texture<gl_utils::RGBA_32FP> faceColorTex( imgSz );
      gl_utils::Texture<gl_utils::RGBA_32FP> faceUVDepthTex( imgSz );
      gl_utils::Texture<gl_utils::RGBA_32FP> faceNormalsTex( imgSz );
      GLuint rTex[3] = {faceColorTex.id, faceUVDepthTex.id, faceNormalsTex.id};

      renderTarget.bind( 3, &rTex[0] );
      glClear( GL_COLOR_BUFFER_BIT );
      glClear( GL_DEPTH_BUFFER_BIT );
      renderTarget.unbind();

      // Load background
      gl_utils::uploadToTexture( faceColorTex, instImg.ptr() );

      // Create the face geometry
      beFaceMMd.get(
          instModel.getShapeCoeffs(),
          instModel.getExpCoeffs(),
          instModel.getTexCoeffs(),
          glm::value_ptr( vecVtx[0] ),
          glm::value_ptr( vtxCol[0] ),
          false );

      gl_utils::TriMeshBuffer mdFaceA;

      // vertices
      mdFaceA.load(
          vecVtx.size(),
          vecVtx.empty() ? nullptr : &vecVtx[0],
          BEFaceMModel::NumFaces,
          beFaceMMd.getFaces() );

      // color
      mdFaceA.loadAttrib( 3, value_ptr( vtxCol[0] ) );

      // normals
      gl_utils::computeNormals(
          BEFaceMModel::NumFaces,
          beFaceMMd.getFaces(),
          BEFaceMModel::NumVertices,
          &vecVtx[0],
          &vtxNorm[0] );
      mdFaceA.loadAttrib( 3, value_ptr( vtxNorm[0] ) );

      // uvs
      mdFaceA.loadAttrib( 2, value_ptr( beFaceMMd.getUVs()[0] ) );

      //  Camera projection
      const mat4 camProj = glm::ortho(0.0f, static_cast<float>(imgSz.x),
        static_cast<float>(imgSz.y), 0.0f, -10000.0f, 10000.0f );

      // Model view
      const mat4 modelView;// = instModel.getPose();

      // Draw the face
      renderTarget.bind( 3, &rTex[0] );

      glEnable( GL_DEPTH_TEST );
      glEnable( GL_CULL_FACE );
      glCullFace( GL_BACK );

      drawFaceModel( mdFaceA, camProj, modelView, lightPos, lightCol, ambient );

      renderTarget.unbind();

      // upload and write the maps
      char sampleId[16];
      sprintf( sampleId, "%08d_", s );
      const string outBasename( sampleId );
      const string outBasenameFull = ( outRootPath / filesystem::path( sampleId ) ).string();

      Mat faceColorImg( faceColorTex.sz.y, faceColorTex.sz.x, CV_32FC4 );
      Mat faceUVDepthImg( faceUVDepthTex.sz.y, faceUVDepthTex.sz.x, CV_32FC4 );
      Mat faceNormalsImg( faceNormalsTex.sz.y, faceNormalsTex.sz.x, CV_32FC4 );
      if ( gl_utils::readbackTexture( faceColorTex, faceColorImg.data ) &&
           gl_utils::readbackTexture( faceUVDepthTex, faceUVDepthImg.data ) &&
           gl_utils::readbackTexture( faceNormalsTex, faceNormalsImg.data ) )
      {
         cvtColor( faceColorImg, faceColorImg, cv::COLOR_RGBA2BGR );
         cvtColor( faceUVDepthImg, faceUVDepthImg, cv::COLOR_RGBA2BGR );
         cvtColor( faceNormalsImg, faceNormalsImg, cv::COLOR_RGBA2BGR );

         imwrite( outBasenameFull + "c.png", faceColorImg * 255.0 );
         imwrite( outBasenameFull + "uvd.exr", faceUVDepthImg );
         imwrite( outBasenameFull + "n.exr", faceNormalsImg );

         std::cout << outBasename + "c.png " << outBasename + "uvd.exr " << outBasename + "n.exr"
                   << std::endl;

         imshow( "color", faceColorImg );
         imshow( "uvdepth", faceUVDepthImg );
         imshow( "normals", faceNormalsImg );
         waitKey( 0 );
      }
   }

   SDL_Quit();

   return ( 0 );
}


#include <ndt_fuser/ndt_fuser_hmt.h>
#include <ndt_offline/VelodyneBagReader.h>
#include <ndt_generic/eigen_utils.h>
#include <ndt_generic/pcl_utils.h>
// PCL specific includes
#include <pcl/conversions.h>
#include <pcl/point_cloud.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <ndt_map/ndt_map.h>
#include <ndt_map/ndt_cell.h>
#include <ndt_map/pointcloud_utils.h>
#include <tf_conversions/tf_eigen.h>
#include <cstdio>
#include <Eigen/Eigen>
#include <Eigen/Geometry>
#include <fstream>
#include <stdio.h>
#include <stdlib.h>
#include <dirent.h>
#include <algorithm>
#include <boost/program_options.hpp>
#include "graph_map/graph_map_fuser.h"
#include "graph_map/ndt/ndt_map_param.h"
#include "graph_map/ndt/ndtd2d_reg_type.h"
#include "graph_map/ndt/ndt_map_type.h"
#include "ros/ros.h"
#include "nav_msgs/Odometry.h"
#include "ros/publisher.h"
#include "tf/transform_broadcaster.h"
#include "ndt_generic/eigen_utils.h"
#include "ndt_generic/io.h"
#include "ndt_offline/pointcloudbagreader.h"
#include "ndt_offline/readbagfilegeneric.h"
#include <unistd.h>
#include "tf_conversions/tf_eigen.h"
using namespace perception_oru;
using namespace libgraphMap;
namespace po = boost::program_options;
using namespace std;

std::string map_dir_name="";
std::string output_dir_name="";
std::string base_name="";
std::string dataset="";
std::string bag_reader_type="";
//map parameters
int itrs=0;
int nb_neighbours=0;
int nb_scan_msgs=0;
bool use_odometry=true;
bool visualize=true;
bool use_multires=false;
bool beHMT=false;
bool filter_fov=false;
bool step_control=false;
bool check_consistency=true;
bool registration2d=true;
bool use_submap=true;
double min_keyframe_dist=0.5;
double min_keyframe_dist_rot_deg=15;
bool use_keyframe=true;
bool alive=false;
bool save_map=true;
bool gt_mapping=false;
bool disable_reg=false, do_soft_constraints=false;
bool pcl_reader=true;
perception_oru::MotionModel2d::Params motion_params;
std::string base_link_id="", gt_base_link_id="", tf_world_frame="";
std::string velodyne_config_file="";
std::string velodyne_packets_topic="";
std::string velodyne_frame_id="";
std::string map_type_name="",registration_type_name="";
std::string tf_topic="";
tf::Transform tf_sensor_pose;
Eigen::Affine3d sensor_offset,fuser_pose, odom_pose;//Mapping from base frame to sensor frame
ros::NodeHandle *n_=NULL;
RegParamPtr regParPtr=NULL;
MapParamPtr mapParPtr=NULL;
GraphParamPtr graphParPtr=NULL;
double sensor_time_offset=0;
double map_size_xy=0;
double map_size_z=0;
double resolution_local_factor=0;
double max_range=0, min_range=0;
double maxRotationNorm_=0;
double compound_radius_=0;
double interchange_radius_=0;
double maxTranslationNorm_=0;
double rotationRegistrationDelta_=0;
double sensorRange_=30;
double translationRegistrationDelta_=0;
double resolution=0;
double hori_min=0, hori_max=0;
double min_dist=0, min_rot_in_deg=0;
unsigned int skip_frame=20;
ros::Publisher *gt_pub,*fuser_pub,*cloud_pub,*odom_pub;
nav_msgs::Odometry gt_pose_msg,fuser_pose_msg,odom_pose_msg;
//VelodyneBagReader<pcl::PointXYZ> *vreader;
//PointCloudBagReader<pcl::PointXYZ> *preader;
//ReadBagFileGeneric<pcl::PointXYZ> *reader;
bool use_pointtype_xyzir;
int min_nb_points_for_gaussian;
bool keep_min_nb_points;
bool min_nb_points_set_uniform;
int nb_measurements=1;
int max_nb_iters=30;

template<class T> std::string toString (const T& x)
{
  std::ostringstream o;

  if (!(o << x))
    throw std::runtime_error ("::toString()");

  return o.str ();
}

std::string transformToEvalString(const Eigen::Transform<double,3,Eigen::Affine,Eigen::ColMajor> &T) {
  std::ostringstream stream;
  stream << std::setprecision(std::numeric_limits<double>::digits10);
  Eigen::Quaternion<double> tmp(T.rotation());
  stream << T.translation().transpose() << " " << tmp.x() << " " << tmp.y() << " " << tmp.z() << " " << tmp.w() << std::endl;
  return stream.str();
}
bool GetSensorPose(const std::string &dataset,  Eigen::Vector3d & transl,  Eigen::Vector3d &euler,tf::Transform &tf_sensor){

  tf::Quaternion quat;

  bool found_sensor_pose=false;
  if(dataset.compare("oru-basement")==0){
    transl[0]=0.3;
    transl[1]=0;
    transl[2]=1.3;
    euler[0]=0;
    euler[1]=0;
    euler[2]=-1.62;
    found_sensor_pose=true;
  }
  else if(dataset.compare("default")==0){
    transl[0]=0.3;
    transl[1]=0;
    transl[2]=0;
    euler[0]=0;
    euler[1]=0;
    euler[2]=-1.62;
    found_sensor_pose=true;
  }
  else if(dataset.compare("arla-2012")==0){
    transl[0]=1.18;
    transl[1]=-0.3;
    transl[2]=2.0;
    euler[0]=0;
    euler[1]=0;
    euler[2]=-1.625;
    found_sensor_pose=true;
  }
  quat.setRPY(euler[0], euler[1], euler[2]);
  tf::Vector3 trans(transl[0], transl[1], transl[2]);
  tf_sensor  = tf::Transform(quat,trans);
  tf::poseTFToEigen(tf_sensor,sensor_offset);
  return found_sensor_pose;
}

bool LocateRosBagFilePaths(const std::string &folder_name,std::vector<std::string> &scanfiles){
  DIR *dir;
  struct dirent *ent;
  if ((dir = opendir (folder_name.c_str())) != NULL) {
    while ((ent = readdir (dir)) != NULL) {
      if(ent->d_name[0] == '.') continue;
      char tmpcname[400];
      snprintf(tmpcname,399,"%s/%s",folder_name.c_str(),ent->d_name);
      std::string tmpfname = tmpcname;
      scanfiles.push_back(tmpfname);
    }
    closedir (dir);
  } else {
    std::cerr<<"Could not parse dir name\n";
    return false;
  }
  sort(scanfiles.begin(),scanfiles.end());
  {
    std::cout << "files to be loaded : " << std::endl;
    for (size_t i = 0; i < scanfiles.size(); i++) {
      std::cout << " " << scanfiles[i] << std::flush;
    }
    std::cout << std::endl;
  }
  return true;
}
bool ReadAllParameters(po::options_description &desc,int &argc, char ***argv){

  Eigen::Vector3d transl;
  Eigen::Vector3d euler;

  // First of all, make sure to advertise all program options
  desc.add_options()
      ("help", "produce help message")
      ("map_type_name", po::value<string>(&map_type_name)->default_value(std::string("default")), "type of map to use e.g. ndt_map or ndt_dl_map (default it default)")
      ("registration_type_name", po::value<string>(&registration_type_name)->default_value(std::string("ndt_d2d_reg")), "type of map to use e.g. ndt_d2d_reg or ndt_dl_reg (default it default)")
      ("visualize", "visualize the output")
      ("use-odometry", "use initial guess from odometry")
      ("disable-mapping", "build maps from cloud data")
      ("no-step-control", "use step control in the optimization (default=false)")
      ("base-name", po::value<string>(&base_name), "prefix for all generated files")
      ("reader-type", po::value<string>(&bag_reader_type)->default_value("velodyne_reader"), "e.g. velodyne_reader or pcl_reader")
      ("data-set", po::value<string>(&dataset)->default_value(std::string("default")), "choose which dataset that is currently used, this option will assist with assigning the sensor pose")
      ("dir-name", po::value<string>(&map_dir_name), "where to look for ros bags")
      ("output-dir-name", po::value<string>(&output_dir_name)->default_value("/home/daniel/.ros/maps"), "where to save the pieces of the map (default it ./map)")
      ("map-size-xy", po::value<double>(&map_size_xy)->default_value(83.), "size of submaps")
      ("map-size-z", po::value<double>(&map_size_z)->default_value(6.0), "size of submaps")
      ("itrs", po::value<int>(&itrs)->default_value(30), "number of iteration in the registration")
      ("fuse-incomplete", "fuse in registration estimate even if iterations ran out. may be useful in combination with low itr numbers")
      ("filter-fov", "cutoff part of the field of view")
      ("hori-max", po::value<double>(&hori_max)->default_value(2*M_PI), "the maximum field of view angle horizontal")
      ("hori-min", po::value<double>(&hori_min)->default_value(-hori_max), "the minimum field of view angle horizontal")
      ("do-soft-constraints", "if soft constraints from odometry should be used")
      ("Dd", po::value<double>(&motion_params.Dd)->default_value(1.), "forward uncertainty on distance traveled")
      ("Dt", po::value<double>(&motion_params.Dt)->default_value(1.), "forward uncertainty on rotation")
      ("Cd", po::value<double>(&motion_params.Cd)->default_value(1.), "side uncertainty on distance traveled")
      ("Ct", po::value<double>(&motion_params.Ct)->default_value(1.), "side uncertainty on rotation")
      ("Td", po::value<double>(&motion_params.Td)->default_value(1.), "rotation uncertainty on distance traveled")
      ("Tt", po::value<double>(&motion_params.Tt)->default_value(1.), "rotation uncertainty on rotation")
      ("min_dist", po::value<double>(&min_dist)->default_value(0.2), "minimum distance traveled before adding cloud")
      ("min_rot_in_deg", po::value<double>(&min_rot_in_deg)->default_value(5), "minimum rotation before adding cloud")
      ("tf_base_link", po::value<std::string>(&base_link_id)->default_value(std::string("/odom_base_link")), "tf_base_link")
      ("tf_gt_link", po::value<std::string>(&gt_base_link_id)->default_value(std::string("/state_base_link")), "tf ground truth link")
      ("velodyne_config_file", po::value<std::string>(&velodyne_config_file)->default_value(std::string("../config/velo32.yaml")), "configuration file for the scanner")
      ("tf_world_frame", po::value<std::string>(&tf_world_frame)->default_value(std::string("/world")), "tf world frame")
      ("velodyne-packets-topic", po::value<std::string>(&velodyne_packets_topic)->default_value(std::string("/velodyne_packets")), "velodyne packets topic used")
      ("velodyne_frame_id", po::value<std::string>(&velodyne_frame_id)->default_value(std::string("/velodyne")), "frame_id of the laser sensor")
      ("alive", "keep the mapper/visualization running even though it is completed (e.g. to take screen shots etc.")
      ("nb_neighbours", po::value<int>(&nb_neighbours)->default_value(2), "number of neighbours used in the registration")
      ("min-range", po::value<double>(&min_range)->default_value(1.0), "minimum range used from scanner")
      ("max-range", po::value<double>(&max_range)->default_value(30), "minimum range used from scanner")
      ("save-map", "saves the graph map at the end of execution")
      ("nb_scan_msgs", po::value<int>(&nb_scan_msgs)->default_value(1), "number of scan messages that should be loaded at once from the bag")
      ("disable-keyframe-update", "use every scan to update map rather than update map upon distance traveled")
      ("keyframe-min-distance", po::value<double>(&min_keyframe_dist)->default_value(0.5), "minimum range used from scanner")
      ("keyframe-min-rot-deg", po::value<double>(&min_keyframe_dist_rot_deg)->default_value(15), "minimum range used from scanner")
      ("gt-mapping", "disable registration and use ground truth as input to mapping")
      ("tf_topic", po::value<std::string>(&tf_topic)->default_value(std::string("/tf")), "tf topic to listen to")
      ("x", po::value<double>(&transl[0])->default_value(0.), "sensor pose - translation vector x")
      ("y", po::value<double>(&transl[1])->default_value(0.), "sensor pose - translation vector y")
      ("z", po::value<double>(&transl[2])->default_value(0.), "sensor pose - translation vector z")
      ("ex", po::value<double>(&euler[0])->default_value(0.), "sensor pose - euler angle vector x")
      ("ey", po::value<double>(&euler[1])->default_value(0.), "sensor pose - euler angle vector y")
      ("ez", po::value<double>(&euler[2])->default_value(0.), "sensor pose - euler angle vector z")
      ("skip-frame", po::value<unsigned int>(&skip_frame)->default_value(20), "sframes to skip before plot map etc.")
      ("sensor_time_offset", po::value<double>(&sensor_time_offset)->default_value(0.), "timeoffset of the scanner data")
      ("registration3d","registration3d")
      ("check-consistency", "if consistency should be checked after registration")
      ("do-soft-constraints", "do_soft_constraints_")
      ("disable-registration", "Disable Registration")
      ("maxRotationNorm",po::value<double>(&maxRotationNorm_)->default_value(0.78539816339),"maxRotationNorm")
      ("maxTranslationNorm",po::value<double>(&maxTranslationNorm_)->default_value(0.4),"maxTranslationNorm")
      ("rotationRegistrationDelta",po::value<double>(&rotationRegistrationDelta_)->default_value(M_PI/6),"rotationRegistrationDelta")
      ("translationRegistrationDelta",po::value<double>(&translationRegistrationDelta_)->default_value(1.5),"sensorRange")
      ("resolution", po::value<double>(&resolution)->default_value(0.4), "resolution of the map")
      ("resolution_local_factor", po::value<double>(&resolution_local_factor)->default_value(1.), "resolution factor of the local map used in the match and fusing step")
      ("disable-submaps", "Adopt the sub-mapping technique which represent the global map as a set of local submaps")
      ("compound-radius", po::value<double>(&compound_radius_)->default_value(10.0), "Requires sub-mapping enabled, When creating new sub-lamps, information from previous map is transfered to the new map. The following radius is used to select the map objects to transfer")
      ("interchange-radius", po::value<double>(&interchange_radius_)->default_value(10.0), "This radius is used to trigger creation or selection of which submap to use")
      ("use_pointtype_xyzir", "If the points to be processed should contain ring and intensity information (velodyne_pointcloud::PointXYZIR)")
      ("min_nb_points_for_gaussian", po::value<int>(&min_nb_points_for_gaussian)->default_value(6), "minimum number of points per cell to compute a gaussian")
      ("keep_min_nb_points", "If the number of points stored in a NDTCell should be cleared if the number is less than min_nb_points_for_gaussian")
      ("min_nb_points_set_uniform", "If the number of points of one cell is less than min_nb_points_for_gaussian, set the distribution to a uniform one (cov = Identity)")
      ("nb_measurements", po::value<int>(&nb_measurements)->default_value(1), "number of scans collecte at each read iteration")
      ("max_nb_iters", po::value<int>(&max_nb_iters)->default_value(30), "max number of iterations used in the registration");

  //Boolean parameres are read through notifiers
  po::variables_map vm;
  po::store(po::parse_command_line(argc, *argv, desc), vm);
  po::notify(vm);

  if (vm.count("help"))
  {
    cout << desc << "\n";
    return false;
  }

  keep_min_nb_points = vm.count("clear_min_nb_points");
  min_nb_points_set_uniform = vm.count("min_nb_points_set_uniform");
  NDTCell::setParameters(0.1, 8*M_PI/18., 1000, min_nb_points_for_gaussian, !keep_min_nb_points, min_nb_points_set_uniform);

  if(GetSensorPose(dataset,transl,euler,tf_sensor_pose)) {
    cout<<"sensor pose from dataset utilized [" << dataset << "]" << endl;
  }

  mapParPtr= GraphFactory::CreateMapParam(map_type_name); //map_type_name
  regParPtr=GraphFactory::CreateRegParam(registration_type_name);
  graphParPtr=GraphFactory::CreateGraphParam();
  if(mapParPtr==NULL || regParPtr==NULL || graphParPtr==NULL)
    return false;

  use_odometry = vm.count("use-odometry");
  visualize = vm.count("visualize");
  filter_fov = vm.count("filter-fov");
  step_control = (vm.count("no-step-control") == 0);
  gt_mapping= vm.count("gt-mapping");
  use_keyframe=!vm.count("disable-keyframe-update");
  if(gt_mapping)
    base_link_id=gt_base_link_id;
  check_consistency=vm.count("check-consistency");
  alive = vm.count("alive");
  save_map = vm.count("save-map");
  registration2d=!vm.count("registration3d");
  do_soft_constraints=vm.count("do-soft-constraints");
  regParPtr->do_soft_constraints_ = vm.count("do-soft-constraints");
  regParPtr->enableRegistration_ = !vm.count("disable-registration") && !gt_mapping;
  regParPtr->registration2d_=registration2d;
  regParPtr->maxRotationNorm_=maxRotationNorm_;
  regParPtr->maxTranslationNorm_=maxTranslationNorm_;
  regParPtr->rotationRegistrationDelta_=rotationRegistrationDelta_;
  regParPtr->translationRegistrationDelta_=translationRegistrationDelta_;
  regParPtr->sensorRange_=max_range;
  regParPtr->mapSizeZ_= map_size_z;
  regParPtr->checkConsistency_=check_consistency;

  mapParPtr->sizez_=map_size_z;
  mapParPtr->max_range_=max_range;
  mapParPtr->min_range_=min_range;
  graphParPtr->compound_radius_=compound_radius_;
  graphParPtr->interchange_radius_=interchange_radius_;
  cout<<"use keyframe="<<use_keyframe;
  graphParPtr->use_keyframe_=use_keyframe;
  graphParPtr->min_keyframe_dist_=min_keyframe_dist;
  graphParPtr->min_keyframe_rot_deg_=min_keyframe_dist_rot_deg;
  mapParPtr->enable_mapping_=!vm.count("disable-mapping");
  mapParPtr->sizey_=map_size_xy;
  mapParPtr->sizex_=map_size_xy;
  use_submap=!vm.count("disable-submaps");
  graphParPtr->use_submap_=use_submap;

  if(  NDTD2DRegParamPtr ndt_reg_ptr=boost::dynamic_pointer_cast<NDTD2DRegParam>(regParPtr)){
    ndt_reg_ptr->resolution_=resolution;
    ndt_reg_ptr->resolutionLocalFactor_=resolution_local_factor;
    ndt_reg_ptr->matcher2D_ITR_MAX = max_nb_iters;
  }
  {
   if(  NDTMapParamPtr ndt_map_ptr=boost::dynamic_pointer_cast<NDTMapParam>(mapParPtr)){
      ndt_map_ptr->resolution_=resolution;
    }
    if (NDTDLMapParamPtr ndt_map_ptr=boost::dynamic_pointer_cast<NDTDLMapParam>(mapParPtr)){
      ndt_map_ptr->resolution_ = resolution;
    }
  }

  //Check if all iputs are assigned
  if (!vm.count("base-name") || !vm.count("dir-name")){
    cout << "Missing base or dir names.\n";
    cout << desc << "\n";
    return false;
  }
  if (vm.count("help")){
    cout << desc << "\n";
    return false;
  }

  use_pointtype_xyzir = vm.count("use_pointtype_xyzir");
  cout<<"base-name:"<<base_name<<endl;
  cout<<"dir-name:"<<map_dir_name<<endl;
  return true;

}
void initializeRosPublishers(){
  ros::Time::init();
  srand(time(NULL));
  gt_pub=new ros::Publisher();
  fuser_pub=new ros::Publisher();
  odom_pub=new ros::Publisher();
  cloud_pub=new ros::Publisher();
  *gt_pub    =n_->advertise<nav_msgs::Odometry>("/GT", 50);
  *fuser_pub =n_->advertise<nav_msgs::Odometry>("/fuser", 50);
  *odom_pub = n_->advertise<nav_msgs::Odometry>("/odom", 50);
  *cloud_pub = n_->advertise<pcl::PointCloud<pcl::PointXYZ>>("/points2", 1);
}
void printParameters(){
  cout<<"Output directory: "<<output_dir_name<<endl;
  if(filter_fov)
    cout << "Filtering FOV of sensor to min/max "<<hori_min<<" "<<hori_max<<endl;
  else
    cout<<"No FOV filter."<<endl;

}
std::string boolToString(bool input){
  return input?std::string("true"):std::string("false");
}

template<typename PointT>
void processData() {

  n_=new ros::NodeHandle("~");

  GraphMapFuser *fuser_;
  stringstream ss;
  string name= gt_mapping? "_gt_":"_fuser_";
  ss<<name<<dataset<<std::string("__Sub=")<<use_submap<<"_sizexy="<<map_size_xy<<"_Z="<<map_size_z<<std::string("_intrchR=")<<interchange_radius_<<std::string("_compR=")<<compound_radius_<<std::string("_res=") <<resolution<<std::string("_maxSensd=") << max_range<<"_keyF="<<use_keyframe<<"_d="<<min_keyframe_dist<<"_deg="<<min_keyframe_dist_rot_deg;
  base_name+=ss.str();
  bool dl = (map_type_name == "ndt_dl_map");
  base_name+="_dl="+toString(dl)+"_xyzir="+toString(use_pointtype_xyzir)+"_mpsu="+toString(min_nb_points_set_uniform)+"_mnpfg="+toString(min_nb_points_for_gaussian)+"kmnp"+toString(keep_min_nb_points);
  ndt_generic::CreateEvalFiles eval_files(output_dir_name,base_name,false);
  printParameters();
  initializeRosPublishers();
  tf::TransformBroadcaster br;
  gt_pose_msg.header.frame_id="/world";
  fuser_pose_msg.header.frame_id="/world";
  odom_pose_msg.header.frame_id="/world";

  std::string tf_interp_link = base_link_id;
  if(gt_mapping)
    tf_interp_link = gt_base_link_id;

  /// Set up the sensor link
  tf::StampedTransform sensor_link; ///Link from /odom_base_link -> velodyne
  sensor_link.child_frame_id_ = velodyne_frame_id;
  sensor_link.frame_id_ = tf_interp_link;//tf_base_link; //"/odom_base_link";
  sensor_link.setData(tf_sensor_pose);

  std::vector<std::string> ros_bag_paths;
  if(!LocateRosBagFilePaths(map_dir_name,ros_bag_paths)){
    cout<<"couldnt locate ros bags"<<endl;
    exit(0);
  }

  int counter = 0;

  cout<<"opening bag files"<<endl;
  for(int i=0; i<ros_bag_paths.size(); i++) {
    std::string bagfilename = ros_bag_paths[i];
    fprintf(stderr,"Opening %s\n",bagfilename.c_str());
    //char c=getchar();
    //reader=new ReadBagFileGeneric<pcl::PointXYZ>(bag_reader_type,
    ReadBagFileGeneric<PointT> reader(bag_reader_type,
                                             tf_interp_link,
                                             velodyne_config_file,
                                             bagfilename,
                                             velodyne_packets_topic,
                                             velodyne_frame_id,
                                             tf_world_frame,
                                             tf_topic,
                                             ros::Duration(3600),
                                             &sensor_link, max_range, min_range,
                                             sensor_time_offset,
                                             nb_measurements);

    pcl::PointCloud<PointT> cloud, cloud_nofilter, points2;
    tf::Transform tf_scan_source;
    tf::Transform tf_gt_base;
    Eigen::Affine3d Todom_base_prev,Tgt_base_prev;
    ros::Time itr=ros::Time::now();
    ros::Time itr_end=itr;
    bool found_scan=true;

    while(found_scan){

      //end_of_bag_file=  vreader.readMultipleMeasurements(nb_scan_msgs,cloud_nofilter,tf_scan_source,tf_gt_base,tf_interp_link);
      //found_scan= preader.readNextMeasurement(cloud_nofilter);
      found_scan= reader.ReadNextMeasurement(cloud_nofilter);
      if(!n_->ok())
        exit(0);

     // cout<<"iteration time="<<ros::Time::now()-itr<<endl;
      itr=ros::Time::now();
     // cout<<"time to read bag file="<<itr_end-itr<<endl;

      if(cloud_nofilter.size()==0) continue;

      if(filter_fov) {
        ndt_generic::filter_fov_fun(cloud,cloud_nofilter,hori_min,hori_max);
      } else {
        cloud = cloud_nofilter;
      }

      if (cloud.size() == 0) continue; // Check that we have something to work with depending on the FOV filter here...

      points2 = cloud;

      // reader.getPoseFor(Todom_base, base_link_id);
      //reader.getPoseFor(Tgt_base, gt_base_link_id);
      tf::Transform tf_odom_base;
      reader.getPoseFor(tf_odom_base, base_link_id);
      reader.getPoseFor(tf_gt_base, gt_base_link_id);

      //  vreader.getPoseFor(tf_odom_base, base_link_id);
      // vreader.getPoseFor(tf_gt_base, gt_base_link_id);

      Eigen::Affine3d Todom_base,Tgt_base;
      tf::transformTFToEigen(tf_gt_base,Tgt_base);
      tf::transformTFToEigen(tf_odom_base,Todom_base);

      if(counter == 0){
        if( found_scan!=true){
          cout<<"Problem with bag filel"<<endl;
          exit(0);
        }
        counter ++;
        cloud.clear();
        cloud_nofilter.clear();
        continue;
      }
      if(counter == 1){
        fuser_pose=Tgt_base;
        odom_pose=fuser_pose;
        Tgt_base_prev = Tgt_base;
        Todom_base_prev = Todom_base;
        fuser_=new GraphMapFuser(regParPtr,mapParPtr,graphParPtr,Tgt_base,sensor_offset);
        cout<<"----------------------PARAMETERS FOR MAPPING--------------------------"<<endl;
        cout<<fuser_->ToString()<<endl;
        cout<<"----------------------PARAMETERS FOR MAPPING--------------------------"<<endl;
        fuser_->Visualize(visualize,plotmarker::point/*plotmarker::sphere*/);
        counter ++;
        cloud.clear();
        cloud_nofilter.clear();
        continue;
      }
      if( found_scan!=true)
        break;
      Eigen::Affine3d Tmotion;
      if(gt_mapping)
        Tmotion = Tgt_base_prev.inverse()*Tgt_base;
      else
        Tmotion = Todom_base_prev.inverse()*Todom_base;
      Eigen::Vector3d Tmotion_euler = Tmotion.rotation().eulerAngles(0,1,2);
      ndt_generic::normalizeEulerAngles(Tmotion_euler);
      //cout<<"Tmotion:"<<Tmotion.translation().transpose()<<endl;
      if(!use_odometry) {
        Tmotion.setIdentity();
      }
      counter++;

      //fuser_pose=fuser_pose*Tmotion;
      ros::Time tplot=ros::Time::now();

      bool registration_update =fuser_->ProcessFrame<PointT>(cloud,fuser_pose,Tmotion);
      tf::Transform tf_fuser_pose;
      tf::poseEigenToTF(fuser_pose,tf_fuser_pose);
      br.sendTransform(tf::StampedTransform(tf_fuser_pose,tplot,"/world","/fuser_base_link"));

      odom_pose = odom_pose*Tmotion;
      tf::Transform tf_odom_pose;
      tf::poseEigenToTF(odom_pose,tf_odom_pose);
      br.sendTransform(tf::StampedTransform(tf_odom_pose,tplot,"/world","/odom_base_link"));

      if(visualize)
      {

        br.sendTransform(tf::StampedTransform(tf_gt_base,tplot,   "/world", "/state_base_link"));
        br.sendTransform(tf::StampedTransform(tf_sensor_pose,tplot,"/fuser_base_link","/fuser_laser_link"));

        if(!gt_mapping){
          fuser_pose_msg.header.stamp=tplot;
          tf::poseEigenToMsg(fuser_pose, fuser_pose_msg.pose.pose);
          fuser_pub->publish(fuser_pose_msg);
        }
        gt_pose_msg.header.stamp=tplot;
        tf::poseEigenToMsg(Tgt_base, gt_pose_msg.pose.pose);
        gt_pub->publish(gt_pose_msg);

        odom_pose_msg.header.stamp=tplot;
        tf::poseEigenToMsg(odom_pose, odom_pose_msg.pose.pose);
        odom_pub->publish(odom_pose_msg);

        if (registration_update) {
          points2.header.frame_id="/world";
          pcl_conversions::toPCL(tplot, points2.header.stamp);
          Eigen::Affine3d tmp = fuser_pose*sensor_offset;
          perception_oru::transformPointCloudInPlace(tmp, points2);
          cloud_pub->publish(points2);
          fuser_->PlotMapType();
        }
      }
      //sleep(1);
      Tgt_base_prev = Tgt_base;
      Todom_base_prev = Todom_base;
      cloud.clear();
      cloud_nofilter.clear();

      eval_files.Write( reader.getTimeStampOfLastSensorMsg(),Tgt_base,Todom_base,fuser_pose,sensor_offset);
      itr_end=ros::Time::now();
    }
  }
  eval_files.Close();

  if(save_map && fuser_!=NULL && fuser_->FramesProcessed()>0){
    char path[1000];
    snprintf(path,999,"%s/%s.map",output_dir_name.c_str(),base_name.c_str());
    fuser_->SaveGraphMap(path);
  }
}

/////////////////////////////////////////////////////////////////////////////////7
/////////////////////////////////////////////////////////////////////////////////7
/// *!!MAIN!!*
/////////////////////////////////////////////////////////////////////////////////7
/////////////////////////////////////////////////////////////////////////////////7
///

int main(int argc, char **argv){
  ros::init(argc, argv, "graph_fuser3d_offline");
  po::options_description desc("Allowed options");

  cout<<"Read params"<<endl;
  bool succesfull=ReadAllParameters(desc,argc,&argv);
  if(!succesfull)
    exit(0);

  if (use_pointtype_xyzir) {
    processData<velodyne_pointcloud::PointXYZIR>();
  }
  else {
    processData<pcl::PointXYZ>();
  }

  if (alive) {
    while (1) {
      usleep(1000);
    }
  }
  usleep(1000*1000);
  std::cout << "Done." << std::endl;
}


#include "StructToRLBotFlatbuffer.hpp"

#define NOMINMAX
#include <algorithm>
#define _USE_MATH_DEFINES
#include <math.h>
#include <rlbot_generated.h>

#include <codecvt>
#include <cwchar>
#include <iostream>
#include <boost/locale/encoding_utf.hpp>

/*
Note: The code in this file looks goofy because flatbuffers requires us to construct things 'pre-order',
i.e. you have to fully create all the sub-objects before you start creating a parent object.

https://github.com/google/flatbuffers/blob/master/samples/sample_binary.cpp

*/
namespace StructToRLBotFlatbuffer
{
	flatbuffers::Offset<rlbot::flat::GameInfo> createGameInfo(flatbuffers::FlatBufferBuilder* builder, GameInfo gameInfo)
	{
		return rlbot::flat::CreateGameInfo(
			*builder,
			gameInfo.timeSeconds,
			gameInfo.gameTimeRemaining,
			gameInfo.overTime,
			gameInfo.unlimitedTime,
			gameInfo.roundActive,
			gameInfo.kickoffPause,
			gameInfo.matchEnded,
			gameInfo.worldGravityZ,
			gameInfo.gameSpeed);
	}

	rlbot::flat::Vector3 createVector3(PyStruct::Vector3 structVec)
	{
		return rlbot::flat::Vector3(structVec.x, structVec.y, structVec.z);
	}

	float convertURot(int rotation)
	{
		return rotation * M_PI / 32768;
	}

	rlbot::flat::Rotator createRotator(PyStruct::Rotator structRot)
	{
		return rlbot::flat::Rotator(
			convertURot(structRot.pitch),
			convertURot(structRot.yaw),
			convertURot(structRot.roll));
	}

	flatbuffers::Offset<rlbot::flat::BoxShape> createBoxShape(flatbuffers::FlatBufferBuilder & builder, BoxShape boxshape)
	{
		return rlbot::flat::CreateBoxShape(builder, boxshape.length, boxshape.width, boxshape.height);
	}

	flatbuffers::Offset<rlbot::flat::SphereShape> createSphereShape(flatbuffers::FlatBufferBuilder & builder, SphereShape sphereshape)
	{
		return rlbot::flat::CreateSphereShape(builder, sphereshape.diameter);
	}

	flatbuffers::Offset<rlbot::flat::CylinderShape> createCylinderShape(flatbuffers::FlatBufferBuilder & builder, CylinderShape cylindershape)
	{
		return rlbot::flat::CreateCylinderShape(builder, cylindershape.diameter, cylindershape.height);
	}

	flatbuffers::Offset<rlbot::flat::BoostPad> createBoostPad(flatbuffers::FlatBufferBuilder* builder, BoostPad boostPad)
	{
		auto location = createVector3(boostPad.location);
		return rlbot::flat::CreateBoostPad(*builder, &location, boostPad.fullBoost);
	}

	flatbuffers::Offset<rlbot::flat::BoostPadState> createBoostPadState(flatbuffers::FlatBufferBuilder* builder, BoostInfo gameBoost)
	{
		return rlbot::flat::CreateBoostPadState(*builder, gameBoost.active, gameBoost.timer);
	}

	flatbuffers::Offset<rlbot::flat::DropshotTile> createDropshotTile(flatbuffers::FlatBufferBuilder* builder, TileInfo dropshotTile)
	{
		rlbot::flat::TileState state;
		if (dropshotTile.tileState == 3)
			state = rlbot::flat::TileState::TileState_Open;
		else if (dropshotTile.tileState == 2)
			state = rlbot::flat::TileState::TileState_Damaged;

		state = rlbot::flat::TileState::TileState_Filled;

		return rlbot::flat::CreateDropshotTile(*builder, state);
	}

	bool FillFieldInfo(flatbuffers::FlatBufferBuilder* builder, FieldInfo fieldInfo)
	{
		std::vector<flatbuffers::Offset<rlbot::flat::BoostPad>> boostPads;

		for (int i = 0; i < fieldInfo.numBoosts; i++)
		{
			boostPads.push_back(createBoostPad(builder, fieldInfo.boostPads[i]));
		}

		std::vector<flatbuffers::Offset<rlbot::flat::GoalInfo>> goals;

		for (int i = 0; i < fieldInfo.numGoals; i++)
		{
			auto location = createVector3(fieldInfo.goals[i].location);
			auto direction = createVector3(fieldInfo.goals[i].direction);
			goals.push_back(rlbot::flat::CreateGoalInfo(*builder,
														fieldInfo.goals[i].teamNum,
														&location,
														&direction,
														fieldInfo.goals[i].width,
														fieldInfo.goals[i].height));
		}

		auto fieldInfoBuffer = rlbot::flat::CreateFieldInfo(*builder, builder->CreateVector(boostPads), builder->CreateVector(goals));
		builder->Finish(fieldInfoBuffer);

		return true;
	}

	flatbuffers::Offset<rlbot::flat::Physics> createPhysics(flatbuffers::FlatBufferBuilder* builder, Physics physics)
	{
		auto location = createVector3(physics.location);
		auto rotation = createRotator(physics.rotation);
		auto velocity = createVector3(physics.velocity);
		auto angular = createVector3(physics.angularVelocity);

		return rlbot::flat::CreatePhysics(*builder, &location, &rotation, &velocity, &angular);
	}

	std::string convertString(wchar_t* w)
	{
		std::wstring ws(w);
		return boost::locale::conv::utf_to_utf<char>(ws.c_str(), ws.c_str() + ws.size());
	}

	flatbuffers::Offset<rlbot::flat::PlayerInfo> createPlayerInfo(flatbuffers::FlatBufferBuilder* builder, PlayerInfo playerInfo)
	{
		rlbot::flat::ScoreInfoBuilder sib(*builder);

		sib.add_score(playerInfo.score.score);
		sib.add_goals(playerInfo.score.goals);
		sib.add_ownGoals(playerInfo.score.ownGoals);
		sib.add_assists(playerInfo.score.assists);
		sib.add_saves(playerInfo.score.saves);
		sib.add_shots(playerInfo.score.shots);
		sib.add_demolitions(playerInfo.score.demolitions);

		auto scoreInfo = sib.Finish();

		std::string name = convertString(playerInfo.name);
		auto flatName = builder->CreateString(name); // Must do this before PlayerInfoBuilder is started.
		auto physics = createPhysics(builder, playerInfo.physics);
		auto hitbox = createBoxShape(*builder, playerInfo.hitbox);
		auto hitboxOffset = createVector3(playerInfo.hitboxOffset);

		rlbot::flat::PlayerInfoBuilder pib(*builder);

		pib.add_scoreInfo(scoreInfo);
		pib.add_isBot(playerInfo.bot);
		pib.add_name(flatName);
		pib.add_isDemolished(playerInfo.demolished);
		pib.add_physics(physics);
		pib.add_hasWheelContact(playerInfo.onGround);
		pib.add_isSupersonic(playerInfo.superSonic);
		pib.add_jumped(playerInfo.jumped);
		pib.add_doubleJumped(playerInfo.doubleJumped);
		pib.add_team(playerInfo.team);
		pib.add_boost(playerInfo.boost);
		pib.add_hitbox(hitbox);
		pib.add_hitboxOffset(&hitboxOffset);
		pib.add_spawnId(playerInfo.spawnId);

		return pib.Finish();
	}

	flatbuffers::Offset<void> createCollisionShape(flatbuffers::FlatBufferBuilder & builder, CollisionShape collisionShape)
	{
		switch (collisionShape.type)
		{
		case BoxType:
			return createBoxShape(builder, collisionShape.box).Union();

		case SphereType:
			return createSphereShape(builder, collisionShape.sphere).Union();

		case CylinderType:
			return createCylinderShape(builder, collisionShape.cylinder).Union();
		}
	}

	rlbot::flat::CollisionShape createCollisionShapeType(CollisionShapeType collisionShapeType)
	{
		switch (collisionShapeType)
		{
		case BoxType:
			return rlbot::flat::CollisionShape_BoxShape;

		case SphereType:
			return rlbot::flat::CollisionShape_SphereShape;

		case CylinderType:
			return rlbot::flat::CollisionShape_CylinderShape;
		}
	}

	flatbuffers::Offset<rlbot::flat::BallInfo> createBallInfo(flatbuffers::FlatBufferBuilder* builder, BallInfo ballInfo)
	{
		bool hasTouch = ballInfo.latestTouch.timeSeconds > 0;
		flatbuffers::Offset<rlbot::flat::Touch> touchOffset;

		if (hasTouch)
		{
			std::string name = convertString(ballInfo.latestTouch.playerName);
			auto flatName = builder->CreateString(name); // Must do this before TouchBuilder is started

			auto touch = rlbot::flat::TouchBuilder(*builder);
			touch.add_playerName(flatName);
			touch.add_gameSeconds(ballInfo.latestTouch.timeSeconds);

			auto location = createVector3(ballInfo.latestTouch.hitLocation);
			touch.add_location(&location);

			auto normal = createVector3(ballInfo.latestTouch.hitNormal);
			touch.add_normal(&normal);

			touch.add_team(ballInfo.latestTouch.team);

			touch.add_playerIndex(ballInfo.latestTouch.playerIndex);

			touchOffset = touch.Finish();
		}

		auto physics = createPhysics(builder, ballInfo.physics);

		flatbuffers::Offset<rlbot::flat::DropShotBallInfo> dropShotBallOffset;

		rlbot::flat::DropShotBallInfoBuilder dbib(*builder);
		dbib.add_absorbedForce(ballInfo.dropShotInfo.absorbedForce);
		dbib.add_damageIndex(ballInfo.dropShotInfo.damageIndex);
		dbib.add_forceAccumRecent(ballInfo.dropShotInfo.forceAccumRecent);

		dropShotBallOffset = dbib.Finish();

		auto collisionshape = createCollisionShape(*builder, ballInfo.collisionShape);

		rlbot::flat::BallInfoBuilder bib(*builder);
		bib.add_physics(physics);

		if (hasTouch)
			bib.add_latestTouch(touchOffset);

		bib.add_dropShotInfo(dropShotBallOffset);
		bib.add_shape(collisionshape);
		bib.add_shape_type(createCollisionShapeType(ballInfo.collisionShape.type));

		return bib.Finish();
	}

	bool FillGameDataPacket(flatbuffers::FlatBufferBuilder* builder, LiveDataPacket liveDataPacket)
	{

		std::vector<flatbuffers::Offset<rlbot::flat::PlayerInfo>> players;
		flatbuffers::Offset<flatbuffers::Vector<flatbuffers::Offset<rlbot::flat::PlayerInfo>>> playersOffset;

		for (int i = 0; i < liveDataPacket.numCars; i++)
		{
			players.push_back(createPlayerInfo(builder, liveDataPacket.gameCars[i]));
		}

		playersOffset = builder->CreateVector(players);

		auto gameInfo = createGameInfo(builder, liveDataPacket.gameInfo);

		std::vector<flatbuffers::Offset<rlbot::flat::BoostPadState>> boosts;
		flatbuffers::Offset<flatbuffers::Vector<flatbuffers::Offset<rlbot::flat::BoostPadState>>> boostsOffset;

		for (int i = 0; i < liveDataPacket.numBoosts; i++)
		{
			boosts.push_back(createBoostPadState(builder, liveDataPacket.gameBoosts[i]));
		}

		boostsOffset = builder->CreateVector(boosts);

		std::vector<flatbuffers::Offset<rlbot::flat::DropshotTile>> tiles;
		flatbuffers::Offset<flatbuffers::Vector<flatbuffers::Offset<rlbot::flat::DropshotTile>>> tilesOffset;

		for (int i = 0; i < liveDataPacket.numTiles; i++)
		{
			tiles.push_back(createDropshotTile(builder, liveDataPacket.gameTiles[i]));
		}

		tilesOffset = builder->CreateVector(tiles);

		flatbuffers::Offset<rlbot::flat::BallInfo> ballOffset;

		ballOffset = createBallInfo(builder, liveDataPacket.gameBall);

		std::vector<flatbuffers::Offset<rlbot::flat::TeamInfo>> teamInfos;
		teamInfos.push_back(rlbot::flat::CreateTeamInfo(*builder, liveDataPacket.teams[0].teamIndex, liveDataPacket.teams[0].score));
		teamInfos.push_back(rlbot::flat::CreateTeamInfo(*builder, liveDataPacket.teams[1].teamIndex, liveDataPacket.teams[1].score));
		auto teamInfosOffset = builder->CreateVector(teamInfos);

		rlbot::flat::GameTickPacketBuilder gtb(*builder);
		gtb.add_gameInfo(gameInfo);

		gtb.add_players(playersOffset);
		gtb.add_boostPadStates(boostsOffset);
		gtb.add_tileInformation(tilesOffset);
		gtb.add_ball(ballOffset);
		gtb.add_teams(teamInfosOffset);

		builder->Finish(gtb.Finish());

		return true;
	}

	flatbuffers::Offset<rlbot::flat::Color> buildColor(flatbuffers::FlatBufferBuilder* builder, Color structColor)
	{
		return rlbot::flat::CreateColor(*builder, structColor.a, structColor.r, structColor.g, structColor.b);
	}

	flatbuffers::Offset<rlbot::flat::PlayerLoadout> buildPlayerLoadout(flatbuffers::FlatBufferBuilder* builder, PlayerConfiguration structPlayerConfig)
	{
		rlbot::flat::LoadoutPaintBuilder paintBuilder(*builder);
		paintBuilder.add_carPaintId(structPlayerConfig.carPaintID);
		paintBuilder.add_decalPaintId(structPlayerConfig.decalPaintID);
		paintBuilder.add_wheelsPaintId(structPlayerConfig.wheelsPaintID);
		paintBuilder.add_boostPaintId(structPlayerConfig.boostPaintID);
		paintBuilder.add_antennaPaintId(structPlayerConfig.antennaPaintID);
		paintBuilder.add_hatPaintId(structPlayerConfig.hatPaintID);
		paintBuilder.add_trailsPaintId(structPlayerConfig.trailsPaintID);
		paintBuilder.add_goalExplosionPaintId(structPlayerConfig.goalExplosionPaintID);
		auto paintOffset = paintBuilder.Finish();

		auto primaryColorOffset = buildColor(builder, structPlayerConfig.primaryColorLookup);
		auto secondaryColorOffset = buildColor(builder, structPlayerConfig.secondaryColorLookup);

		rlbot::flat::PlayerLoadoutBuilder loadoutBuilder(*builder);
		loadoutBuilder.add_teamColorId(structPlayerConfig.teamColorID);
		loadoutBuilder.add_customColorId(structPlayerConfig.customColorID);
		loadoutBuilder.add_carId(structPlayerConfig.carID);
		loadoutBuilder.add_decalId(structPlayerConfig.decalID);
		loadoutBuilder.add_wheelsId(structPlayerConfig.wheelsID);
		loadoutBuilder.add_boostId(structPlayerConfig.boostID);
		loadoutBuilder.add_antennaId(structPlayerConfig.antennaID);
		loadoutBuilder.add_hatId(structPlayerConfig.hatID);
		loadoutBuilder.add_paintFinishId(structPlayerConfig.paintFinishID);
		loadoutBuilder.add_customFinishId(structPlayerConfig.customFinishID);
		loadoutBuilder.add_engineAudioId(structPlayerConfig.engineAudioID);
		loadoutBuilder.add_trailsId(structPlayerConfig.trailsID);
		loadoutBuilder.add_goalExplosionId(structPlayerConfig.goalExplosionID);
		loadoutBuilder.add_loadoutPaint(paintOffset);
		if (structPlayerConfig.useRgbLookup)
		{
			loadoutBuilder.add_primaryColorLookup(primaryColorOffset);
			loadoutBuilder.add_secondaryColorLookup(secondaryColorOffset);
		}

		return loadoutBuilder.Finish();
	}

	flatbuffers::Offset<rlbot::flat::PlayerConfiguration> buildPlayerConfiguration(flatbuffers::FlatBufferBuilder* builder, PlayerConfiguration structPlayerConfig)
	{
		auto name = builder->CreateString(convertString(structPlayerConfig.name));
		auto loadout = buildPlayerLoadout(builder, structPlayerConfig);

		flatbuffers::Offset<void> player;
		rlbot::flat::PlayerClass variety;
		if (structPlayerConfig.bot)
		{
			if (structPlayerConfig.rlbotControlled)
			{
				variety = rlbot::flat::PlayerClass_RLBotPlayer;
				player = rlbot::flat::CreateRLBotPlayer(*builder).Union();
			}
			else
			{
				variety = rlbot::flat::PlayerClass_PsyonixBotPlayer;
				player = rlbot::flat::CreatePsyonixBotPlayer(*builder, structPlayerConfig.botSkill).Union();
			}
		}
		else
		{
			if (structPlayerConfig.rlbotControlled)
			{
				variety = rlbot::flat::PlayerClass_PartyMemberBotPlayer;
				player = rlbot::flat::CreatePartyMemberBotPlayer(*builder).Union();
			}
			else
			{
				variety = rlbot::flat::PlayerClass_HumanPlayer;
				player = rlbot::flat::CreateHumanPlayer(*builder).Union();
			}
		}

		rlbot::flat::PlayerConfigurationBuilder configBuilder(*builder);
		configBuilder.add_name(name);
		configBuilder.add_loadout(loadout);
		configBuilder.add_team(structPlayerConfig.team);
		configBuilder.add_variety(player);
		configBuilder.add_variety_type(variety);
		configBuilder.add_spawnId(structPlayerConfig.spawnId);

		return configBuilder.Finish();
	}

	flatbuffers::Offset<rlbot::flat::MutatorSettings> buildMutatorSettings(flatbuffers::FlatBufferBuilder* builder, MutatorSettings structMutators)
	{
		rlbot::flat::MutatorSettingsBuilder mutatorBuilder(*builder);
		// We rely on the enums in the flatbuffer being in the exact same order as the ones in the struct, and therefore being numbered the same.
		mutatorBuilder.add_matchLength(static_cast<rlbot::flat::MatchLength>(structMutators.matchLength));
		mutatorBuilder.add_maxScore(static_cast<rlbot::flat::MaxScore>(structMutators.maxScore));
		mutatorBuilder.add_overtimeOption(static_cast<rlbot::flat::OvertimeOption>(structMutators.overtimeOptions));
		mutatorBuilder.add_seriesLengthOption(static_cast<rlbot::flat::SeriesLengthOption>(structMutators.seriesLengthOptions));
		mutatorBuilder.add_gameSpeedOption(static_cast<rlbot::flat::GameSpeedOption>(structMutators.gameSpeedOptions));
		mutatorBuilder.add_ballMaxSpeedOption(static_cast<rlbot::flat::BallMaxSpeedOption>(structMutators.ballMaxSpeedOptions));
		mutatorBuilder.add_ballTypeOption(static_cast<rlbot::flat::BallTypeOption>(structMutators.ballTypeOptions));
		mutatorBuilder.add_ballWeightOption(static_cast<rlbot::flat::BallWeightOption>(structMutators.ballWeightOptions));
		mutatorBuilder.add_ballSizeOption(static_cast<rlbot::flat::BallSizeOption>(structMutators.ballSizeOptions));
		mutatorBuilder.add_ballBouncinessOption(static_cast<rlbot::flat::BallBouncinessOption>(structMutators.ballBouncinessOptions));
		mutatorBuilder.add_boostOption(static_cast<rlbot::flat::BoostOption>(structMutators.boostOptions));
		mutatorBuilder.add_rumbleOption(static_cast<rlbot::flat::RumbleOption>(structMutators.rumbleOptions));
		mutatorBuilder.add_boostStrengthOption(static_cast<rlbot::flat::BoostStrengthOption>(structMutators.boostStrengthOptions));
		mutatorBuilder.add_gravityOption(static_cast<rlbot::flat::GravityOption>(structMutators.gravityOptions));
		mutatorBuilder.add_demolishOption(static_cast<rlbot::flat::DemolishOption>(structMutators.demolishOptions));
		mutatorBuilder.add_respawnTimeOption(static_cast<rlbot::flat::RespawnTimeOption>(structMutators.respawnTimeOptions));
		return mutatorBuilder.Finish();
	}

	bool BuildStartMatchMessage(flatbuffers::FlatBufferBuilder* builder, MatchSettings matchSettings)
	{
		std::vector<flatbuffers::Offset<rlbot::flat::PlayerConfiguration>> playerConfigOffsets;

		for (int i = 0; i < matchSettings.numPlayers; i++)
		{
			playerConfigOffsets.push_back(buildPlayerConfiguration(builder, matchSettings.playerConfiguration[i]));
		}

		auto matchSettingsFlat = rlbot::flat::CreateMatchSettings(
			*builder,
			builder->CreateVector(playerConfigOffsets),
			static_cast<rlbot::flat::GameMode>(matchSettings.gameMode),
			static_cast<rlbot::flat::GameMap>(matchSettings.gameMap),
			matchSettings.skipReplays,
			matchSettings.instantStart,
			buildMutatorSettings(builder, matchSettings.mutatorSettings),
			static_cast<rlbot::flat::ExistingMatchBehavior>(matchSettings.existingMatchBehavior),
			matchSettings.enableLockstep,
			matchSettings.enableRendering,
			matchSettings.enableStateSetting,
			matchSettings.autoSaveReplay);

		builder->Finish(matchSettingsFlat);

		return true;
	}

	rlbot::flat::Quaternion createQuaternion(Quaternion structQuaternion)
	{
		return rlbot::flat::Quaternion(structQuaternion.x, structQuaternion.y, structQuaternion.z, structQuaternion.w);
	}

	flatbuffers::Offset<rlbot::flat::RigidBodyState> createRigidBodyState(flatbuffers::FlatBufferBuilder* builder, RigidBodyState state)
	{
		auto location = createVector3(state.location);
		auto rotation = createQuaternion(state.rotation);
		auto velocity = createVector3(state.velocity);
		auto angular = createVector3(state.angularVelocity);

		return rlbot::flat::CreateRigidBodyState(*builder, state.frame, &location, &rotation, &velocity, &angular);
	}

	flatbuffers::Offset<rlbot::flat::ControllerState> createControllerState(flatbuffers::FlatBufferBuilder* builder, PlayerInput structPlayerInput)
	{
		return rlbot::flat::CreateControllerState(*builder,
			structPlayerInput.throttle,
			structPlayerInput.steer,
			structPlayerInput.pitch,
			structPlayerInput.yaw,
			structPlayerInput.roll,
			structPlayerInput.jump,
			structPlayerInput.boost,
			structPlayerInput.handbrake,
			structPlayerInput.useItem);
	}

	flatbuffers::Offset<rlbot::flat::PlayerRigidBodyState> createPlayerRigid(flatbuffers::FlatBufferBuilder* builder, PlayerRigidBodyState stateStruct)
	{
		auto state = createRigidBodyState(builder, stateStruct.state);
		auto input = createControllerState(builder, stateStruct.input);

		return rlbot::flat::CreatePlayerRigidBodyState(*builder, state, input);

	}

	flatbuffers::Offset<rlbot::flat::BallRigidBodyState> createBallRigidBody(flatbuffers::FlatBufferBuilder* builder, BallRigidBodyState structBallState)
	{
		return rlbot::flat::CreateBallRigidBodyState(*builder, createRigidBodyState(builder, structBallState.state));
	}

	bool FillRigidBody(flatbuffers::FlatBufferBuilder* builder, RigidBodyTick rigidBodyTick)
	{
		std::vector<flatbuffers::Offset<rlbot::flat::PlayerRigidBodyState>> players;
		flatbuffers::Offset<flatbuffers::Vector<flatbuffers::Offset<rlbot::flat::PlayerRigidBodyState>>> playersOffset;

		for (int i = 0; i < rigidBodyTick.numPlayers; i++)
		{
			players.push_back(createPlayerRigid(builder, rigidBodyTick.players[i]));
		}

		playersOffset = builder->CreateVector(players);

		flatbuffers::Offset<rlbot::flat::BallRigidBodyState> ballOffset;

		ballOffset = createBallRigidBody(builder, rigidBodyTick.ball);

		auto tickOffset = rlbot::flat::CreateRigidBodyTick(*builder, ballOffset, playersOffset);

		builder->Finish(tickOffset);

		return true;
	}
}


/*
    Copyright 2012 Ulrik Mikaelsson <ulrik.mikaelsson@gmail.com>

    Licensed 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.
*/

#include "hashstore.hpp"

#include "../lib/hashtree.hpp"

#include <algorithm>
#include <sstream>

#include <boost/filesystem.hpp>
#include <boost/smart_ptr/make_shared.hpp>
#include <netinet/in.h>

using namespace std;
using namespace bithorded::store;

namespace io = boost::iostreams;
namespace fs = boost::filesystem;

bithorded::store::TigerNode::TigerNode(HashStore& metaFile, size_t offset) :
	TigerBaseNode(metaFile.read(offset)),
	_metaFile(metaFile),
	_offset(offset),
	_unmodified(*this)
{
}

bithorded::store::TigerNode::~TigerNode()
{
	if (_unmodified != *static_cast<TigerBaseNode*>(this)) {
		_metaFile.write(_offset, *this);
	}
}

HashStore::HashStore( const bithorded::IDataArray::Ptr& storage, uint8_t hashLevelsSkipped )
	: _storage(storage), _hashLevelsSkipped(hashLevelsSkipped)
{
	if (_storage->size() == 0) {
		throw ios_base::failure("Hash storage of size 0 is pointless; "+storage->describe());
	} else if (_storage->size() % sizeof(TigerBaseNode)) {
		throw ios_base::failure("Hash storage not even multiple of nodes; "+storage->describe());
	}
}

HashStore::NodePtr HashStore::operator[](const size_t offset)
{
	if (auto node = _nodeMap[offset]) {
		return node;
	} else {
		auto res = boost::make_shared<TigerNode>(*this, offset);
		_nodeMap.set(offset, res);
		return res;
	}
}

size_t HashStore::size() const
{
	return _storage->size() / sizeof(TigerBaseNode);
}

TigerBaseNode HashStore::read(size_t offset) const
{
	uint64_t f_offset = offset*sizeof(TigerBaseNode);
	TigerBaseNode res;
	auto read = _storage->read(f_offset, sizeof(TigerBaseNode), reinterpret_cast<byte*>(&res));
	if (read != sizeof(TigerBaseNode)) {
		ostringstream buf;
		buf << "Failed reading node at offset " << offset;
		throw ios_base::failure(buf.str());
	}
	return res;
}

void HashStore::write(size_t offset, const TigerBaseNode& node)
{
	uint64_t f_offset = offset*sizeof(TigerBaseNode);
	auto written = _storage->write(f_offset, reinterpret_cast<const byte*>(&node), sizeof(TigerBaseNode));
	if (written != sizeof(TigerBaseNode)) {
		ostringstream buf;
		buf << "Failed writing node at offset " << offset;
		throw ios_base::failure(buf.str());
	}
}

uint64_t HashStore::atoms_needed_for_content ( uint64_t content_size ) {
	auto atomSize = TreeHasher<Node::HashAlgorithm>::ATOMSIZE;
	return (content_size + atomSize - 1) / atomSize;
}

uint64_t HashStore::leaves_needed_for_atoms ( uint64_t atoms, uint8_t levelsSkipped ) {
	auto stored_leaves = atoms >> levelsSkipped;
	if ((stored_leaves << levelsSkipped) != atoms) { // Check for overflow
		stored_leaves += 1;
	}
	return stored_leaves;
}

uint64_t HashStore::leaves_needed_for_content ( uint64_t content_size, uint8_t levelsSkipped ) {
	return leaves_needed_for_atoms(atoms_needed_for_content(content_size), levelsSkipped);
}

uint64_t HashStore::nodes_needed_for_atoms ( uint64_t atoms, uint8_t levelsSkipped ) {
	return treesize(leaves_needed_for_atoms(atoms, levelsSkipped));
}

uint64_t HashStore::nodes_needed_for_content ( uint64_t content_size, uint8_t levelsSkipped ) {
	return nodes_needed_for_atoms(atoms_needed_for_content(content_size), levelsSkipped);
}

uint64_t HashStore::size_needed_for_atoms ( uint64_t atoms, uint8_t levelsSkipped ) {
	return nodes_needed_for_atoms(atoms, levelsSkipped) * sizeof(Node);
}

uint64_t HashStore::size_needed_for_content ( uint64_t content_size, uint8_t levelsSkipped ) {
	return size_needed_for_atoms(atoms_needed_for_content(content_size), levelsSkipped);
}


/*****************************************************************************************************************
 * ReelRobotix Inc. - Software License Agreement      Copyright (c) 2018
 * 	 Authors: Pablo Inigo Blasco, Brett Aldrich
 *
 ******************************************************************************************************************/
#include <angles/angles.h>
#include <pluginlib/class_list_macros.h>
#include <forward_local_planner/forward_local_planner.h>
#include <visualization_msgs/MarkerArray.h>
#include <boost/intrusive_ptr.hpp>
#include <angles/angles.h>

#include <base_local_planner/simple_trajectory_generator.h>

//register this planner as a BaseLocalPlanner plugin
PLUGINLIB_EXPORT_CLASS(cl_move_base_z::forward_local_planner::ForwardLocalPlanner, nav_core::BaseLocalPlanner)

namespace cl_move_base_z
{
namespace forward_local_planner
{
/**
******************************************************************************************************************
* ForwardLocalPlanner()
******************************************************************************************************************
*/
ForwardLocalPlanner::ForwardLocalPlanner()
{
}

/**
******************************************************************************************************************
* ForwardLocalPlanner()
******************************************************************************************************************
*/
ForwardLocalPlanner::~ForwardLocalPlanner()
{
}

void ForwardLocalPlanner::initialize()
{
    k_rho_ = 1.0;
    k_alpha_ = -0.4;
    k_betta_ = -1.0; // set to zero means that orientation is not important
    //k_betta_ = 1.0;
    //betta_offset_=0;

    goalReached_ = false;
    carrot_distance_ = 0.4;

    ros::NodeHandle private_nh("~");

    currentPoseIndex_ = 0;

    ros::NodeHandle nh("~/ForwardLocalPlanner");

    nh.param("k_rho", k_rho_, k_rho_);
    nh.param("k_alpha", k_alpha_, k_alpha_);
    nh.param("k_betta", k_betta_, k_betta_);
    nh.param("carrot_distance", carrot_distance_, carrot_distance_);

    nh.param("yaw_goal_tolerance", yaw_goal_tolerance_, 0.05);
    nh.param("xy_goal_tolerance", xy_goal_tolerance_, 0.10);
    nh.param("max_linear_x_speed", max_linear_x_speed_, 1.0);
    nh.param("max_angular_z_speed", max_angular_z_speed_, 2.0);

    ROS_INFO("[ForwardLocalPlanner] max linear speed: %lf, max angular speed: %lf, k_rho: %lf, carrot_distance: %lf, ", max_linear_x_speed_, max_angular_z_speed_, k_rho_, carrot_distance_);
    goalMarkerPublisher_ = nh.advertise<visualization_msgs::MarkerArray>("goal_marker", 1);

    waiting_ = false;
    waitingTimeout_ = ros::Duration(10);
    ROS_INFO("[ForwardLocalPlanner] initialized");
}

void ForwardLocalPlanner::initialize(std::string name, tf2_ros::Buffer *tf, costmap_2d::Costmap2DROS *costmap_ros)
{
    costmapRos_ = costmap_ros;
    this->initialize();
}

void ForwardLocalPlanner::generateTrajectory(const Eigen::Vector3f &pos, const Eigen::Vector3f &vel, float maxdist, float maxanglediff, float maxtime, float dt, std::vector<Eigen::Vector3f> &outtraj)
{
    //simulate the trajectory and check for collisions, updating costs along the way
    bool end = false;
    float time = 0;
    Eigen::Vector3f currentpos = pos;
    int i = 0;
    while (!end)
    {
        //add the point to the trajectory so we can draw it later if we want
        //traj.addPoint(pos[0], pos[1], pos[2]);

        // if (continued_acceleration_) {
        //   //calculate velocities
        //   loop_vel = computeNewVelocities(sample_target_vel, loop_vel, limits_->getAccLimits(), dt);
        //   //ROS_WARN_NAMED("Generator", "Flag: %d, Loop_Vel %f, %f, %f", continued_acceleration_, loop_vel[0], loop_vel[1], loop_vel[2]);
        // }

        auto loop_vel = vel;
        //update the position of the robot using the velocities passed in
        auto newpos = computeNewPositions(currentpos, loop_vel, dt);

        auto dx = newpos[0] - currentpos[0];
        auto dy = newpos[1] - currentpos[1];
        float dist, angledist;

        //ROS_INFO("traj point %d", i);
        dist = sqrt(dx * dx + dy * dy);
        if (dist > maxdist)
        {
            end = true;
            //ROS_INFO("dist break: %f", dist);
        }
        else
        {
            // ouble from, double to
            angledist = fabs(angles::shortest_angular_distance(currentpos[2], newpos[2]));
            if (angledist > maxanglediff)
            {
                end = true;
                //ROS_INFO("angle dist break: %f", angledist);
            }
            else
            {
                outtraj.push_back(newpos);

                time += dt;
                if (time > maxtime)
                {
                    end = true;
                    //ROS_INFO("time break: %f", time);
                }

                //ROS_INFO("dist: %f, angledist: %f, time: %f", dist, angledist, time);
            }
        }

        currentpos = newpos;
        i++;
    } // end for simulation steps
}

Eigen::Vector3f ForwardLocalPlanner::computeNewPositions(const Eigen::Vector3f &pos, const Eigen::Vector3f &vel, double dt)
{
    Eigen::Vector3f new_pos = Eigen::Vector3f::Zero();
    new_pos[0] = pos[0] + (vel[0] * cos(pos[2]) + vel[1] * cos(M_PI_2 + pos[2])) * dt;
    new_pos[1] = pos[1] + (vel[0] * sin(pos[2]) + vel[1] * sin(M_PI_2 + pos[2])) * dt;
    new_pos[2] = pos[2] + vel[2] * dt;
    return new_pos;
}

/**
******************************************************************************************************************
* initialize()
******************************************************************************************************************
*/
void ForwardLocalPlanner::initialize(std::string name, tf::TransformListener *tf, costmap_2d::Costmap2DROS *costmap_ros)
{
    costmapRos_ = costmap_ros;
    this->initialize();
}

/**
******************************************************************************************************************
* publishGoalMarker()
******************************************************************************************************************
*/
void ForwardLocalPlanner::publishGoalMarker(double x, double y, double phi)
{
    visualization_msgs::Marker marker;

    marker.header.frame_id = this->costmapRos_->getGlobalFrameID();
    marker.header.stamp = ros::Time::now();
    marker.ns = "my_namespace2";
    marker.id = 0;
    marker.type = visualization_msgs::Marker::ARROW;
    marker.action = visualization_msgs::Marker::ADD;
    marker.pose.orientation.w = 1;

    marker.scale.x = 0.1;
    marker.scale.y = 0.3;
    marker.scale.z = 0.1;
    marker.color.a = 1.0;
    marker.color.r = 0;
    marker.color.g = 0;
    marker.color.b = 1.0;

    geometry_msgs::Point start, end;
    start.x = x;
    start.y = y;

    end.x = x + 0.5 * cos(phi);
    end.y = y + 0.5 * sin(phi);

    marker.points.push_back(start);
    marker.points.push_back(end);

    visualization_msgs::MarkerArray ma;
    ma.markers.push_back(marker);

    this->goalMarkerPublisher_.publish(ma);
}

// MELODIC
#if ROS_VERSION_MINIMUM(1, 13, 0)
tf::Stamped<tf::Pose> optionalRobotPose(costmap_2d::Costmap2DROS *costmapRos)
{
    geometry_msgs::PoseStamped paux;
    costmapRos->getRobotPose(paux);
    tf::Stamped<tf::Pose> tfpose;
    tf::poseStampedMsgToTF(paux, tfpose);
    return tfpose;
}
#else
// INDIGO AND PREVIOUS
tf::Stamped<tf::Pose> optionalRobotPose(costmap_2d::Costmap2DROS *costmapRos)
{
    tf::Stamped<tf::Pose> tfpose;
    costmapRos->getRobotPose(tfpose);
    return tfpose;
}
#endif

void clamp(geometry_msgs::Twist &cmd_vel, double max_linear_x_speed_, double max_angular_z_speed_)
{
    if (max_angular_z_speed_ == 0 || max_linear_x_speed_ == 0)
        return;

    if (cmd_vel.angular.z == 0)
    {
        cmd_vel.linear.x = max_linear_x_speed_;
    }
    else
    {
        double kurvature = cmd_vel.linear.x / cmd_vel.angular.z;

        double linearAuthority = fabs(cmd_vel.linear.x / max_linear_x_speed_);
        double angularAuthority = fabs(cmd_vel.angular.z / max_angular_z_speed_);
        if (linearAuthority < angularAuthority)
        {
            // lets go to maximum linear speed
            cmd_vel.linear.x = max_linear_x_speed_;
            cmd_vel.angular.z = kurvature / max_linear_x_speed_;
            ROS_WARN_STREAM("k=" << kurvature << "lets go to maximum linear capacity: " << cmd_vel);
        }
        else
        {
            // lets go with maximum angular speed
            cmd_vel.angular.x = max_angular_z_speed_;
            cmd_vel.linear.x = kurvature * max_angular_z_speed_;
            ROS_WARN_STREAM("lets go to maximum angular capacity: " << cmd_vel);
        }
    }
}

/**
******************************************************************************************************************
* computeVelocityCommands()
******************************************************************************************************************
*/
bool ForwardLocalPlanner::computeVelocityCommands(geometry_msgs::Twist &cmd_vel)
{
    goalReached_ = false;
    ROS_DEBUG("[ForwardLocalPlanner] ----- COMUTE VELOCITY COMMAND LOCAL PLANNER ---");

    tf::Stamped<tf::Pose> tfpose = optionalRobotPose(costmapRos_);

    geometry_msgs::PoseStamped currentPose;
    tf::poseStampedTFToMsg(tfpose,currentPose);
    ROS_DEBUG_STREAM("[ForwardLocalPlanner] current robot pose " << currentPose);

    bool ok = false;
    while (!ok)
    {
        // iterate the point from the current position and ahead until reaching a new goal point in the path
        while (!ok && currentPoseIndex_ < plan_.size())
        {
            auto &pose = plan_[currentPoseIndex_];
            const geometry_msgs::Point &p = pose.pose.position;
            tf::Quaternion q;
            tf::quaternionMsgToTF(pose.pose.orientation, q);

            // take error from the current position to the path point
            double dx = p.x - tfpose.getOrigin().x();
            double dy = p.y - tfpose.getOrigin().y();
            double dist = sqrt(dx * dx + dy * dy);

            double pangle = tf::getYaw(q);
            double angle = tf::getYaw(tfpose.getRotation());
            double angular_error = angles::shortest_angular_distance(pangle, angle);

            if (dist >= carrot_distance_ || fabs(angular_error) > 0.1)
            {
                // the target pose is enough different to be defined as a target
                ok = true;
                ROS_DEBUG("current index: %d, carrot goal percentaje: %lf, dist: %lf, maxdist: %lf, angle_error: %lf", currentPoseIndex_, 100.0 * currentPoseIndex_ / plan_.size(), dist, carrot_distance_, angular_error);
            }
            else
            {
                currentPoseIndex_++;
            }
        }

        ROS_DEBUG_STREAM("[ForwardLocalPlanner] selected carrot pose index " << currentPoseIndex_ << "/" << plan_.size());

        if (currentPoseIndex_ >= plan_.size())
        {
            // even the latest point is quite similar, then take the last since it is the final goal
            cmd_vel.linear.x = 0;
            cmd_vel.angular.z = 0;
            //ROS_INFO("End Local planner");
            ok = true;
            currentPoseIndex_ = plan_.size() - 1;
            //return true;
        }
    }

    //ROS_INFO("pose control algorithm");

    const geometry_msgs::PoseStamped &finalgoalpose = plan_.back();
    const geometry_msgs::PoseStamped &carrot_goalpose = plan_[currentPoseIndex_];
    const geometry_msgs::Point &goalposition = carrot_goalpose.pose.position;

    tf::Quaternion carrotGoalQ;
    tf::quaternionMsgToTF(carrot_goalpose.pose.orientation, carrotGoalQ);
    //ROS_INFO_STREAM("Plan goal quaternion at "<< goalpose.pose.orientation);

    //goal orientation (global frame)
    double betta = tf::getYaw(carrot_goalpose.pose.orientation) + betta_offset_;

    double dx = goalposition.x - tfpose.getOrigin().x();
    double dy = goalposition.y - tfpose.getOrigin().y();

    //distance error to the targetpoint
    double rho_error = sqrt(dx * dx + dy * dy);

    //current angle
    tf::Quaternion currentOrientation = tfpose.getRotation();
    double theta = tf::getYaw(currentOrientation);
    double alpha = atan2(dy, dx);
    alpha = alpha + alpha_offset_;

    double alpha_error = angles::shortest_angular_distance(alpha, theta);
    double betta_error = angles::shortest_angular_distance(betta, theta);

    double vetta; // = k_rho_ * rho_error;
    double gamma; //= k_alpha_ * alpha_error + k_betta_ * betta_error;

    if (rho_error > xy_goal_tolerance_) // reguular control rule, be careful, rho error is with the carrot not with the final goal (this is something to improve like the backwards planner)
    {
        vetta = k_rho_ * rho_error;
        gamma = k_alpha_ * alpha_error;
    }
    else if (fabs(betta_error) >= yaw_goal_tolerance_) // pureSpining
    {
        vetta = 0;
        gamma = k_betta_ * betta_error;
    }
    else // goal reached
    {
        ROS_DEBUG("GOAL REACHED");
        vetta = 0;
        gamma = 0;
        goalReached_ = true;
    }

    // linear speed clamp
    if (vetta > max_linear_x_speed_)
    {
        vetta = max_linear_x_speed_;
    }
    else if (vetta < -max_linear_x_speed_)
    {
        vetta = -max_linear_x_speed_;
    }

    // angular speed clamp
    if (gamma > max_angular_z_speed_)
    {
        gamma = max_angular_z_speed_;
    }
    else if (gamma < -max_angular_z_speed_)
    {
        gamma = -max_angular_z_speed_;
    }

    cmd_vel.linear.x = vetta;
    cmd_vel.angular.z = gamma;

    //clamp(cmd_vel, max_linear_x_speed_, max_angular_z_speed_);

    //ROS_INFO_STREAM("Local planner: "<< cmd_vel);

    publishGoalMarker(goalposition.x, goalposition.y, betta);

    ROS_DEBUG_STREAM("Forward local planner," << std::endl
                                              << " theta: " << theta << std::endl
                                              << " betta: " << betta << std::endl
                                              << " err_x: " << dx << std::endl
                                              << " err_y:" << dy << std::endl
                                              << " rho_error:" << rho_error << std::endl
                                              << " alpha_error:" << alpha_error << std::endl
                                              << " betta_error:" << betta_error << std::endl
                                              << " vetta:" << vetta << std::endl
                                              << " gamma:" << gamma << std::endl
                                              << " xy_goal_tolerance:" << xy_goal_tolerance_ << std::endl
                                              << " yaw_goal_tolerance:" << yaw_goal_tolerance_ << std::endl);

    //if(cmd_vel.linear.x==0 && cmd_vel.angular.z == 0 )
    //{
    //}

    //integrate trajectory and check collision

    tf::Stamped<tf::Pose> global_pose = optionalRobotPose(costmapRos_);

    //->getRobotPose(global_pose);

    auto *costmap2d = costmapRos_->getCostmap();
    auto yaw = tf::getYaw(global_pose.getRotation());

    auto &pos = global_pose.getOrigin();

    Eigen::Vector3f currentpose(pos.x(), pos.y(), yaw);
    Eigen::Vector3f currentvel(cmd_vel.linear.x, cmd_vel.linear.y, cmd_vel.angular.z);
    std::vector<Eigen::Vector3f> trajectory;
    this->generateTrajectory(currentpose, currentvel, 0.8 /*meters*/, M_PI / 8 /*rads*/, 3.0 /*seconds*/, 0.05 /*seconds*/, trajectory);

    // check plan rejection
    bool aceptedplan = true;

    unsigned int mx, my;

    int i = 0;
    // ROS_INFO_STREAM("lplanner goal: " << finalgoalpose.pose.position);
    for (auto &p : trajectory)
    {
        float dx = p[0] - finalgoalpose.pose.position.x;
        float dy = p[1] - finalgoalpose.pose.position.y;

        float dst = sqrt(dx * dx + dy * dy);
        if (dst < xy_goal_tolerance_)
        {
            //  ROS_INFO("trajectory checking skipped, goal reached");
            break;
        }

        costmap2d->worldToMap(p[0], p[1], mx, my);
        unsigned int cost = costmap2d->getCost(mx, my);

        // ROS_INFO("checking cost pt %d [%lf, %lf] cell[%d,%d] = %d", i, p[0], p[1], mx, my, cost);
        // ROS_INFO_STREAM("cost: " << cost);

        // static const unsigned char NO_INFORMATION = 255;
        // static const unsigned char LETHAL_OBSTACLE = 254;
        // static const unsigned char INSCRIBED_INFLATED_OBSTACLE = 253;
        // static const unsigned char FREE_SPACE = 0;

        if (costmap2d->getCost(mx, my) >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE)
        {
            aceptedplan = false;
            // ROS_WARN("ABORTED LOCAL PLAN BECAUSE OBSTACLE DETEDTED");
            break;
        }
        i++;
    }

    if (aceptedplan)
    {
        waiting_ = false;
        return true;
    }
    else
    {
        // stop and wait
        cmd_vel.linear.x = 0;
        cmd_vel.angular.z = 0;

        if (waiting_ == false)
        {
            waiting_ = true;
            waitingStamp_ = ros::Time::now();
        }
        else
        {
            auto waitingduration = ros::Time::now() - waitingStamp_;

            if (waitingduration > this->waitingTimeout_)
            {
                return false;
            }
        }

        return true;
    }
}

/**
******************************************************************************************************************
* isGoalReached()
******************************************************************************************************************
*/
bool ForwardLocalPlanner::isGoalReached()
{
    return goalReached_;
}

/**
******************************************************************************************************************
* setPlan()
******************************************************************************************************************
*/
bool ForwardLocalPlanner::setPlan(const std::vector<geometry_msgs::PoseStamped> &plan)
{
    plan_ = plan;
    goalReached_ = false;
    return true;
}
} // namespace forward_local_planner
} // namespace cl_move_base_z