#include <cstdio>
#include <cstdlib>
#include <cassert>
#include <unistd.h>
#include <sstream>
#include <cmath>
#include "IDType.h"
#include "BonsaiSharedData.h"
#include "BonsaiIO.h"
#include "SharedMemory.h"
#ifndef BONSAI_CATALYST_CLANG
 #include <omp.h>
#endif
#ifdef BONSAI_CATALYST_STDLIB
 #include <boost/function.hpp>
 #define bonsaistd boost
#else
 #include <functional>
 #define bonsaistd std
#endif
#include <mpi.h>

#include "anyoption.h"
#include "BonsaiCatalystData.h"

static void renderer(
    int argc, char** argv,
    const int rank, const int nrank, const MPI_Comm &comm,
    BonsaiCatalystData &data,
    const char *fullScreenMode /* = "" */,
    const bool stereo /* = false */,
    bonsaistd::function<void(int)> &callback)
{
  /* do rendering here */
  while (1)
  {
    sleep(1);
    callback(0);  /* fetch new data */
    if (data.isNewData())
    {
      fprintf(stderr , "rank= %d: --copying new data --\n", rank);
      /* copy new data into my buffer */
      data.unsetNewData();
    }
    fprintf(stderr ," rank= %d: rendering ... \n", rank);
    data.coProcess(0.0, 0);
  }
}

using ShmQHeader = SharedMemoryClient<BonsaiSharedQuickHeader>;
using ShmQData   = SharedMemoryClient<BonsaiSharedQuickData>;
static ShmQHeader *shmQHeader = NULL;
static ShmQData   *shmQData   = NULL;

static bool terminateRenderer = false;

bool fetchSharedData(const bool quickSync, BonsaiCatalystData &rData, const int rank, const int nrank, const MPI_Comm &comm,
    const int reduceDM = 1, const int reduceS = 1)
{
  if (shmQHeader == NULL)
  {
    shmQHeader = new ShmQHeader(ShmQHeader::type::sharedFile(rank));
    shmQData   = new ShmQData  (ShmQData  ::type::sharedFile(rank));
  }

  auto &header = *shmQHeader;
  auto &data   = *shmQData;

  static bool first = true;
  if (quickSync && first)
  {
    /* handshake */

    header.acquireLock();
    header[0].handshake = true;
    header.releaseLock();

    while (header[0].handshake)
      usleep(1000);

    header.acquireLock();
    header[0].handshake = true;
    header.releaseLock();

    /* handshake complete */
    first = false;
  }

  static float tLast = -1.0f;

  if (rData.isNewData())
    return false;

#if 0
  //  if (rank == 0)
  fprintf(stderr, " rank= %d: attempting to fetch data \n",rank);
#endif

  // header
  header.acquireLock();
  const float tCurrent = header[0].tCurrent;

  terminateRenderer = tCurrent == -1;

  int sumL = quickSync ? !header[0].done_writing : tCurrent != tLast;
  int sumG ;
  MPI_Allreduce(&sumL, &sumG, 1, MPI_INT, MPI_SUM, comm);

  bool completed = false;
  if (sumG == nrank) //tCurrent != tLast)
  {
    tLast = tCurrent;
    completed = true;

    // data
    const size_t nBodies = header[0].nBodies;
    data.acquireLock();

    const size_t size = data.size();
    assert(size == nBodies);

    /* skip particles that failed to get density, or with too big h */
    auto skipPtcl = [&](const int i)
    {
      return (data[i].rho == 0 || data[i].h == 0.0 || data[i].h > 100);
    };

    size_t nDM = 0, nS = 0;
    constexpr int ntypecount = 10;
    bonsaistd::array<size_t,ntypecount> ntypeloc, ntypeglb;
    std::fill(ntypeloc.begin(), ntypeloc.end(), 0);
    for (size_t i = 0; i < size; i++)
    {
      const int type = data[i].ID.getType();
      if  (type < ntypecount)
        ntypeloc[type]++;
      if (skipPtcl(i))
        continue;
      switch (type)
      {
        case 0:
          nDM++;
          break;
        default:
          nS++;
      }
    }

    MPI_Reduce(&ntypeloc, &ntypeglb, ntypecount, MPI_LONG_LONG, MPI_SUM, 0, comm);
    if (rank == 0)
    {
      for (int type = 0; type < ntypecount; type++)
        if (ntypeglb[type] > 0)
          fprintf(stderr, " ptype= %d:  np= %zu \n",type, ntypeglb[type]);
    }

    rData.resize(nS);
    size_t ip = 0;
    for (size_t i = 0; i < size; i++)
    {
      if (skipPtcl(i))
        continue;
      if (data[i].ID.getType() == 0 )  /* pick stars only */
        continue;

      rData.posx(ip) = data[i].x;
      rData.posy(ip) = data[i].y;
      rData.posz(ip) = data[i].z;
      rData.ID  (ip) = data[i].ID;
      rData.attribute(BonsaiCatalystData::MASS, ip) = data[i].mass;
      rData.attribute(BonsaiCatalystData::VEL,  ip) =
        std::sqrt(
            data[i].vx*data[i].vx+
            data[i].vy*data[i].vy+
            data[i].vz*data[i].vz);
      rData.attribute(BonsaiCatalystData::RHO, ip) = data[i].rho;
      rData.attribute(BonsaiCatalystData::H,   ip) = data[i].h;

      ip++;
      assert(ip <= nS);
    }
    rData.resize(ip);

    data.releaseLock();
  }

  header[0].done_writing = true;
  header.releaseLock();

#if 0
  //  if (rank == 0)
  fprintf(stderr, " rank= %d: done fetching data \n", rank);
#endif

  if (completed)
    rData.computeMinMax();

  return completed;
}

  template<typename T>
static T* readBonsai(
    const int rank, const int nranks, const MPI_Comm &comm,
    const std::string &fileName,
    const int reduceDM,
    const int reduceS,
    const bool print_header = false)
{
  BonsaiIO::Core out(rank, nranks, comm, BonsaiIO::READ, fileName);
  if (rank == 0 && print_header)
  {
    fprintf(stderr, "---- Bonsai header info ----\n");
    out.getHeader().printFields();
    fprintf(stderr, "----------------------------\n");
  }
  typedef float float4[4];
  typedef float float3[3];
  typedef float float2[2];

  BonsaiIO::DataType<IDType> IDListS("Stars:IDType");
  BonsaiIO::DataType<float4> posS("Stars:POS:real4");
  BonsaiIO::DataType<float3> velS("Stars:VEL:float[3]");
  BonsaiIO::DataType<float2> rhohS("Stars:RHOH:float[2]");

  if (reduceS > 0)
  {
    if (!out.read(IDListS, true, reduceS)) return NULL;
    if (rank  == 0)
      fprintf(stderr, " Reading star data \n");
    assert(out.read(posS,    true, reduceS));
    assert(out.read(velS,    true, reduceS));
    bool renderDensity = true;
    if (!out.read(rhohS,  true, reduceS))
    {
      if (rank == 0)
      {
        fprintf(stderr , " -- Stars RHOH data is found \n");
        fprintf(stderr , " -- rendering stars w/o density info \n");
      }
      renderDensity = false;
    }
    assert(IDListS.getNumElements() == posS.getNumElements());
    assert(IDListS.getNumElements() == velS.getNumElements());
    if (renderDensity)
      assert(IDListS.getNumElements() == posS.getNumElements());
  }

  BonsaiIO::DataType<IDType> IDListDM("DM:IDType");
  BonsaiIO::DataType<float4> posDM("DM:POS:real4");
  BonsaiIO::DataType<float3> velDM("DM:VEL:float[3]");
  BonsaiIO::DataType<float2> rhohDM("DM:RHOH:float[2]");
  if (reduceDM > 0)
  {
    if (rank  == 0)
      fprintf(stderr, " Reading DM data \n");
    if(!out.read(IDListDM, true, reduceDM)) return NULL;
    assert(out.read(posDM,    true, reduceDM));
    assert(out.read(velDM,    true, reduceDM));
    bool renderDensity = true;
    if (!out.read(rhohDM,  true, reduceDM))
    {
      if (rank == 0)
      {
        fprintf(stderr , " -- DM RHOH data is found \n");
        fprintf(stderr , " -- rendering stars w/o density info \n");
      }
      renderDensity = false;
    }
    assert(IDListS.getNumElements() == posS.getNumElements());
    assert(IDListS.getNumElements() == velS.getNumElements());
    if (renderDensity)
      assert(IDListS.getNumElements() == posS.getNumElements());
  }

  const int nS  = IDListS.getNumElements();
  const int nDM = IDListDM.getNumElements();
  long long int nSloc = nS, nSglb;
  long long int nDMloc = nDM, nDMglb;

  MPI_Allreduce(&nSloc, &nSglb, 1, MPI_LONG, MPI_SUM, comm);
  MPI_Allreduce(&nDMloc, &nDMglb, 1, MPI_LONG, MPI_SUM, comm);
  if (rank == 0)
  {
    fprintf(stderr, "nStars = %lld\n", nSglb);
    fprintf(stderr, "nDM    = %lld\n", nDMglb);
  }

  T *rDataPtr = new T(rank,nranks,comm);
  rDataPtr->resize(nS+nDM);
  auto &rData = *rDataPtr;
  for (int i = 0; i < nS; i++)
  {
    const int ip = i;
    rData.posx(ip) = posS[i][0];
    rData.posy(ip) = posS[i][1];
    rData.posz(ip) = posS[i][2];
    rData.ID  (ip) = IDListS[i];
    assert(rData.ID(ip).getType() > 0); /* sanity check */
    rData.attribute(BonsaiCatalystData::MASS, ip) = posS[i][3];
    rData.attribute(BonsaiCatalystData::VEL,  ip) =
      std::sqrt(
          velS[i][0]*velS[i][0] +
          velS[i][1]*velS[i][1] +
          velS[i][2]*velS[i][2]);
    if (rhohS.size() > 0)
    {
      rData.attribute(BonsaiCatalystData::RHO, ip) = rhohS[i][0];
      rData.attribute(BonsaiCatalystData::H,  ip)  = rhohS[i][1];
    }
    else
    {
      rData.attribute(BonsaiCatalystData::RHO, ip) = 0.0;
      rData.attribute(BonsaiCatalystData::H,   ip) = 0.0;
    }
  }
  for (int i = 0; i < nDM; i++)
  {
    const int ip = i + nS;
    rData.posx(ip) = posDM[i][0];
    rData.posy(ip) = posDM[i][1];
    rData.posz(ip) = posDM[i][2];
    rData.ID  (ip) = IDListDM[i];
    assert(rData.ID(ip).getType() == 0); /* sanity check */
    rData.attribute(BonsaiCatalystData::MASS, ip) = posDM[i][3];
    rData.attribute(BonsaiCatalystData::VEL,  ip) =
      std::sqrt(
          velDM[i][0]*velDM[i][0] +
          velDM[i][1]*velDM[i][1] +
          velDM[i][2]*velDM[i][2]);
    if (rhohDM.size() > 0)
    {
      rData.attribute(BonsaiCatalystData::RHO, ip) = rhohDM[i][0];
      rData.attribute(BonsaiCatalystData::H,   ip) = rhohDM[i][1];
    }
    else
    {
      rData.attribute(BonsaiCatalystData::RHO, ip) = 0.0;
      rData.attribute(BonsaiCatalystData::H,   ip) = 0.0;
    }
  }

  return rDataPtr;
}

#ifndef BONSAI_CATALYST_CLANG
int main(int argc, char * argv[], MPI_Comm commWorld)
{
#else
int main(int argc, char * argv[])
{
 MPI_Comm commWorld;
#endif

  std::string fileName;
  int reduceDM    =  10;
  int reduceS=  1;
#ifndef PARTICLESRENDERER
  std::string fullScreenMode    = "";
  bool stereo     = false;
#endif
  int nmaxsample = 200000;
  std::string display;

  bool inSitu = false;
  bool quickSync = true;
  int sleeptime = 1;

  {
    AnyOption opt;

#define ADDUSAGE(line) {{std::stringstream oss; oss << line; opt.addUsage(oss.str());}}

    ADDUSAGE(" ");
    ADDUSAGE("Usage:");
    ADDUSAGE(" ");
    ADDUSAGE(" -h  --help             Prints this help ");
    ADDUSAGE(" -i  --infile #         Input snapshot filename ");
    ADDUSAGE(" -I  --insitu          Enable in-situ rendering ");
    ADDUSAGE("     --sleep  #        start up sleep in sec [1]  ");
    ADDUSAGE("     --noquicksync      disable syncing with simulation [enabled] ");
    ADDUSAGE("     --reduceDM    #    cut down DM dataset by # factor [10]. 0-disable DM");
    ADDUSAGE("     --reduceS     #    cut down stars dataset by # factor [1]. 0-disable S");
#ifndef PARTICLESRENDERER
    ADDUSAGE("     --fullscreen  #    set fullscreen mode string");
    ADDUSAGE("     --stereo           enable stereo rendering");
#endif
    ADDUSAGE(" -s  --nmaxsample   #   set max number of samples for DD [" << nmaxsample << "]");
    ADDUSAGE(" -D  --display      #   set DISPLAY=display, otherwise inherited from environment");

    opt.setFlag  ( "help" ,        'h');
    opt.setOption( "infile",       'i');
    opt.setFlag  ( "insitu",       'I');
    opt.setOption( "reduceDM");
    opt.setOption( "sleep");
    opt.setOption( "reduceS");
    opt.setOption( "fullscreen");
    opt.setFlag("stereo");
    opt.setOption("nmaxsample", 's');
    opt.setOption("display", 'D');
    opt.setFlag  ( "noquicksync");

    opt.processCommandArgs( argc, argv );

    if( ! opt.hasOptions() ||  opt.getFlag( "help" ) || opt.getFlag( 'h' ) )
    {
      /* print usage if no options or requested help */
      opt.printUsage();
      ::exit(0);
    }

    char *optarg = NULL;
    if (opt.getFlag("insitu"))  inSitu = true;
    if ((optarg = opt.getValue("infile")))       fileName           = std::string(optarg);
    if ((optarg = opt.getValue("reduceDM"))) reduceDM       = atoi(optarg);
    if ((optarg = opt.getValue("reduceS"))) reduceS       = atoi(optarg);
#ifndef PARTICLESRENDERER
    if ((optarg = opt.getValue("fullscreen")))	 fullScreenMode     = std::string(optarg);
    if (opt.getFlag("stereo"))  stereo = true;
#endif
    if ((optarg = opt.getValue("nmaxsample"))) nmaxsample = atoi(optarg);
    if ((optarg = opt.getValue("display"))) display = std::string(optarg);
    if ((optarg = opt.getValue("sleep"))) sleeptime = atoi(optarg);
    if (opt.getValue("noquicksync")) quickSync = false;

    if ((fileName.empty() && !inSitu) ||
        reduceDM < 0 || reduceS < 0)
    {
      opt.printUsage();
      ::exit(0);
    }

#undef ADDUSAGE
  }

  MPI_Comm comm = MPI_COMM_WORLD;
  int mpiInitialized = 0;
  MPI_Initialized(&mpiInitialized);
  if (!mpiInitialized)
    MPI_Init(&argc, &argv);
  else
    comm = commWorld;

  int nranks, rank;
  MPI_Comm_size(comm, &nranks);
  MPI_Comm_rank(comm, &rank);

  char processor_name[MPI_MAX_PROCESSOR_NAME];
  int namelen;
  MPI_Get_processor_name(processor_name,&namelen);
  fprintf(stderr, "bonsai_renderer:: Proc id: %d @ %s , total processes: %d (mpiInit) \n", rank, processor_name, nranks);

  if (rank == 0)
  {
    char hostname[256];
    gethostname(hostname,256);
    char * display = getenv("DISPLAY");
    fprintf(stderr, "root: %s  display: %s \n", hostname, display);
  }

  if (!display.empty())
  {
    std::string var="DISPLAY="+display;
    putenv((char*)var.c_str());
  }

  if (rank == 0)
    fprintf(stderr, " Sleeping for %d seconds \n", sleeptime);
  sleep(sleeptime);

  using BonsaiCatalystDataT = BonsaiCatalystData;
  BonsaiCatalystDataT *rDataPtr;
  if (inSitu)
  {
    rDataPtr = new BonsaiCatalystDataT(rank,nranks,comm);
  }
  else
  {
    if ((rDataPtr = readBonsai<BonsaiCatalystDataT>(rank, nranks, comm, fileName, reduceDM, reduceS)))
    {}
    else
    {
      if (rank == 0)
        fprintf(stderr, " I don't recognize the format ... please try again , or recompile to use with old tipsy if that is what you use ..\n");
      MPI_Finalize();
      ::exit(-1);
    }
    rDataPtr->computeMinMax();
    rDataPtr->setNewData();
  }

  assert(rDataPtr != 0);

  auto callbackFunc = [&](const int code)
  {
    int quitL = (code == -1) || terminateRenderer;  /* exit code */
    int quitG;
    MPI_Allreduce(&quitL, &quitG, 1, MPI_INT, MPI_SUM, comm);
    if (quitG)
    {
      MPI_Finalize();
      ::exit(0);
    }

    if (inSitu )
      if (fetchSharedData(quickSync, *rDataPtr, rank, nranks, comm, reduceDM, reduceS))
      {
        rDataPtr->setNewData();
      }
  };

  bonsaistd::function<void(int)> callback = callbackFunc;
  callback(0);  /* init data set */

  renderer(
      argc, argv,
      rank, nranks, comm,
      *rDataPtr,
      fullScreenMode.c_str(),
      stereo,
      callback);

//  while(1) {}
  return 0;
}


#include <math.h>
#include <vector>
#include "viewer.h"
#include <pcl/io/pcd_io.h>
#include <pcl/common/centroid.h>
#include <pcl/common/common.h>
#include <pcl/visualization/common/common.h>
#include <boost/filesystem.hpp>

using namespace ssv3d;

PCLViewer::PCLViewer(const std::string& title)
{
  m_viewer = new pcl::visualization::PCLVisualizer(title);
  m_viewer->initCameraParameters();
  m_viewer->setSize(1000,800);
  m_viewer->addCoordinateSystem (0.5, "cloud", 0);
  m_viewer->setBackgroundColor(1,1,1);
  m_viewer->setCameraPosition(5.0,-5.0,5.0,0,0,0,0,0,1,0);
}

PCLViewer::~PCLViewer()
{
  delete m_viewer;
}

bool PCLViewer::IsStop() const
{
  return m_viewer->wasStopped();
}

void PCLViewer::AddPointCloud(const WSPointCloudPtr cloud, int vp)
{
  m_viewer->removeAllShapes();
  m_viewer->removeAllPointClouds();
  std::string name("cloud_");
  name.append(std::to_string(vp));
  m_viewer->addPointCloud<WSPoint>(cloud,name,vp);
  m_viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE,1,name,vp);

  // set camera position
  Eigen::Vector4f centroid, min, max;
  pcl::compute3DCentroid(*cloud, centroid);
  pcl::getMinMax3D<WSPoint>(*cloud, min, max);
  double dRadius = 1.0*std::sqrt((max[0]-min[0])*(max[0]-min[0])+(max[1]-min[1])*(max[1]-min[1])+(max[2]-min[2])*(max[2]-min[2]));
  m_viewer->setCameraPosition(dRadius,-dRadius,dRadius,centroid[0],centroid[1],centroid[2],0,0,1,vp);
}

void PCLViewer::AddCube(const WSPoint& point, double s, int id, double r,double g, double b, int vp)
{
  std::string name("cube_");
  name.append(std::to_string(id));
  m_viewer->removeShape(name);
  m_viewer->addCube(point.x-0.5*s,point.x+0.5*s,point.y-0.5*s,point.y+0.5*s,point.z-0.5*s,point.z+0.5*s,r,g,b,name,vp);
  m_viewer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_REPRESENTATION, 1, name);
  m_viewer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_OPACITY, 0.2, name);
  m_viewer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_LINE_WIDTH, 1, name);
}

void PCLViewer::AddArrow(const WSPoint& pt, const WSNormal& normal, double length, int id, double r,double g, double b, int vp)
{
  std::string name("arrow_");
  name.append(std::to_string(id));
  m_viewer->removeShape(name);
  WSPoint pte;
  pte.x = pt.x + length*normal.normal_x;
  pte.y = pt.y + length*normal.normal_y;
  pte.z = pt.z + length*normal.normal_z;
  m_viewer->addArrow(pt,pte,r,g,b,false,name,vp);
}

void PCLViewer::AddMesh(const pcl::PolygonMesh& mesh)
{
  if (!m_viewer->updatePolygonMesh(mesh, "mesh"))
  {
    m_viewer->addPolygonMesh(mesh, "mesh");
    m_viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "mesh");
  }
}

void PCLViewer::AddCoordinateSystem(const Eigen::Affine3f& camPose, int idIndex, double scale,int vp, bool removeall)
{
  std::string name("ccs");
  name.append(std::to_string(vp));
  name.append(std::to_string(idIndex));
  m_viewer->addCoordinateSystem(scale,camPose,name,vp);
}

void PCLViewer::SpinOnce(double duration)
{
  m_viewer->spinOnce(duration);
}

void PCLViewer::Spin() const
{
  m_viewer->spin();
}

void PCLViewer::AddText(const std::string& text, const std::string& id, int vp)
{
    m_viewer->addText(text,50,50,30,0.0,0.0,0.0,id,vp);
}

void PCLViewer::AddLine(const WSPoint& startPt, const WSPoint& endPt, int idx, double r,double g, double b, int vp)
{
  std::string id("line_");
  id.append(std::to_string(idx));
  m_viewer->addLine(startPt,endPt,r,g,b,id,vp);
  m_viewer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_LINE_WIDTH, 5, id);
}

void PCLViewer::AddPolygon(const WSPointCloudPtr& polygon, const std::string& id, double r, double g, double b, int vp)
{
  m_viewer->addPolygon<WSPoint>(polygon,r,g,b,id,vp);
  m_viewer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_REPRESENTATION, 1, id);
  m_viewer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_OPACITY, 0.2, id);
  m_viewer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_LINE_WIDTH, 2, id);
}

void PCLViewer::AddNormals(const WSPointCloudPtr cloud, const WSPointCloudNormalPtr normal, int size, double arrow, int vp)
{
  std::string name("normal_");
  name.append(std::to_string(vp));
  m_viewer->addPointCloudNormals<WSPoint, WSNormal>(cloud,normal,size,arrow,name,vp);
}


// Boost.Units - A C++ library for zero-overhead dimensional analysis and
// unit/quantity manipulation and conversion
//
// Copyright (C) 2003-2008 Matthias Christian Schabel
// Copyright (C) 2007-2008 Steven Watanabe
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

// No include guards.  This header is intended to be included
// multiple times.

// imperial units

#if 0

#if defined(BOOST_UNITS_BASE_UNITS_IMPERIAL_PINT_HPP_INCLUDED) && defined(BOOST_UNITS_BASE_UNITS_IMPERIAL_GALLON_HPP_INCLUDED) &&\
    !defined(BOOST_BOOST_UNITS_BASE_UNITS_IMPERIAL_PINT_TO_GALLON_CONVERSION_DEFINED)
    #define BOOST_BOOST_UNITS_BASE_UNITS_IMPERIAL_PINT_TO_GALLON_CONVERSION_DEFINED
    #include <boost/units/conversion.hpp>
    BOOST_UNITS_DEFINE_CONVERSION_FACTOR(boost::units::imperial::pint_base_unit,boost::units::imperial::gallon_base_unit, double, 1./8.);
#endif

#if defined(BOOST_UNITS_BASE_UNITS_IMPERIAL_PINT_HPP_INCLUDED) && defined(BOOST_UNITS_BASE_UNITS_IMPERIAL_QUART_HPP_INCLUDED) &&\
    !defined(BOOST_BOOST_UNITS_BASE_UNITS_IMPERIAL_PINT_TO_QUART_CONVERSION_DEFINED)
    #define BOOST_BOOST_UNITS_BASE_UNITS_IMPERIAL_PINT_TO_QUART_CONVERSION_DEFINED
    #include <boost/units/conversion.hpp>
    BOOST_UNITS_DEFINE_CONVERSION_FACTOR(boost::units::imperial::pint_base_unit,boost::units::imperial::quart_base_unit, double, 1./2.);
#endif

#if defined(BOOST_UNITS_BASE_UNITS_IMPERIAL_PINT_HPP_INCLUDED) && defined(BOOST_UNITS_BASE_UNITS_IMPERIAL_GILL_HPP_INCLUDED) &&\
    !defined(BOOST_BOOST_UNITS_BASE_UNITS_IMPERIAL_PINT_TO_GILL_CONVERSION_DEFINED)
    #define BOOST_BOOST_UNITS_BASE_UNITS_IMPERIAL_PINT_TO_GILL_CONVERSION_DEFINED
    #include <boost/units/conversion.hpp>
    BOOST_UNITS_DEFINE_CONVERSION_FACTOR(boost::units::imperial::pint_base_unit,boost::units::imperial::gill_base_unit, double, 4.);
#endif

#if defined(BOOST_UNITS_BASE_UNITS_IMPERIAL_PINT_HPP_INCLUDED) && defined(BOOST_UNITS_BASE_UNITS_IMPERIAL_FLUID_OUNCE_HPP_INCLUDED) &&\
    !defined(BOOST_BOOST_UNITS_BASE_UNITS_IMPERIAL_PINT_TO_FLUID_OUNCE_CONVERSION_DEFINED)
    #define BOOST_BOOST_UNITS_BASE_UNITS_IMPERIAL_PINT_TO_FLUID_OUNCE_CONVERSION_DEFINED
    #include <boost/units/conversion.hpp>
    BOOST_UNITS_DEFINE_CONVERSION_FACTOR(boost::units::imperial::pint_base_unit,boost::units::imperial::fluid_ounce_base_unit, double, 20.);
#endif

#endif


// Copyright 2021 iRobot Corporation. All Rights Reserved.
// @author Justin Kearns (jkearns@irobot.com)

#ifndef IROBOT_CREATE_TOOLBOX__MOTION_CONTROL__SIMPLE_GOAL_CONTROLLER_HPP_
#define IROBOT_CREATE_TOOLBOX__MOTION_CONTROL__SIMPLE_GOAL_CONTROLLER_HPP_

#include <angles/angles.h>
#include <boost/optional.hpp>
#include <geometry_msgs/msg/twist.hpp>
#include <irobot_create_toolbox/motion_control/behaviors_scheduler.hpp>
#include <tf2/utils.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>

#include <deque>
#include <functional>
#include <mutex>
#include <vector>

namespace irobot_create_toolbox
{

/**
 * @brief This class provides an API to give velocity commands given a goal and position.
 */
class SimpleGoalController
{
public:
  SimpleGoalController()
  {
  }

  /// \brief Structure to keep information for each point in commanded path
  //  including pose with position and orientation of point
  //  radius that is considered close enough to achieving the point
  //  drive_backwards whether the robot should drive backwards towards the point (for undocking)
  struct CmdPathPoint
  {
    CmdPathPoint(tf2::Transform p, float r, bool db)
    : pose(p), radius(r), drive_backwards(db) {}
    tf2::Transform pose;
    float radius;
    bool drive_backwards;
  };
  using CmdPath = std::vector<CmdPathPoint>;

  /// \brief Set goal path for controller along with max rotation and translation speed
  void initialize_goal(const CmdPath & cmd_path, double max_rotation, double max_translation)
  {
    const std::lock_guard<std::mutex> lock(mutex_);
    // Convert path points to goal points
    goal_points_.clear();
    goal_points_.resize(cmd_path.size());
    for (size_t i = 0; i < cmd_path.size(); ++i) {
      GoalPoint & gp = goal_points_[i];
      const tf2::Vector3 & pt_position = cmd_path[i].pose.getOrigin();
      gp.x = pt_position.getX();
      gp.y = pt_position.getY();
      gp.theta = tf2::getYaw(cmd_path[i].pose.getRotation());
      gp.radius = cmd_path[i].radius;
      gp.drive_backwards = cmd_path[i].drive_backwards;
    }
    navigate_state_ = NavigateStates::ANGLE_TO_GOAL;
    max_rotation_ = max_rotation;
    max_translation_ = max_translation;
  }

  /// \brief Clear goal
  void reset()
  {
    const std::lock_guard<std::mutex> lock(mutex_);
    goal_points_.clear();
  }

  // \brief Generate velocity based on current position and next goal point looking for convergence
  // with goal point based on radius.
  // \return empty optional if no goal or velocity command to get to next goal point
  BehaviorsScheduler::optional_output_t get_velocity_for_position(
    const tf2::Transform & current_pose)
  {
    BehaviorsScheduler::optional_output_t servo_vel;
    const std::lock_guard<std::mutex> lock(mutex_);
    if (goal_points_.size() == 0) {
      return servo_vel;
    }
    double current_angle = tf2::getYaw(current_pose.getRotation());
    const tf2::Vector3 & current_position = current_pose.getOrigin();
    // Generate velocity based on current position and next goal point looking for convergence
    // with goal point based on radius.
    switch (navigate_state_) {
      case NavigateStates::ANGLE_TO_GOAL:
        {
          const GoalPoint & gp = goal_points_.front();
          double dist_to_goal = std::hypot(
            gp.x - current_position.getX(),
            gp.y - current_position.getY());
          if (dist_to_goal <= gp.radius) {
            servo_vel = geometry_msgs::msg::Twist();
            navigate_state_ = NavigateStates::GO_TO_GOAL_POSITION;
          } else {
            double ang = diff_angle(gp, current_position, current_angle);
            if (gp.drive_backwards) {
              // Angle is 180 from travel direction
              ang = angles::normalize_angle(ang + M_PI);
            }
            bound_rotation(ang);
            servo_vel = geometry_msgs::msg::Twist();
            if (std::abs(ang) < TO_GOAL_ANGLE_CONVERGED) {
              navigate_state_ = NavigateStates::GO_TO_GOAL_POSITION;
            } else {
              servo_vel->angular.z = ang;
            }
          }
          break;
        }
      case NavigateStates::GO_TO_GOAL_POSITION:
        {
          const GoalPoint & gp = goal_points_.front();
          double dist_to_goal = std::hypot(
            gp.x - current_position.getX(),
            gp.y - current_position.getY());
          double ang = diff_angle(gp, current_position, current_angle);
          double abs_ang = std::abs(ang);
          if (gp.drive_backwards) {
            // Angle is 180 from travel direction
            abs_ang = angles::normalize_angle(abs_ang + M_PI);
          }
          servo_vel = geometry_msgs::msg::Twist();
          // If robot is close enough to goal, move to final stage
          if (dist_to_goal < goal_points_.front().radius) {
            navigate_state_ = NavigateStates::GOAL_ANGLE;
            // If robot angle has deviated too much from path, reset
          } else if (abs_ang > GO_TO_GOAL_ANGLE_TOO_FAR) {
            navigate_state_ = NavigateStates::ANGLE_TO_GOAL;
            // If niether of above conditions met, drive towards goal
          } else {
            double translate_velocity = dist_to_goal;
            if (translate_velocity > max_translation_) {
              translate_velocity = max_translation_;
            }
            if (gp.drive_backwards) {
              translate_velocity *= -1;
            }
            servo_vel->linear.x = translate_velocity;
            if (abs_ang > GO_TO_GOAL_APPLY_ROTATION_ANGLE) {
              servo_vel->angular.z = ang;
            }
          }
          break;
        }
      case NavigateStates::GOAL_ANGLE:
        {
          double ang =
            angles::shortest_angular_distance(current_angle, goal_points_.front().theta);
          bound_rotation(ang);
          if (std::abs(ang) > GOAL_ANGLE_CONVERGED) {
            servo_vel = geometry_msgs::msg::Twist();
            servo_vel->angular.z = ang;
          } else {
            goal_points_.pop_front();
            if (goal_points_.size() > 0) {
              servo_vel = geometry_msgs::msg::Twist();
              navigate_state_ = NavigateStates::ANGLE_TO_GOAL;
            }
          }
          break;
        }
    }
    return servo_vel;
  }

private:
  enum class NavigateStates
  {
    ANGLE_TO_GOAL,
    GO_TO_GOAL_POSITION,
    GOAL_ANGLE,
  };

  struct GoalPoint
  {
    double x;
    double y;
    double theta;
    float radius;
    bool drive_backwards;
  };

  void bound_rotation(double & rotation_velocity)
  {
    double abs_rot = std::abs(rotation_velocity);
    if (abs_rot > max_rotation_) {
      rotation_velocity = std::copysign(max_rotation_, rotation_velocity);
    } else if (abs_rot < MIN_ROTATION && abs_rot > 0.01) {
      // min speed if desire small non zero velocity
      rotation_velocity = std::copysign(MIN_ROTATION, rotation_velocity);
    }
  }

  double diff_angle(const GoalPoint & goal_pt, const tf2::Vector3 & cur_position, double cur_angle)
  {
    return angles::shortest_angular_distance(
      cur_angle, std::atan2(
        goal_pt.y - cur_position.getY(),
        goal_pt.x - cur_position.getX()));
  }

  std::mutex mutex_;
  std::deque<GoalPoint> goal_points_;
  NavigateStates navigate_state_;
  double max_rotation_;
  double max_translation_;
  const double MIN_ROTATION {0.1};
  const double TO_GOAL_ANGLE_CONVERGED {0.03};
  const double GO_TO_GOAL_ANGLE_TOO_FAR {M_PI / 16.0};
  const double GO_TO_GOAL_APPLY_ROTATION_ANGLE {0.02};
  const double GOAL_ANGLE_CONVERGED {0.02};
};

}  // namespace irobot_create_toolbox
#endif   // IROBOT_CREATE_TOOLBOX__MOTION_CONTROL__SIMPLE_GOAL_CONTROLLER_HPP_


#include <string>
#include <iostream>
#include <sstream>
#include <fstream>
#include <algorithm>
#include <vector>
#include <utility>

//#include <boost/thread/thread.hpp>
#include <pcl/common/common_headers.h>
#include <pcl/features/normal_3d.h>
#include <pcl/features/normal_3d_omp.h>
#include <pcl/features/shot.h>
#include <pcl/features/shot_lrf.h>
#include <pcl/features/shot_omp.h>
#include <pcl/features/shot_lrf_omp.h>
#include <pcl/features/board.h>
#include <pcl/io/pcd_io.h>
#include <pcl/common/transforms.h>
#include <pcl/console/parse.h>
#include <pcl/filters/filter.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/point_types.h>

#include "liblzf-3.6/lzf_c.c"
#include "liblzf-3.6/lzf_d.c"

#include <ros/ros.h>
#include <pcl_ros/point_cloud.h>
#include <pcl/point_types.h>
#include <boost/foreach.hpp>

#include <stdio.h>
#include <Python.h>
// #include <pyhelper.hpp>

using namespace std;

void usage(const char* program)
{
    cout << "Usage: " << program << " [options] <input.pcd>" << endl << endl;
    cout << "Options: " << endl;
    cout << "--relative If selected, scale is relative to the diameter of the model (-d). Otherwise scale is absolute." << endl;
    cout << "-r R Number of subdivisions in the radial direction. Default 17." << endl;
    cout << "-p P Number of subdivisions in the elevation direction. Default 11." << endl;
    cout << "-a A Number of subdivisions in the azimuth direction. Default 12." << endl;
    cout << "-s S Radius of sphere around each point. Default 1.18 (absolute) or 17\% of diameter (relative)." << endl;
    cout << "-d D Diameter of full model. Must be provided for relative scale." << endl;
    cout << "-m M Smallest radial subdivision. Default 0.1 (absolute) or 1.5\% of diameter (relative)." << endl;
    cout << "-l L Search radius for local reference frame. Default 0.25 (absolute) or 2\% of diameter (relative)." << endl;
    cout << "-t T Number of threads. Default 16." << endl;
    cout << "-o Output file name." << endl;
    cout << "-h Help menu." << endl;
}

vector<vector<double> > compute_intensities(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud,
                                            pcl::PointCloud<pcl::PointNormal>::Ptr normals,
                                            int num_bins_radius,
                                            int num_bins_polar,
                                            int num_bins_azimuth,
                                            double search_radius,
                                            double lrf_radius,
                                            double rmin,
                                            int num_threads)
{
    vector<vector<double> > intensities;
    intensities.resize(cloud->points.size());

    pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>());
    tree->setInputCloud(cloud);

    pcl::PointCloud<pcl::ReferenceFrame>::Ptr frames(new pcl::PointCloud<pcl::ReferenceFrame>());
    pcl::SHOTLocalReferenceFrameEstimation<pcl::PointXYZ>::Ptr lrf_estimator(new pcl::SHOTLocalReferenceFrameEstimation<pcl::PointXYZ>());
    lrf_estimator->setRadiusSearch(lrf_radius);
    lrf_estimator->setInputCloud(cloud);

    lrf_estimator->compute(*frames);

    pcl::StopWatch watch_intensities;

    double ln_rmin = log(rmin);
    double ln_rmax_rmin = log(search_radius/rmin);

    double azimuth_interval = 360.0 / num_bins_azimuth;
    double polar_interval = 180.0 / num_bins_polar;
    vector<double> radii_interval, azimuth_division, polar_division;
    for(int i = 0; i < num_bins_radius+1; i++) {
        radii_interval.push_back(exp(ln_rmin + ((double)i) / num_bins_radius * ln_rmax_rmin));
    }
    for(int i = 0; i < num_bins_azimuth + 1; i++) {
        azimuth_division.push_back(i * azimuth_interval);
    }
    for(int i = 0; i < num_bins_polar + 1; i++) {
        polar_division.push_back(i * polar_interval);
    }
    radii_interval[0] = 0;

    vector<double> integr_radius, integr_polar;
    double integr_azimuth;
    for(int i = 0; i < num_bins_radius; i++) {
        integr_radius.push_back((radii_interval[i+1]*radii_interval[i+1]*radii_interval[i+1])/3 - (radii_interval[i]*radii_interval[i]*radii_interval[i])/3 );
    }
    integr_azimuth = pcl::deg2rad(azimuth_division[1]) - pcl::deg2rad(azimuth_division[0]);
    for(int i = 0; i < num_bins_polar; i++) {
        integr_polar.push_back(cos(pcl::deg2rad(polar_division[i]))-cos(pcl::deg2rad(polar_division[i+1])));
    }

    for(int i = 0; i < cloud->points.size(); i++) {
        vector<int> indices;
        vector<float> distances;
        vector<double> intensity;
        int sum = 0;
        intensity.resize(num_bins_radius * num_bins_polar * num_bins_azimuth);

        pcl::ReferenceFrame current_frame = (*frames)[i];
        Eigen::Vector4f current_frame_x (current_frame.x_axis[0], current_frame.x_axis[1], current_frame.x_axis[2], 0);
        Eigen::Vector4f current_frame_y (current_frame.y_axis[0], current_frame.y_axis[1], current_frame.y_axis[2], 0);
        Eigen::Vector4f current_frame_z (current_frame.z_axis[0], current_frame.z_axis[1], current_frame.z_axis[2], 0);

        if(isnan(current_frame_x[0]) || isnan(current_frame_x[1]) || isnan(current_frame_x[2]) ) {
            current_frame_x[0] = 1, current_frame_x[1] = 0, current_frame_x[2] = 0;
            current_frame_y[0] = 0, current_frame_y[1] = 1, current_frame_y[2] = 0;
            current_frame_z[0] = 0, current_frame_z[1] = 0, current_frame_z[2] = 1;
        } else {
            float nx = normals->points[i].normal_x, ny = normals->points[i].normal_y, nz = normals->points[i].normal_z;
            Eigen::Vector4f n(nx, ny, nz, 0);
            if(current_frame_z.dot(n) < 0) {
                current_frame_x = -current_frame_x;
                current_frame_y = -current_frame_y;
                current_frame_z = -current_frame_z;
            }
        }

        fill(intensity.begin(), intensity.end(), 0);
        tree->radiusSearch(cloud->points[i], search_radius, indices, distances);
        for(int j = 1; j < indices.size(); j++) {
            if(distances[j] > 1E-15) {
                Eigen::Vector4f v = cloud->points[indices[j]].getVector4fMap() - cloud->points[i].getVector4fMap();
                double x_l = (double)v.dot(current_frame_x);
                double y_l = (double)v.dot(current_frame_y);
                double z_l = (double)v.dot(current_frame_z);

                double r = sqrt(x_l*x_l + y_l*y_l + z_l*z_l);
                double theta = pcl::rad2deg(acos(z_l / r));
                double phi = pcl::rad2deg(atan2(y_l, x_l));
                int bin_r = int((num_bins_radius - 1) * (log(r) - ln_rmin) / ln_rmax_rmin + 1);
                int bin_theta = int(num_bins_polar * theta / 180);
                int bin_phi = int(num_bins_azimuth * (phi + 180) / 360);

                bin_r = bin_r >= 0 ? bin_r : 0;
                bin_r = bin_r < num_bins_radius ? bin_r : num_bins_radius - 1;
                bin_theta = bin_theta < num_bins_polar ? bin_theta : num_bins_polar - 1;
                bin_phi = bin_phi < num_bins_azimuth ? bin_phi : num_bins_azimuth - 1;
                int idx = bin_r + bin_theta * num_bins_radius + bin_phi * num_bins_radius * num_bins_polar;
                intensity[idx] += 1;
                sum += 1;
            }
        }
        if(sum > 0) {
            for(int j = 0; j < intensity.size(); j++) {
                intensity[j] /= sum;
            }
        }
        intensities[i] = intensity;
    }
    pcl::console::print_highlight("Raw Spherical Histograms Time: %f (s)\n", watch_intensities.getTimeSeconds());
    return intensities;
}

typedef pcl::PointCloud<pcl::PointXYZ>::Ptr PointCloud;

void callback(PointCloud point_cloud_wf)
{
    int num_bins_radius = 17, num_bins_polar = 11, num_bins_azimuth = 12;
    int num_threads = 16;
    double search_radius = 1.18, lrf_radius = 0.25;
    double diameter = 4*sqrt(3);
    double rmin = 0.1;
    //string output_file = "/home/user/Desktop/LoopClosure/SRC/histo_output.lzf";

    bool relative_scale = 0>= 0;

    std::cout << relative_scale << std::endl;
    if(relative_scale) {
        search_radius = 0.17 * diameter;
        lrf_radius = 0.02 * diameter;
        rmin = 0.015 * diameter;
    }

    // pcl::PointCloud<pcl::PointXYZ>::Ptr point_cloud_wf (new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PointCloud<pcl::PointNormal>::Ptr cloud_with_normals(new pcl::PointCloud<pcl::PointNormal>);

    int success = 1;
    std::vector<int> indices;
    pcl::removeNaNFromPointCloud (*point_cloud_wf, *point_cloud_wf,indices);

    const float VOXEL_GRID_SIZE = 0.9f;
    pcl::VoxelGrid<pcl::PointXYZ> vox_grid;
    vox_grid.setLeafSize( VOXEL_GRID_SIZE, VOXEL_GRID_SIZE, VOXEL_GRID_SIZE );
    vox_grid.setInputCloud(point_cloud_wf);
    vox_grid.filter(*cloud);

    if(success == -1) {
        PCL_ERROR("Could not read file.");
        return;
    }

    cout<<"Calculate Spherical Coordinate"<<endl;

    vector<vector<double> > intensities = compute_intensities(cloud, cloud_with_normals,
                                                              num_bins_radius, num_bins_polar, num_bins_azimuth,
                                                              search_radius, lrf_radius,
                                                              rmin, num_threads);

    // vector<double> intensities_flat;

    cout<<"Convert to C++ vector to PyObject list"<<endl;
    PyObject *listObj = PyList_New(intensities.size()*intensities.size());
    PyObject *pName, *pModule, *pDict, *pFunc;
    PyObject *pArgs, *pValue;
    Py_Initialize();
	if (!listObj) throw logic_error("Unable to allocate memory for Python list");
    // intensities_flat.resize(intensities.size()*intensities.size();
    int cnt = 0;

    for(int i = 0; i < intensities.size(); i++) {
        for(int j = 0; j < intensities[i].size(); j++) {
            PyObject *num = PyFloat_FromDouble( (double) intensities[i][j]);
		    if (!num) {
			Py_DECREF(listObj);
			throw logic_error("Unable to allocate memory for Python list");
		    }
            PyList_SET_ITEM(listObj, cnt, num);
            cnt++;
        }
    }

    PyRun_SimpleString("import sys\n" "import os\n" "import time\n"  "import lzf\n" "import struct\n");
    PyRun_SimpleString("sys.path.append( os.path.dirname(os.getcwd()) +'/catkin_ws/src/lcROS/src/')");

    cout<<"Call to Kafka from C++"<<endl;
    pName = PyUnicode_FromString("kakfa_send_test");
    /* Error checking of pName left out */

    pModule = PyImport_Import(pName);
    Py_DECREF(pName);

    if (pModule != NULL) {
        // here pass the function name
        pFunc = PyObject_GetAttrString(pModule, "mainfunc");
        /* pFunc is a new reference */

        if (pFunc && PyCallable_Check(pFunc)) {
            pArgs = PyTuple_New(1);
            // const char* intensity_char(reinterpret_cast<const char*>(&intensities_compressed[0]));
            PyTuple_SetItem(pArgs, 0, listObj);
            pValue = PyObject_CallObject(pFunc, pArgs);
            Py_DECREF(pArgs);
            if (pValue != NULL) {
                printf("Result of call: %ld\n", PyLong_AsLong(pValue));
                Py_DECREF(pValue);
            }
            else {
                Py_DECREF(pFunc);
                Py_DECREF(pModule);
                PyErr_Print();
                fprintf(stderr,"Call failed\n");
                return;
            }
        }
        else {
            if (PyErr_Occurred())
                PyErr_Print();
            fprintf(stderr, "Cannot find function \n");
        }
        Py_XDECREF(pFunc);
        Py_DECREF(pModule);
    }
    else {
        PyErr_Print();
        fprintf(stderr, "Failed to load \n");
        return;
    }
    Py_DECREF(listObj);
    Py_Finalize();

    return;
}

int main(int argc, char** argv)
{
  cout<<"Subcribe to Kitti/velo/points"<<endl;
  ros::init(argc, argv, "sub_pcl");
  ros::NodeHandle nh;
  ros::Subscriber sub = nh.subscribe<PointCloud>("/kitti/velo/pointcloud", 1, callback);
  ros::spin();
 }


/* Copyright (c) 2017, CNRS-LAAS
All rights reserved.

Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:

 * Redistributions of source code must retain the above copyright notice, this
   list of conditions and the following disclaimer.

 * Redistributions in binary form must reproduce the above copyright notice,
   this list of conditions and the following disclaimer in the documentation
   and/or other materials provided with the distribution.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */

#include <iostream>

#include "../ext/dubins.h"
#include "../core/trajectory.hpp"
#include "../core/raster.hpp"
#include "../core/uav.hpp"
#include "../vns/vns_interface.hpp"
#include "../vns/factory.hpp"
#include "../vns/neighborhoods/dubins_optimization.hpp"
#include "../core/fire_data.hpp"
#include <boost/test/included/unit_test.hpp>

namespace SAOP {
    namespace Test {

        using namespace boost::unit_test;

        UAV uav("test", 10., 32. * M_PI / 180, 0.1);

        void test_single_point_to_observe() {
            // all points ignited at time 0, except ont at time 100
            DRaster ignitions(100, 100, 0, 0, 25);
            ignitions.set(10, 10, 100);

            DRaster elevation(100, 100, 0, 0, 25);

            auto fd = make_shared<FireData>(ignitions, elevation);

            // only interested in the point ignited at time 100
            vector<TrajectoryConfig> confs{TrajectoryConfig(uav, 100)};
            Plan p(confs, fd, TimeWindow{90, 110});

            auto vns = SAOP::build_default();

            auto res = vns->search(p, 0, 1);
//            BOOST_CHECK(res.final());

            cout << "SUCCESS" << endl;
        }

        void test_many_points_to_observe() {
            // circular firedata spread
            DRaster ignitions(100, 100, 0, 0, 1);
            for (size_t x = 0; x < 100; x++) {
                for (size_t y = 0; y < 100; y++) {
                    ignitions.set(x, y, sqrt(pow((double) x - 50, 2) + pow((double) y - 50, 2)));
                }
            }

            DRaster elevation(100, 100, 0, 0, 1);

            auto fd = make_shared<FireData>(ignitions, elevation);
            vector<TrajectoryConfig> confs{TrajectoryConfig(uav, 10)};
            Plan p(confs, fd, TimeWindow{0, 110});

            auto vns = SAOP::build_default();

            auto res = vns->search(std::move(p), 0, 1);
//            BOOST_CHECK(Plan(res.final()));

            cout << "SUCCESS" << endl;
        }

        void test_many_points_to_observe_with_start_end_positions() {
            Waypoint3d start(5, 5, 0, 0);
            Waypoint3d end(11, 11, 0, 0);

            // circular firedata spread
            DRaster ignitions(100, 100, 0, 0, 1);
            for (size_t x = 0; x < 100; x++) {
                for (size_t y = 0; y < 100; y++) {
                    ignitions.set(x, y, sqrt(pow((double) x - 50, 2) + pow((double) y - 50, 2)));
                }
            }

            DRaster elevation(100, 100, 0, 0, 1);

            auto fd = make_shared<FireData>(ignitions, elevation);
            vector<TrajectoryConfig> confs{TrajectoryConfig(
                    uav,
                    start,
                    end,
                    10)};
            Plan p(confs, fd, TimeWindow{0, 110});

            auto vns = SAOP::build_default();

            auto res = vns->search(std::move(p), 0, 1);
//            BOOST_CHECK(Plan(res.final()));

            const auto& traj = res.final().trajectories()[0];
            //ASSERT(traj[0] == start);
            //ASSERT(traj[traj.size()-1] == end);
            BOOST_CHECK(traj.insertion_range_start() == 1);
            BOOST_CHECK(traj.insertion_range_end() == traj.size() - 2);

            cout << "SUCCESS" << endl;
        }

        void test_segment_rotation() {
            for (size_t i = 0; i < 100; i++) {
                Waypoint wp(drand(-100000, 10000), drand(-100000, 100000), drand(-10 * M_PI, 10 * M_PI));
                Segment seg(wp, drand(0, 1000));

                Segment seg_rotated = uav.rotate_on_visibility_center(seg, drand(-10 * M_PI, 10 * M_PI));
                Segment seg_back = uav.rotate_on_visibility_center(seg_rotated, wp.dir);
                BOOST_CHECK(seg == seg_back);
            }
        }

        void test_projection_on_firefront() {
            // uniform propagation along the y axis
            {
                DRaster ignitions(100, 100, 0, 0, 1);
                for (size_t x = 0; x < 100; x++) {
                    for (size_t y = 0; y < 100; y++) {
                        ignitions.set(x, y, y);
                    }
                }

                DRaster elevation(100, 100, 0, 0, 1);

                FireData fd(ignitions, elevation);
                auto res = fd.project_on_fire_front(Cell{1, 1}, 50.5);
                BOOST_CHECK(res && res->y == 50);

                auto res_back = fd.project_on_fire_front(Cell{79, 1}, 50.5);
                BOOST_CHECK(res_back && res_back->y == 50);
            }

            // uniform propagation along the x axis
            {
                DRaster ignitions(10, 10, 0, 0, 1);
                for (size_t x = 0; x < 10; x++) {
                    for (size_t y = 0; y < 10; y++) {
                        ignitions.set(x, y, x);
                    }
                }

                DRaster elevation(10, 10, 0, 0, 1);

                FireData fd(ignitions, elevation);
                auto res = fd.project_on_fire_front(Cell{1, 1}, 5.5);
                BOOST_CHECK(res && res->x == 5);

                auto res_back = fd.project_on_fire_front(Cell{7, 1}, 5.5);
                BOOST_CHECK(res_back && res_back->x == 5);
            }
            // circular propagation center on (50,50)
            {
                auto dist = [](size_t x, size_t y) {
                    return sqrt(pow((double) x - 50., 2.) + pow((double) y - 50., 2.));
                };
                DRaster ignitions(100, 100, 0, 0, 1);
                for (size_t x = 0; x < 100; x++) {
                    for (size_t y = 0; y < 100; y++) {
                        ignitions.set(x, y, dist(x, y));

                    }
                }

                DRaster elevation(100, 100, 0, 0, 1);

                FireData fd(ignitions, elevation);
                for (size_t i = 0; i < 100; i++) {
                    const size_t x = rand(0, 100);
                    const size_t y = rand(0, 100);
                    auto res = fd.project_on_fire_front(Cell{x, y}, 25);
                    BOOST_CHECK(res && abs(dist(res->x, res->y) - 25) < 1.5);
                }
            }
        }

        void test_trajectory_as_waypoints() {
            Trajectory traj((TrajectoryConfig(uav)));
            traj.sampled(2);
        }

        void test_trajectory_slice() {

            TimeWindow tw1 = TimeWindow(10, 300);

            TrajectoryConfig config1 = TrajectoryConfig(uav, tw1.start, tw1.end);
            Trajectory traj = Trajectory(config1);
            traj.append_segment(Segment3d(Waypoint3d(0, 0, 0, 0)));
            traj.append_segment(Segment3d(Waypoint3d(100, 100, 0, 0), 50));
            traj.append_segment(Segment3d(Waypoint3d(300, 200, 0, 0), 50));
            traj.append_segment(Segment3d(Waypoint3d(500, 500, 0, 0)));

            Trajectory sliced1 = traj.slice(TimeWindow(tw1.start + 1, tw1.end - 1));
            Trajectory sliced2 = traj.slice(TimeWindow(tw1.start + 1, 85));

            BOOST_CHECK(sliced1.size() == traj.size() - 1);
            BOOST_CHECK(sliced2.size() == traj.size() - 2);

            BOOST_CHECK_CLOSE(traj.start_time(1), sliced1.start_time(0), 0.1);
            BOOST_CHECK_CLOSE(traj.start_time(1), sliced2.start_time(0), 0.1);
            BOOST_CHECK_CLOSE(traj.start_time(3), sliced1.start_time(2), 0.1);
            BOOST_CHECK_CLOSE(traj.start_time(2), sliced2.start_time(1), 0.1);

        }

        void test_time_window_order() {
            double s = 10;
            double e = 25;
            TimeWindow tw1 = TimeWindow(s, e);
            TimeWindow tw2 = TimeWindow(e, s);

            BOOST_CHECK(tw1.start == s);
            BOOST_CHECK(tw1.end == e);
            BOOST_CHECK(tw1.start == tw2.start);
            BOOST_CHECK(tw1.end == tw1.end);
        }

        void test_time_window() {
            double s = 10;
            double e = 25;
            TimeWindow tw1 = TimeWindow(s, e);
            TimeWindow tw2 = TimeWindow(e, s);
            TimeWindow tw3 = TimeWindow(s - 1, e);
            TimeWindow tw4 = TimeWindow(s, e + 1);

            BOOST_CHECK(tw1 == tw2);
            BOOST_CHECK(tw1 != tw3);

            BOOST_CHECK(tw3.contains(tw1));
            BOOST_CHECK(tw3.contains(tw1.center()));
            BOOST_CHECK(tw3.intersects(tw4));
            BOOST_CHECK(tw3.union_with(tw1) == tw3);
            BOOST_CHECK(tw4.intersection_with(tw3) == tw1);

            auto empty_intersect = TimeWindow(s - 1, s).intersection_with(TimeWindow(e, e + 1));
            BOOST_CHECK(empty_intersect.is_empty());
        }

        test_suite* position_manipulation_test_suite() {
            test_suite* ts2 = BOOST_TEST_SUITE("position_manipulation_tests");
            srand(time(0));
            ts2->add(BOOST_TEST_CASE(&test_trajectory_slice));
            ts2->add(BOOST_TEST_CASE(&test_time_window_order));
            ts2->add(BOOST_TEST_CASE(&test_time_window));
            ts2->add(BOOST_TEST_CASE(&test_trajectory_as_waypoints));
            ts2->add(BOOST_TEST_CASE(&test_segment_rotation));
            ts2->add(BOOST_TEST_CASE(&test_single_point_to_observe));
            ts2->add(BOOST_TEST_CASE(&test_many_points_to_observe));
            ts2->add(BOOST_TEST_CASE(&test_many_points_to_observe_with_start_end_positions));
            ts2->add(BOOST_TEST_CASE(&test_projection_on_firefront));

            return ts2;
        }
    }
}


#include <cassert>
#include <iostream>
#include <fstream>
#include <memory>

#include <Eigen/Core>
#include <Eigen/Geometry>
#include <Eigen/Dense>

#include <boost/version.hpp>
#include <boost/thread.hpp>
#include <boost/thread/future.hpp>
#include <boost/format.hpp>
#include "boost/filesystem/path.hpp"
#include "boost/filesystem/operations.hpp"

#include <ros/ros.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl_ros/transforms.h>
#include <pcl_conversions/pcl_conversions.h>
#include <sensor_msgs/PointCloud2.h>

#include "nav_msgs/Odometry.h"
#include "tf/transform_broadcaster.h"
#include "tf/transform_listener.h"

#include <pcl/filters/normal_space.h>
#include <pcl/filters/filter.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/passthrough.h>
#include <pcl/filters/random_sample.h>
#include <pcl/filters/sampling_surface_normal.h>

#include <pcl/registration/icp.h>
#include <pcl/registration/icp_nl.h>
#include <pcl/registration/gicp.h>
#include <pcl/registration/ndt.h>
#include <pcl/filters/approximate_voxel_grid.h>
#include <pcl/registration/transformation_estimation_point_to_plane.h>
#include <pcl/registration/transformation_estimation_lm.h>

#include <pcl/registration/correspondence_rejection_distance.h>
#include <pcl/registration/correspondence_estimation_backprojection.h>
#include <pcl/registration/correspondence_estimation_normal_shooting.h>

#include <pcl/features/normal_3d.h>

using namespace std;
using namespace pcl;

class Odometer
{
    typedef pcl::PointCloud<pcl::PointNormal> DP;
    typedef Eigen::Matrix<double, Eigen::Dynamic, 1> VectorX;
    typedef Eigen::Matrix<double, 4, 1> Vector4;
    typedef Eigen::Matrix<double, 4, 4> Matrix4;

    FILE* fileVar;

    ros::NodeHandle& n;
    ros::NodeHandle& nh;

    // Subscribers
    ros::Subscriber cloudSub;

    // Publisher
    ros::Publisher voxGrid_cloud;
    ros::Publisher passThrough_cloud;
    ros::Publisher normalSpaceSampling_cloud;
    ros::Publisher localmapPub;
    ros::Publisher globalmapPub;
    ros::Publisher odomPub;
    ros::Publisher do_pub;

    ros::Time mapCreationTime;

    // Main algorithm definition
    DP *localMapPointCloud;

    int inputQueueSize;

    tf::TransformListener tfListener;
    tf::TransformBroadcaster tfBroadcaster;

    boost::thread publishThread;
    boost::mutex publishLock;
    ros::Time publishStamp;

    // multi-threading mapper
    typedef boost::packaged_task<DP*> MapBuildingTask;
    typedef boost::unique_future<DP*> MapBuildingFuture;
    boost::thread mapBuildingThread;
    MapBuildingTask mapBuildingTask;
    MapBuildingFuture mapBuildingFuture;

    bool processingNewCloud;
    bool mapBuildingInProgress;

    int LocalMapNum;
    unsigned int minReadingPointCount;

    string odomFrame;
    string mapFrame;

    //Define some transformation matrix
    Eigen::Matrix4f Trans_Odom2Map;
    Eigen::Matrix4f Trans_Odom2Camera;
    Eigen::Matrix4f Trans_Camera2Map;
    Eigen::Matrix4f Trans_AsusInit2AsusFrame;
    Eigen::Matrix4f Trans_AsusInit2AsusFrame_Last;

    Eigen::Matrix4f LastTrans_Camera2Map;
    Eigen::Matrix4f CurrTrans_Camera2Map;
    Eigen::Matrix4f Relative_Trans;

    int random_sampling_num;
    double voxel_leafsize;
    bool useMap;

    double max_corresdist;
    int max_iterations;
    double max_trans_epsilon;
    int min_number_correspondences;
    double euclidean_fitness_epsilon;
    double rej_max_corresdist;
    double mapVoxelLeafSize;

    int localMapNum;

    double prvTime;
    double currTime;

    DP *tempCloud;

    nav_msgs::Odometry measurementMsg;

    int skip;
    int skipNum;
    bool refine;
    bool useRandom_sampling;
    bool useNDT;

    double max_epsilon;
    double step_size;
    double resolution;
    int max_iterations_ndt;

    tf::Transform rel_transform_last;
    Eigen::Matrix4f Trans_InterFrame_last;

  public:
    Odometer ( ros::NodeHandle& n, ros::NodeHandle& nh );
    ~Odometer();

  protected:
    void gotCloud ( const pcl::PCLPointCloud2ConstPtr& cloudMsgIn );
    void processCloud ( std::unique_ptr<DP> cloud, const std::string& scannerFrame, const ros::Time& stamp, uint32_t seq );
    DP* updateMap ( DP* newPointCloud, Eigen::Matrix4f Ticp, bool updateExisting );
    void setMap ( DP* newPointCloud );
    void publishLoop ( double publishPeriod );
    void publishTransform();
    nav_msgs::Odometry EigenMatrix2OdomMsg ( const Eigen::Matrix4f& inTr, double deltaT, const std::string& frame_id,
        const std::string& child_frame_id, const ros::Time& stamp );

};

Odometer::Odometer ( ros::NodeHandle& n, ros::NodeHandle& nh ) :
  n ( n ), nh ( nh ), localMapPointCloud ( new DP ), mapBuildingInProgress ( false ), minReadingPointCount (
    500 ), odomFrame ( "odom" ), mapFrame ( "world" ), Trans_Camera2Map ( Eigen::Matrix4f::Identity ( 4, 4 ) ), prvTime (
      0.0 ), currTime ( 0.0 ), tempCloud ( new DP )
{

  nh.param<string> ( "odomFrame", odomFrame, "camera_rgb_optical_frame" );
  nh.param<string> ( "mapFrame", mapFrame, "world" );

  nh.param<int> ( "localMapNum", localMapNum, 6000 );

  nh.param<bool> ( "useRandom_sampling", useRandom_sampling, false );
  nh.param<int> ( "random_sampling_num", random_sampling_num, 3500 );
  nh.param<double> ( "voxel_leafsize", voxel_leafsize, 0.05 );

  nh.param<double> ( "setMaxCorresDist", max_corresdist, 0.05 );
  nh.param<int> ( "setMaxIterationNum", max_iterations, 50 );
  nh.param<double> ( "setTransEpsilon", max_trans_epsilon, 1e-8 );
  nh.param<int> ( "min_number_correspondences", min_number_correspondences, 300 );
  nh.param<double> ( "euclidean_fitness_epsilon", euclidean_fitness_epsilon, 0.001 );

  nh.param<double> ( "rej_max_corresdist", rej_max_corresdist, 0.3 );
  nh.param<double> ( "mapVoxelLeafSize", mapVoxelLeafSize, 0.05 );

  nh.param<int> ( "skipNum", skipNum, 3 );
  nh.param<bool> ( "refine", refine, true );

  cloudSub = n.subscribe ( "input_cloud", inputQueueSize, &Odometer::gotCloud, this );

  voxGrid_cloud = n.advertise<sensor_msgs::PointCloud2> ( "/voxelgrid/cloud", 2, true );
  passThrough_cloud = n.advertise<sensor_msgs::PointCloud2> ( "/passThrough/cloud", 2, true );

  localmapPub = n.advertise<sensor_msgs::PointCloud2> ( "local_point_map", 2, true );
  odomPub = n.advertise<nav_msgs::Odometry> ( "icp_odometry", 50, true );
  do_pub = n.advertise<nav_msgs::Odometry> ( "icp_measurement", 50, true );

  //refreshing tf transform thread
// publishThread = boost::thread(boost::bind(&Odometer::publishLoop, this, 0.01));
  Trans_AsusInit2AsusFrame = Eigen::Matrix4f::Identity();
  Trans_AsusInit2AsusFrame_Last = Eigen::Matrix4f::Identity();
  skip = 0;

}

Odometer::~Odometer()
{
  delete tempCloud;
}

void Odometer::gotCloud ( const pcl::PCLPointCloud2ConstPtr& cloudMsgIn )
{
  if ( skip < skipNum )
  {
    skip ++;
    return;
  }
  skip = 0;

  ros::WallTime startTime = ros::WallTime::now();

  ROS_INFO_STREAM ( "Input point cloud size: "<< cloudMsgIn->width * cloudMsgIn->height );
  pcl::PCLPointCloud2 cloud_filtered;

  if ( useRandom_sampling )
  {
    pcl::RandomSample<pcl::PCLPointCloud2> sor;
    sor.setInputCloud ( cloudMsgIn );
    sor.setSample ( random_sampling_num );
    sor.setSeed ( rand() );
    sor.filter ( cloud_filtered );
    double deltaT = ( ros::WallTime::now() - startTime ).toSec();
    ROS_INFO_STREAM ( "Random downsampling took: " << deltaT << "s " );
  }
  else
  {
    pcl::VoxelGrid<pcl::PCLPointCloud2> sor;
    sor.setInputCloud ( cloudMsgIn );
    sor.setLeafSize ( voxel_leafsize, voxel_leafsize, voxel_leafsize );
    sor.filter ( cloud_filtered );
    double deltaT = ( ros::WallTime::now() - startTime ).toSec();
    ROS_INFO_STREAM ( "VoxelGrid downsampling took: " << deltaT << "s " );
    ROS_INFO_STREAM ( "Cloud size after downsampling: " << cloud_filtered.width * cloud_filtered.height );
  }

  pcl::PointCloud<pcl::PointXYZ>::Ptr src ( new pcl::PointCloud<pcl::PointXYZ> );
  pcl::fromPCLPointCloud2 ( cloud_filtered, *src );

  pcl::PointCloud<pcl::PointNormal>::Ptr ptCloud ( new pcl::PointCloud<pcl::PointNormal> );
  pcl::copyPointCloud ( *src, *ptCloud );

  std::unique_ptr<DP> cloud ( new DP ( *ptCloud ) );
  processCloud ( std::move ( cloud ), cloudMsgIn->header.frame_id, pcl_conversions::fromPCL ( cloud_filtered.header ).stamp,
                 cloudMsgIn->header.seq );

  double dt = ( ros::WallTime::now() - startTime ).toSec();
  ROS_INFO_STREAM ( "Total ICP Odometry Estimation took: " << dt << "s " << endl );
}

void Odometer::processCloud ( unique_ptr<DP> newPointCloud, const std::string& scannerFrame, const ros::Time& stamp,
                              uint32_t seq )
{
  currTime = stamp.toSec();

  processingNewCloud = true;
  mapCreationTime = stamp;

  // Convert point cloud
  if ( newPointCloud->size() == 0 )
  {
    ROS_ERROR ( "I found no good points in the cloud" );
    return;
  }

  string reason;
  if ( localMapPointCloud->size() == 0 )
  {
    publishLock.lock();
    Trans_Odom2Map = Eigen::Matrix4f::Identity();
    publishLock.unlock();
  }

  // Fetch transformation from scanner to odom
  if ( !tfListener.canTransform ( scannerFrame, odomFrame, stamp, &reason ) )
  {
    ROS_ERROR_STREAM ( "Cannot lookup TOdomToScanner(" << odomFrame<< " to " << scannerFrame << "):\n" << reason );
    return;
  }
  ROS_INFO_STREAM ( "scannerFrame is: " << scannerFrame );

  tf::StampedTransform trans_camera_to_Odom;
  try
  {
    tfListener.lookupTransform ( scannerFrame, odomFrame, stamp, trans_camera_to_Odom );
  }
  catch ( tf::TransformException ex )
  {
    ROS_ERROR ( "%s", ex.what() );
  }
  pcl_ros::transformAsMatrix ( trans_camera_to_Odom, Trans_Odom2Camera );
  ROS_DEBUG_STREAM ( "Trans_Odom2Camera is: " << Trans_Odom2Camera );

  pcl::PointCloud<pcl::PointNormal>::Ptr Original_Reading ( new pcl::PointCloud<pcl::PointNormal> );
  pcl::copyPointCloud ( *newPointCloud, *Original_Reading );

  //Step1: Filtering the points along a specified dimension and and the accepted interval values are set to (0, 7)
  //Because the measurement bigger than 7 meters are too noisy

  pcl::PassThrough<pcl::PointNormal> pass;
  pass.setInputCloud ( Original_Reading );
  pass.setFilterFieldName ( "z" );
  pass.setFilterLimits ( 0.0, 7.0 );
  pass.filter ( *Original_Reading );
  ROS_INFO_STREAM ( "Point cloud size after PassThrough filtering: " << Original_Reading->points.size() );

  sensor_msgs::PointCloud2 cloud_filtered;
  pcl::toROSMsg ( *Original_Reading, cloud_filtered );
  passThrough_cloud.publish ( cloud_filtered );

  if ( Original_Reading->size() < minReadingPointCount )
  {
    ROS_ERROR_STREAM ( "Not enough points in the newPointCloud: only " << Original_Reading->size() << "pts." );
    return;
  }

  // Initialize the map if empty
  if ( localMapPointCloud->size() == 0 )
  {
    DP *tempCloud ( new DP );
    pcl::copyPointCloud ( *Original_Reading, *tempCloud );
    setMap ( updateMap ( tempCloud, Trans_Camera2Map, false ) ); //The map's Coordinate is Global
    ROS_DEBUG_STREAM ( "create initial map successfuly!" );
    LastTrans_Camera2Map = Eigen::Matrix4f::Identity ( 4, 4 );
    return;
  }

  // if the future has completed, use the new map
  if ( mapBuildingInProgress && useMap && ( mapBuildingFuture.has_value() ) )
  {
    setMap ( mapBuildingFuture.get() );
    mapBuildingInProgress = false;
  }

  //Step4: Transform the selected new reading cloud into Global Map coordinate
  pcl::PointCloud<pcl::PointNormal>::Ptr src ( new pcl::PointCloud<pcl::PointNormal> );
  pcl::PointCloud<pcl::PointNormal>::Ptr tgt ( new pcl::PointCloud<pcl::PointNormal> );

  if ( refine )
  {
    // Fetch the relative transform between previous frame and current frame computed by depth flow method
    tfListener.waitForTransform ( "/camera_base", "/cam_zforward",stamp, ros::Duration ( 0.01 ) );
    if ( !tfListener.canTransform ( "camera_base", "cam_zforward",  stamp, &reason ) )
    {
      ROS_ERROR_STREAM ( "Cannot lookup camera_init to camera):\n" << reason );
      return;
    }
    tf::StampedTransform trans_asusInit2asusFrame;
    try
    {
      tfListener.lookupTransform ( "camera_base", "cam_zforward",  stamp, trans_asusInit2asusFrame );
    }
    catch ( tf::TransformException ex )
    {
      ROS_ERROR ( "%s", ex.what() );
    }
    pcl_ros::transformAsMatrix ( trans_asusInit2asusFrame, Trans_AsusInit2AsusFrame );
    ROS_DEBUG_STREAM ( "Trans_AsusInit2AsusFrame is: " << endl << Trans_AsusInit2AsusFrame );

    // The relative transform from current to previous frame, compute its inverse because the direction is opposite
    Eigen::Matrix4f Trans_InterFrame;
    Trans_InterFrame = ( Trans_AsusInit2AsusFrame_Last.inverse () * Trans_AsusInit2AsusFrame ).inverse();

    ROS_DEBUG_STREAM ( "Trans_AsusInit2AsusFrame_Last: " << endl << Trans_AsusInit2AsusFrame_Last );
    ROS_DEBUG_STREAM ( "Trans_InterFrame is: " << endl << Trans_InterFrame );

//     // Add the relative transform to previous global transform as the initial guess of ICP
//     tf::Transform rel_transform;
//     tf::Matrix3x3 tf_r;
//     for ( int i = 0; i < 3; i ++ )
//       for ( int j = 0; j < 3; j ++ )
//         tf_r[i][j] = Trans_InterFrame ( i,j );
//
//     tf::Vector3 tf_t;
//     tf_t[0] = Trans_InterFrame ( 0,3 );
//     tf_t[1] = Trans_InterFrame ( 1,3 );
//     tf_t[2] = Trans_InterFrame ( 2,3 );
//
//     rel_transform.setOrigin ( tf_t );
//     rel_transform.setBasis ( tf_r );
//
//     double yaw, pitch, roll;
//     rel_transform.getBasis().getRPY ( roll, pitch, yaw );
//
//     if ( rel_transform.getOrigin().length() <= 0.05
//          && std::abs ( yaw ) <= 0.05 )
//     {
//       Trans_Camera2Map = Trans_InterFrame * Trans_Camera2Map;
//     }
//     else
//     {
//       Trans_Camera2Map = Trans_InterFrame_last * Trans_Camera2Map;
//     }
//
//     Trans_InterFrame_last = Trans_InterFrame;

    Trans_Camera2Map = Trans_InterFrame * Trans_Camera2Map;
  }

  Eigen::Matrix4f Incr_Trans_Camera2Map;
  Incr_Trans_Camera2Map = LastTrans_Camera2Map.inverse() * Trans_Camera2Map;

  // Add the relative transform to previous global transform as the initial guess of ICP
  tf::Transform rel_transform;
  tf::Matrix3x3 tf_r;
  for ( int i = 0; i < 3; i ++ )
    for ( int j = 0; j < 3; j ++ )
      tf_r[i][j] = Incr_Trans_Camera2Map ( i,j );

  tf::Vector3 tf_t;
  tf_t[0] = Incr_Trans_Camera2Map ( 0,3 );
  tf_t[1] = Incr_Trans_Camera2Map ( 1,3 );
  tf_t[2] = Incr_Trans_Camera2Map ( 2,3 );

  rel_transform.setOrigin ( tf_t );
  rel_transform.setBasis ( tf_r );

  double yaw, pitch, roll;
  rel_transform.getBasis().getRPY ( roll, pitch, yaw );

  if ( rel_transform.getOrigin().length() >= 0.05 || std::abs ( yaw ) >= 0.05 || refine == false)
  {

    pcl::transformPointCloud ( *Original_Reading, *src,  Trans_Camera2Map );
    pcl::copyPointCloud ( *localMapPointCloud, *tgt );

    ROS_INFO_STREAM ( "reading cloud size is: " << src->size() );
    ROS_INFO_STREAM ( "reference cloud size is: " << tgt->size() );

    // Reading cloud prepared for iteration
    pcl::PointCloud<pcl::PointNormal>::Ptr input_transformed ( src );

    //Define temporary transformation and final transformation
//     Eigen::Matrix4f transformation_ = Eigen::Matrix4f::Identity();
    Eigen::Matrix4f final_transformation_ = Eigen::Matrix4f::Identity ( 4, 4 );
//     Eigen::Matrix4f pre_transformation_ = Eigen::Matrix4f::Identity ( 4, 4 );

      /*****************************************PCL ICP Solution************************************************
      int nr_iterations_ = 0;
      bool converged_ = false;

      //Step5: Find closest point correspondences in source and target point cloud
      boost::shared_ptr<pcl::Correspondences> correspondences ( new pcl::Correspondences );
      pcl::registration::CorrespondenceEstimation<pcl::PointNormal, pcl::PointNormal> corr_est;
      corr_est.setInputSource ( input_transformed );
      corr_est.setInputTarget ( tgt );

      pcl::registration::DefaultConvergenceCriteria<float>::Ptr convergence_criteria_ (
        new pcl::registration::DefaultConvergenceCriteria<float> ( nr_iterations_, transformation_,
            *correspondences ) );
      convergence_criteria_->setMaximumIterations ( max_iterations );
      convergence_criteria_->setRelativeMSE ( euclidean_fitness_epsilon );
      convergence_criteria_->setTranslationThreshold ( max_trans_epsilon );
      convergence_criteria_->setRotationThreshold ( 1.0 - max_trans_epsilon );

      //Step6: Reject correspondence outliers using different methods
      typedef pcl::registration::CorrespondenceRejector::Ptr CorrespondenceRejectorPtr;
      std::vector<CorrespondenceRejectorPtr> correspondence_rejectors_;

      boost::shared_ptr<pcl::registration::CorrespondenceRejectorDistance> rejector_distance (
        new pcl::registration::CorrespondenceRejectorDistance );
      rejector_distance->setInputSource<pcl::PointNormal> ( input_transformed );
      rejector_distance->setInputTarget<pcl::PointNormal> ( tgt );
      rejector_distance->setMaximumDistance ( rej_max_corresdist );
      correspondence_rejectors_.push_back ( rejector_distance );

      ros::WallTime startTime = ros::WallTime::now();
      // Step7: Repeat until convergence
      do
      {
        ros::WallTime t0 = ros::WallTime::now();
        corr_est.determineCorrespondences ( *correspondences, max_corresdist );

        ros::WallTime t1 = ros::WallTime::now();
        ROS_DEBUG_STREAM ( "Finding correspondence took: " << ( t1 - t0 ).toSec() << "s " );

        //corr_est.determineReciprocalCorrespondences(*correspondences, max_corresdist);
        //ROS_DEBUG("No. of correspondences: %i ", (int) correspondences->size());

        //Reject some potential wrong correspondence according to different kind of methods
        pcl::CorrespondencesPtr temp_correspondences ( new pcl::Correspondences ( *correspondences ) );
        boost::shared_ptr<pcl::Correspondences> selected_correspondences ( new pcl::Correspondences );
        for ( size_t i = 0; i < correspondence_rejectors_.size(); ++i )
        {
          correspondence_rejectors_[i]->setInputCorrespondences ( temp_correspondences );
          correspondence_rejectors_[i]->getCorrespondences ( *correspondences );
          // Modify input for the next iteration
          if ( i < correspondence_rejectors_.size() - 1 )
            *temp_correspondences = *correspondences;
        }
        size_t cnt = correspondences->size();
        //ROS_INFO("Selected point correspondence number is: %i ", (int ) cnt);

        ros::WallTime t2 = ros::WallTime::now();
        ROS_DEBUG_STREAM ( "Rejecting correspondence took: " << ( t2 - t1 ).toSec() << "s " );

        // Check whether we have enough correspondences
        if ( ( int ) cnt < min_number_correspondences )
        {
          ROS_ERROR_STREAM (
            "[pcl::%s::computeTransformation] Not enough correspondences found. Relax your threshold parameters.\n" );
          convergence_criteria_->setConvergenceState (
            pcl::registration::DefaultConvergenceCriteria<float>::CONVERGENCE_CRITERIA_NO_CORRESPONDENCES );
          converged_ = false;

          final_transformation_ = pre_transformation_;
          break;
        }

        pcl::registration::TransformationEstimationPointToPlaneLLS<pcl::PointNormal, pcl::PointNormal, float> trans_est_lm;
        //pcl::registration::TransformationEstimationLM<pcl::PointNormal, pcl::PointNormal, float> trans_est_lm;

        trans_est_lm.estimateRigidTransformation ( *input_transformed, *tgt, *correspondences, transformation_ );

        ros::WallTime t3 = ros::WallTime::now();
        ROS_DEBUG_STREAM ( "Solving the equation took " << ( t3 - t2 ).toSec() << "s " );
        ROS_DEBUG_STREAM ( "                                                         " );

        // Tranform the iterative input cloud for next iteration
        pcl::transformPointCloud ( *input_transformed, *input_transformed, transformation_ );

        // Obtain the final transformation
        final_transformation_ = transformation_ * final_transformation_;

        ++nr_iterations_;

        //Check whether the iteration should be stopped
        converged_ = convergence_criteria_->hasConverged();
      }
      while ( !converged_ );

      ROS_DEBUG_STREAM ( "ICP iteration number is: " << nr_iterations_ );
      double dt = ( ros::WallTime::now() - startTime ).toSec();
      ROS_INFO_STREAM ( "ICP Iteration took: " << dt << "s " );
       **************************************************************************************************************************************/

    pcl::IterativeClosestPoint<pcl::PointNormal, pcl::PointNormal> icp;
    typedef pcl::registration::TransformationEstimationPointToPlaneLLS<pcl::PointNormal, pcl::PointNormal> PointToPlane;
    boost::shared_ptr<PointToPlane> point_to_plane ( new PointToPlane );
    icp.setTransformationEstimation ( point_to_plane );

    boost::shared_ptr<pcl::registration::CorrespondenceRejectorDistance> rejector_distance ( new pcl::registration::CorrespondenceRejectorDistance );
    rejector_distance->setInputSource<pcl::PointNormal> ( src );
    rejector_distance->setInputTarget<pcl::PointNormal> ( tgt );
    rejector_distance->setMaximumDistance (rej_max_corresdist );

    icp.addCorrespondenceRejector ( rejector_distance );

    icp.setInputSource ( src );
    icp.setInputTarget ( tgt );

    // Set the max correspondence distance to 5cm (e.g., correspondences with higher distances will be ignored)
    icp.setMaxCorrespondenceDistance (max_corresdist);
    // Set the maximum number of iterations (criterion 1)
    icp.setMaximumIterations ( max_iterations );
    // Set the transformation epsilon (criterion 2)
    icp.setTransformationEpsilon ( max_trans_epsilon );
    // Set the euclidean distance difference epsilon (criterion 3)
    icp.setEuclideanFitnessEpsilon ( euclidean_fitness_epsilon );

    pcl::PointCloud<pcl::PointNormal> Final;
    icp.align ( Final );
    final_transformation_ = icp.getFinalTransformation();

//     //GICP
//     pcl::GeneralizedIterativeClosestPoint<pcl::PointNormal, pcl::PointNormal> gicp;
//     //gicp.setMaxCorrespondenceDistance(0.05);
//     gicp.setInputSource ( input_transformed );
//     gicp.setInputTarget ( tgt );
//     pcl::PointCloud<pcl::PointNormal> Final1;
//     gicp.align ( Final1 );
//     std::cout << "GICP has converged:" << gicp.hasConverged() << " score: " << gicp.getFitnessScore() << std::endl;
//     final_transformation_ = gicp.getFinalTransformation();

    //get the transformation matrix between reading cloud and map cloud in Global Map Coordinate
    Trans_Camera2Map = final_transformation_ * Trans_Camera2Map;
    //std::cout << "Trans_Camera2Map" << std::endl << Trans_Camera2Map << std::endl;

    Eigen::Matrix4f Final_Trans = Trans_Camera2Map;

    nav_msgs::Odometry accumOdom;
    accumOdom = EigenMatrix2OdomMsg ( Final_Trans, 1, mapFrame, "", stamp );
    // Publish accumulate odometry
    if ( odomPub.getNumSubscribers() )
      odomPub.publish ( accumOdom );

    //Compute the relative odometry between current frame and previous frame
    double deltaT = currTime - prvTime;
    if ( deltaT > 0 )
    {
      ROS_DEBUG ( "deltaT:  %f", deltaT );

      CurrTrans_Camera2Map = Trans_Camera2Map;
      Eigen::Matrix4f temp_Trans = LastTrans_Camera2Map.inverse() * CurrTrans_Camera2Map;
      Relative_Trans = temp_Trans.inverse();

      if ( do_pub.getNumSubscribers() )
        do_pub.publish ( EigenMatrix2OdomMsg ( Relative_Trans, deltaT, "prev_asus_frame", "asus_frame", stamp ) );

      LastTrans_Camera2Map = CurrTrans_Camera2Map;
    }

    // Compute tf and Publish tf
    publishLock.lock();
    tf::Matrix3x3 rot_mat ( Final_Trans ( 0, 0 ), Final_Trans ( 0, 1 ), Final_Trans ( 0, 2 ),
                            Final_Trans ( 1, 0 ), Final_Trans ( 1, 1 ), Final_Trans ( 1, 2 ),
                            Final_Trans ( 2, 0 ), Final_Trans ( 2, 1 ), Final_Trans ( 2, 2 ) );
    tf::Vector3 t ( Final_Trans ( 0, 3 ), Final_Trans ( 1, 3 ), Final_Trans ( 2, 3 ) );
    tf::Transform transform ( rot_mat, t );
    tfBroadcaster.sendTransform ( tf::StampedTransform ( transform, stamp, mapFrame, odomFrame ) );
    publishLock.unlock();
    processingNewCloud = false;

    //Local Map Based ICP. Construct global map using another thread
    if ( !mapBuildingInProgress )
    {

      tempCloud->clear();
      pcl::copyPointCloud ( *Original_Reading, *tempCloud );
      // make sure we process the last available map
      ROS_DEBUG_STREAM ( "Adding new points to the map in background" );
      ROS_DEBUG_STREAM ( "The added cloud size is: " << tempCloud->size() );

      mapBuildingTask = MapBuildingTask (
                          boost::bind ( &Odometer::updateMap, this, tempCloud, Trans_Camera2Map, true ) );
      mapBuildingFuture = mapBuildingTask.get_future();
      mapBuildingThread = boost::thread ( boost::move ( boost::ref ( mapBuildingTask ) ) );
      mapBuildingInProgress = true;
    }

  }

  prvTime = currTime;
  Trans_AsusInit2AsusFrame_Last = Trans_AsusInit2AsusFrame;
  LastTrans_Camera2Map = Trans_Camera2Map;
}

Odometer::DP* Odometer::updateMap ( DP* newPointCloud, Eigen::Matrix4f Ticp, bool updateExisting )
{
  pcl::PointCloud<pcl::PointNormal>::Ptr mapCloud ( new pcl::PointCloud<pcl::PointNormal> );
  pcl::PointCloud<pcl::PointNormal>::Ptr downSizeCloud ( new pcl::PointCloud<pcl::PointNormal> );
  pcl::PointCloud<pcl::PointNormal>::Ptr truncated_Cloud ( new pcl::PointCloud<pcl::PointNormal> );
  pcl::PointCloud<pcl::PointNormal>::Ptr finalCloud ( new pcl::PointCloud<pcl::PointNormal> );
  //DP *integralMap(new DP);

  std::unique_ptr<DP> integralMap ( new DP );

  ROS_DEBUG ( "Previous Map point cloud size is: %i", localMapPointCloud->height * localMapPointCloud->width );
  //Transform the new point cloud into Global Map Coordinate and concatenate with previous global map
  pcl::transformPointCloud ( *newPointCloud, *mapCloud, Ticp );
  ROS_DEBUG_STREAM ( "New added point cloud size is: " << mapCloud->points.size() );

  if ( useMap )
  {
    // Merge point clouds to map
    if ( updateExisting )
    {
      *mapCloud += *localMapPointCloud;
    }

    // Downsampling the global map point cloud to a fixed density
    pcl::VoxelGrid<pcl::PointNormal> sor;
    sor.setInputCloud ( mapCloud );
    sor.setLeafSize ( mapVoxelLeafSize, mapVoxelLeafSize, mapVoxelLeafSize );
    sor.filter ( *downSizeCloud );
    ROS_DEBUG_STREAM ( "Map size after downsampling is: " << downSizeCloud->points.size() );

    pcl::copyPointCloud ( *mapCloud, *downSizeCloud );

    if ( ( int ) downSizeCloud->points.size() > localMapNum )
    {
      truncated_Cloud->header.frame_id = downSizeCloud->header.frame_id;
      truncated_Cloud->height = 1;
      truncated_Cloud->is_dense = true;
      for ( int i = 0; i < localMapNum; i++ )
      {
        truncated_Cloud->push_back ( downSizeCloud->points[i] );
      }
      truncated_Cloud->header.seq = downSizeCloud->header.seq;
      truncated_Cloud->header.stamp = downSizeCloud->header.stamp;
      pcl::copyPointCloud ( *truncated_Cloud, *finalCloud );
    }
    else
    {
      pcl::copyPointCloud ( *downSizeCloud, *finalCloud );
    }

    // Calculate the Normals of the point cloud using KD-tree
    pcl::NormalEstimation<pcl::PointNormal, pcl::PointNormal> norm_est;
    norm_est.setSearchMethod ( pcl::search::KdTree<pcl::PointNormal>::Ptr ( new pcl::search::KdTree<pcl::PointNormal> ) );
    norm_est.setKSearch ( 5 );
    norm_est.setInputCloud ( finalCloud );
    norm_est.compute ( *finalCloud );

    pcl::copyPointCloud ( *finalCloud, *integralMap );
  }
  else
  {
    pcl::copyPointCloud ( *mapCloud, *integralMap );
  }

  return integralMap.release();
}

void Odometer::setMap ( DP* newPointCloud )
{
  // delete old map
  if ( localMapPointCloud )
    delete localMapPointCloud;

  // set new map
  localMapPointCloud = newPointCloud;

  sensor_msgs::PointCloud2 localMapCloudMsg;
  pcl::toROSMsg ( *localMapPointCloud, localMapCloudMsg );
  localMapCloudMsg.header.frame_id = mapFrame;

  // Publish map point cloud
  if ( localmapPub.getNumSubscribers() )
    localmapPub.publish ( localMapCloudMsg );
}

void Odometer::publishLoop ( double publishPeriod )
{
  if ( publishPeriod == 0 )
    return;
  ros::Rate r ( 1.0 / publishPeriod );
  while ( ros::ok() )
  {
    publishTransform();
    r.sleep();
  }
}

void Odometer::publishTransform()
{

  if ( processingNewCloud == false )
  {
    this->publishLock.lock();

    tf::Matrix3x3 rot_mat ( Trans_Camera2Map ( 0, 0 ), Trans_Camera2Map ( 0, 1 ), Trans_Camera2Map ( 0, 2 ),
                            Trans_Camera2Map ( 1, 0 ), Trans_Camera2Map ( 1, 1 ), Trans_Camera2Map ( 1, 2 ),
                            Trans_Camera2Map ( 2, 0 ), Trans_Camera2Map ( 2, 1 ), Trans_Camera2Map ( 2, 2 ) );
    tf::Vector3 t ( Trans_Camera2Map ( 0, 3 ), Trans_Camera2Map ( 1, 3 ), Trans_Camera2Map ( 2, 3 ) );
    tf::Transform transform ( rot_mat, t );
    // Note: we use now as timestamp to refresh the tf and avoid other buffer to be empty
    tfBroadcaster.sendTransform ( tf::StampedTransform ( transform, ros::Time::now(), mapFrame, odomFrame ) );

    this->publishLock.unlock();
  }

}

nav_msgs::Odometry Odometer::EigenMatrix2OdomMsg ( const Eigen::Matrix4f& inTr, double deltaT,
    const std::string& frame_id, const std::string& child_frame_id, const ros::Time& stamp )
{
  nav_msgs::Odometry odom;
  odom.header.stamp = stamp;
  odom.header.frame_id = frame_id;
  odom.child_frame_id = child_frame_id;

  odom.pose.pose.position.x = inTr ( 0, 3 );
  odom.pose.pose.position.y = inTr ( 1, 3 );
  odom.pose.pose.position.z = inTr ( 2, 3 );
  odom.twist.twist.linear.x = inTr ( 0, 3 ) / deltaT;
  odom.twist.twist.linear.y = inTr ( 1, 3 ) / deltaT;
  odom.twist.twist.linear.z = inTr ( 2, 3 ) / deltaT;

  Eigen::Matrix3d R;
  for ( int row = 0; row < 3; row++ )
  {
    for ( int col = 0; col < 3; col++ )
    {
      R ( row, col ) = inTr ( row, col );
    }
  }

  // rotate coordinate frame so that look vector is +X, and up is +Z
  Eigen::Matrix3d M;
  M <<  0,  0, 1,
    1,  0, 0,
    0,  1, 0;

  R = R * M.transpose ();

  std::vector<float> euler ( 3 );
  euler[0] = atan ( R ( 1, 2 ) / R ( 2, 2 ) );
  euler[1] = asin ( -R ( 0, 2 ) );
  euler[2] = atan ( R ( 0, 1 ) / R ( 0, 0 ) );

  Eigen::Quaterniond quat ( R );

  odom.pose.pose.orientation.x = ( double ) quat.x();
  odom.pose.pose.orientation.y = ( double ) quat.y();
  odom.pose.pose.orientation.z = ( double ) quat.z();
  odom.pose.pose.orientation.w = ( double ) quat.w();

  odom.twist.twist.angular.x = euler[0] / deltaT;
  odom.twist.twist.angular.y = euler[1] / deltaT;
  odom.twist.twist.angular.z = euler[2] / deltaT;

  odom.pose.covariance[0 + 0 * 6] = 0.1;
  odom.pose.covariance[1 + 1 * 6] = 0.1;
  odom.pose.covariance[2 + 2 * 6] = 0.1;
  odom.pose.covariance[3 + 3 * 6] = 0.1;
  odom.pose.covariance[4 + 4 * 6] = 0.1;
  odom.pose.covariance[5 + 5 * 6] = 0.1;
  odom.twist.covariance[0 + 0 * 6] = 0.01;
  odom.twist.covariance[1 + 1 * 6] = 0.01;
  odom.twist.covariance[2 + 2 * 6] = 0.01;
  odom.twist.covariance[3 + 3 * 6] = 0.01;
  odom.twist.covariance[4 + 4 * 6] = 0.01;
  odom.twist.covariance[5 + 5 * 6] = 0.01;

  return odom;
}

// Main function supporting the Mapper class
int main ( int argc, char **argv )
{
  ros::init ( argc, argv, "odometer" );
  ros::NodeHandle n;
  ros::NodeHandle nh ( "~" );

  //vis.setBackgroundColor(0,0,0);

  Odometer Odometer ( n, nh );
  ros::spin();

  return 0;
}


/*
 * @Descripttion:
 * @version: 1.0版本
 * @Author: Frank.Wu
 * @Date: 2019-11-18 21:31:07
 * @LastEditors: Frank.Wu
 * @LastEditTime: 2020-05-26 12:06:41
 */
#ifdef _USE_PCL_
#include "LidarRegistration.h"
#include <iostream>
#include <pcl/registration/icp.h>           //ICP类相关头文件
#include <pcl/registration/icp_nl.h>        //非线性ICP 相关头文件
#include <pcl/registration/transforms.h>      //变换矩阵类头文件

#include <pcl/features/fpfh.h>
#include <pcl/registration/ia_ransac.h>
#include <pcl/features/normal_3d.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <boost/thread/thread.hpp>
#include <pcl/features/fpfh_omp.h> //包含fpfh加速计算的omp(多核并行计算)
#include <pcl/registration/correspondence_estimation.h>
#include <pcl/registration/correspondence_rejection_features.h> //特征的错误对应关系去除
#include <pcl/registration/correspondence_rejection_sample_consensus.h> //随机采样一致性去除

using namespace std;
long LidarRegistration::LidarRegistration_ICP(pcl::PointCloud<pcl::PointXYZ>::Ptr ref_cloud,
                          pcl::PointCloud<pcl::PointXYZ>::Ptr input_cloud,
                          const char* pathRegistration)
{
    pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;   //创建IterativeClosestPoint的对象

    pcl::search::KdTree<pcl::PointXYZ>::Ptr tree1 (new pcl::search::KdTree<pcl::PointXYZ>);
	tree1->setInputCloud(input_cloud);
	pcl::search::KdTree<pcl::PointXYZ>::Ptr tree2 (new pcl::search::KdTree<pcl::PointXYZ>);
	tree2->setInputCloud(ref_cloud);
	icp.setSearchMethodSource(tree1);
	icp.setSearchMethodTarget(tree2);

    icp.setInputSource(input_cloud);                                 //cloud_in设置为点云的源点
    icp.setInputTarget(ref_cloud);                                  //cloud_out设置为与cloud_in对应的匹配目标
    icp.setMaxCorrespondenceDistance(1500);
    icp.setMaximumIterations(300);
    icp.setTransformationEpsilon (1e-8);
    icp.setEuclideanFitnessEpsilon (0.1);
    pcl::PointCloud<pcl::PointXYZ> registration;                    //存储经过配准变换点云后的点云
    icp.align(registration);                                        //打印配准相关输入信息
    std::cout << "has converged:" << icp.hasConverged() <<" score: " <<icp.getFitnessScore() << std::endl;
    std::cout << icp.getFinalTransformation() << std::endl;

    //配准点云保存到文件中
    pcl::io::savePCDFileASCII (pathRegistration, registration); //将点云保存到PCD文件中
    return 0;
}

long LidarRegistration::LidarRegistration_FPFH(pcl::PointCloud<pcl::PointXYZ>::Ptr ref_cloud,
                          pcl::PointCloud<pcl::PointXYZ>::Ptr input_cloud,
                          const char* pathRegistration)
{
    clock_t start,end,time;
    start  = clock();
    pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ> ());
    pcl::PointCloud<pcl::FPFHSignature33>::Ptr source_fpfh =  compute_fpfh_feature(input_cloud,tree);
    pcl::PointCloud<pcl::FPFHSignature33>::Ptr target_fpfh =  compute_fpfh_feature(ref_cloud,tree);

    //对齐(占用了大部分运行时间)
    pcl::SampleConsensusInitialAlignment<pcl::PointXYZ,pcl::PointXYZ,pcl::FPFHSignature33> sac_ia;
    sac_ia.setInputSource(input_cloud);
    sac_ia.setSourceFeatures(source_fpfh);
    sac_ia.setInputTarget(ref_cloud);
    sac_ia.setTargetFeatures(target_fpfh);
    pcl::PointCloud<pcl::PointXYZ>::Ptr align (new pcl::PointCloud<pcl::PointXYZ>());
    //sac_ia.setNumberOfSamples(20);  //设置每次迭代计算中使用的样本数量（可省）,可节省时间
    sac_ia.setCorrespondenceRandomness(6); //设置计算协方差时选择多少近邻点，该值越大，协防差越精确，但是计算效率越低.(可省)
    sac_ia.align(*align);
    end = clock();
    pcl::io::savePCDFile (pathRegistration, *align);
    printf("calculate time is: %lf\n",float (end-start)/CLOCKS_PER_SEC);
}

/**
 * TODO:
 * 在计算FPFH特征的过程中一直提示内存溢出，另外计算的特征向量的值一直不太对，需要进一步检查
 **/
pcl::PointCloud<pcl::FPFHSignature33>::Ptr LidarRegistration::compute_fpfh_feature(pcl::PointCloud<pcl::PointXYZ>::Ptr input_cloud,pcl::search::KdTree<pcl::PointXYZ>::Ptr tree)
{
    //法向量
    pcl::PointCloud<pcl::Normal>::Ptr point_normal (new  pcl::PointCloud<pcl::Normal>());
    pcl::NormalEstimation<pcl::PointXYZ,pcl::Normal> est_normal;
    est_normal.setInputCloud(input_cloud);
    est_normal.setSearchMethod(tree);
    est_normal.setKSearch(5);
    // est_normal.setRadiusSearch(5);
    est_normal.compute(*point_normal);
    //std::cout<<point_normal->points[1000]<<std::endl;
    //fpfh 估计
    pcl::PointCloud<pcl::FPFHSignature33>::Ptr fpfh (new pcl::PointCloud<pcl::FPFHSignature33>());
    //pcl::FPFHEstimation<pcl::PointXYZ,pcl::Normal,pcl::FPFHSignature33> est_target_fpfh;
    pcl::FPFHEstimationOMP<pcl::PointXYZ,pcl::Normal,pcl::FPFHSignature33> est_fpfh;
    est_fpfh.setNumberOfThreads(4); //指定4核计算
    // pcl::search::KdTree<pcl::PointXYZ>::Ptr tree4 (new pcl::search::KdTree<pcl::PointXYZ> ());
    est_fpfh.setInputCloud(input_cloud);
    est_fpfh.setInputNormals(point_normal);
    est_fpfh.setSearchMethod(tree);
    est_fpfh.setKSearch(10);
    // est_fpfh.setRadiusSearch(8);
    est_fpfh.compute(*fpfh);
    return fpfh;
}

#endif