#ifndef PhysicsTools_Utilities_interface_Lumi3DReWeighting_cc
#define PhysicsTools_Utilities_interface_Lumi3DReWeighting_cc

/**
  \class    Lumi3DReWeighting Lumi3DReWeighting.h "PhysicsTools/Utilities/interface/Lumi3DReWeighting.h"
  \brief    Class to provide lumi weighting for analyzers to weight "flat-to-N" MC samples to data

  This class will trivially take two histograms:
  1. The generated "flat-to-N" distributions from a given processing (or any other generated input)
  2. A histogram generated from the "estimatePileup" macro here:

  https://twiki.cern.ch/twiki/bin/view/CMS/LumiCalc#How_to_use_script_estimatePileup

  and produce weights to convert the input distribution (1) to the latter (2).

  \author Salvatore Rappoccio, modified by Mike Hildreth

*/
#include "TRandom1.h"
#include "TRandom2.h"
#include "TRandom3.h"
#include "TStopwatch.h"
#include "TH1.h"
#include "TH3.h"
#include "TFile.h"
#include <iostream>
#include <string>
#include <algorithm>
#include <boost/shared_ptr.hpp>
#include "SimDataFormats/PileupSummaryInfo/interface/PileupSummaryInfo.h"
#include "PhysicsTools/Utilities/interface/Lumi3DReWeighting.h"
#include "DataFormats/Common/interface/Handle.h"
#include "DataFormats/Provenance/interface/ProcessHistory.h"
#include "DataFormats/Provenance/interface/Provenance.h"
#include "FWCore/Common/interface/EventBase.h"

using namespace edm;

Lumi3DReWeighting::Lumi3DReWeighting( std::string generatedFile,
				      std::string dataFile,
				      std::string GenHistName = "pileup",
				      std::string DataHistName = "pileup",
				      std::string WeightOutputFile ="") :
      generatedFileName_( generatedFile),
      dataFileName_     ( dataFile ),
      GenHistName_      ( GenHistName ),
      DataHistName_     ( DataHistName ),
      weightFileName_   (WeightOutputFile)
      {
	generatedFile_ = boost::shared_ptr<TFile>( new TFile(generatedFileName_.c_str()) ); //MC distribution
	dataFile_      = boost::shared_ptr<TFile>( new TFile(dataFileName_.c_str()) );      //Data distribution

	boost::shared_ptr<TH1> Data_temp = boost::shared_ptr<TH1>(  (static_cast<TH1*>(dataFile_->Get( DataHistName_.c_str() )->Clone() )) );

	boost::shared_ptr<TH1> MC_temp = boost::shared_ptr<TH1>(  (static_cast<TH1*>(generatedFile_->Get( GenHistName_.c_str() )->Clone() )) );

	MC_distr_ = boost::shared_ptr<TH1>(  (static_cast<TH1*>(generatedFile_->Get( GenHistName_.c_str() )->Clone() )) );
	Data_distr_ = boost::shared_ptr<TH1>(  (static_cast<TH1*>(dataFile_->Get( DataHistName_.c_str() )->Clone() )) );

	// MC * data/MC = data, so the weights are data/MC:

	// normalize both histograms first

	Data_distr_->Scale( 1.0/ Data_distr_->Integral() );
	MC_distr_->Scale( 1.0/ MC_distr_->Integral() );

}

Lumi3DReWeighting::Lumi3DReWeighting( const std::vector< float >& MC_distr, const std::vector< float >& Lumi_distr,
				      std::string WeightOutputFile ="") {

  weightFileName_ = WeightOutputFile;

  // no histograms for input: use vectors

  // now, make histograms out of them:

  Int_t NMCBins = MC_distr.size();

  MC_distr_ = boost::shared_ptr<TH1> ( new TH1F("MC_distr","MC dist",NMCBins,0., float(NMCBins)) );

  Int_t NDBins = Lumi_distr.size();

  Data_distr_ = boost::shared_ptr<TH1> ( new TH1F("Data_distr","Data dist",NDBins,0., float(NDBins)) );

  for(int ibin = 1; ibin<NMCBins+1; ++ibin ) {
    MC_distr_->SetBinContent(ibin,MC_distr[ibin-1]);
  }

  for(int ibin = 1; ibin<NDBins+1; ++ibin ) {
    Data_distr_->SetBinContent(ibin, Lumi_distr[ibin-1]);
  }

  // check integrals, make sure things are normalized

  float deltaH = Data_distr_->Integral();
  if(fabs(1.0 - deltaH) > 0.001 ) { //*OOPS*...
    Data_distr_->Scale( 1.0/ Data_distr_->Integral() );
  }
  float deltaMC = MC_distr_->Integral();
  if(fabs(1.0 - deltaMC) > 0.001 ) {
    MC_distr_->Scale(1.0/ MC_distr_->Integral());
  }

}

double Lumi3DReWeighting::weight3D( int pv1, int pv2, int pv3 ) {

  using std::min;

  int npm1 = min(pv1,49);
  int np0 = min(pv2,49);
  int npp1 = min(pv3,49);

  return Weight3D_[npm1][np0][npp1];

}

// This version does all of the work for you, just taking an event pointer as input

double Lumi3DReWeighting::weight3D( const edm::EventBase &e ) {

  using std::min;

  // get pileup summary information

  Handle<std::vector< PileupSummaryInfo > >  PupInfo;
  e.getByLabel(edm::InputTag("addPileupInfo"), PupInfo);

  std::vector<PileupSummaryInfo>::const_iterator PVI;

  int npm1=-1;
  int np0=-1;
  int npp1=-1;

  for(PVI = PupInfo->begin(); PVI != PupInfo->end(); ++PVI) {

    int BX = PVI->getBunchCrossing();

    if(BX == -1) {
      npm1 = PVI->getPU_NumInteractions();
    }
    if(BX == 0) {
      np0 = PVI->getPU_NumInteractions();
    }
    if(BX == 1) {
      npp1 = PVI->getPU_NumInteractions();
    }

  }

  npm1 = min(npm1,49);
  np0 = min(np0,49);
  npp1 = min(npp1,49);

  //  std::cout << " vertices " << npm1 << " " << np0 << " " << npp1 << std::endl;

  return Weight3D_[npm1][np0][npp1];

}

void Lumi3DReWeighting::weight3D_set( std::string generatedFile, std::string dataFile, std::string GenHistName, std::string DataHistName, std::string WeightOutputFile )
{

  generatedFileName_ = generatedFile;
  dataFileName_ = dataFile ;
  GenHistName_ = GenHistName ;
  DataHistName_= DataHistName ;
  weightFileName_  = WeightOutputFile;

  std::cout<< " seting values: " << generatedFileName_ << " " << dataFileName_ << " " << GenHistName_ << " " << DataHistName_ << std::endl;

  generatedFile_ = boost::shared_ptr<TFile>( new TFile(generatedFileName_.c_str()) ); //MC distribution
  dataFile_      = boost::shared_ptr<TFile>( new TFile(dataFileName_.c_str()) );      //Data distribution

  boost::shared_ptr<TH1> Data_temp = boost::shared_ptr<TH1>(  (static_cast<TH1*>(dataFile_->Get( DataHistName_.c_str() )->Clone() )) );

  boost::shared_ptr<TH1> MC_temp = boost::shared_ptr<TH1>(  (static_cast<TH1*>(generatedFile_->Get( GenHistName_.c_str() )->Clone() )) );

  MC_distr_ = boost::shared_ptr<TH1>(  (static_cast<TH1*>(generatedFile_->Get( GenHistName_.c_str() )->Clone() )) );
  Data_distr_ = boost::shared_ptr<TH1>(  (static_cast<TH1*>(dataFile_->Get( DataHistName_.c_str() )->Clone() )) );

  // MC * data/MC = data, so the weights are data/MC:

  // normalize both histograms first

  Data_distr_->Scale( 1.0/ Data_distr_->Integral() );
  MC_distr_->Scale( 1.0/ MC_distr_->Integral() );
}

void Lumi3DReWeighting::weight3D_init( float ScaleFactor ) {

  // Scale factor is used to shift target distribution (i.e. luminosity scale)  1. = no shift

  //create histogram to write output weights, save pain of generating them again...

  TH3D* WHist = new TH3D("WHist","3D weights",50,0.,50.,50,0.,50.,50,0.,50. );
  TH3D* DHist = new TH3D("DHist","3D weights",50,0.,50.,50,0.,50.,50,0.,50. );
  TH3D* MHist = new TH3D("MHist","3D weights",50,0.,50.,50,0.,50.,50,0.,50. );

  using std::min;

  if( MC_distr_->GetEntries() == 0 ) {
    std::cout << " MC and Data distributions are not initialized! You must call the Lumi3DReWeighting constructor. " << std::endl;
  }

  // arrays for storing number of interactions

  double MC_ints[50][50][50];
  double Data_ints[50][50][50];

  for (int i=0; i<50; i++) {
    for(int j=0; j<50; j++) {
      for(int k=0; k<50; k++) {
	MC_ints[i][j][k] = 0.;
	Data_ints[i][j][k] = 0.;
      }
    }
  }

  double factorial[50];
  double PowerSer[50];
  double base = 1.;

  factorial[0] = 1.;
  PowerSer[0]=1.;

  for (int i = 1; i<50; ++i) {
    base = base*float(i);
    factorial[i] = base;
  }

  double x;
  double xweight;
  double probi, probj, probk;
  double Expval, mean;
  int xi;

  // Get entries for Data, MC, fill arrays:

  int NMCbin = MC_distr_->GetNbinsX();

  for (int jbin=1;jbin<NMCbin+1;jbin++) {
    x =  MC_distr_->GetBinCenter(jbin);
    xweight = MC_distr_->GetBinContent(jbin); //use as weight for matrix

    //for Summer 11, we have this int feature:
    xi = int(x);

    // Generate Poisson distribution for each value of the mean

    mean = double(xi);

    if(mean<0.) {
      throw cms::Exception("BadInputValue") << " Your histogram generates MC luminosity values less than zero!"
						<< " Please Check.  Terminating." << std::endl;
    }

    if(mean==0.){
      Expval = 1.;
    }
    else {
      Expval = exp(-1.*mean);
    }

    base = 1.;

    for (int i = 1; i<50; ++i) {
      base = base*mean;
      PowerSer[i] = base; // PowerSer is mean^i
    }

    // compute poisson probability for each Nvtx in weight matrix

    for (int i=0; i<50; i++) {
      probi = PowerSer[i]/factorial[i]*Expval;
      for(int j=0; j<50; j++) {
	probj = PowerSer[j]/factorial[j]*Expval;
	for(int k=0; k<50; k++) {
	  probk = PowerSer[k]/factorial[k]*Expval;
	  // joint probability is product of event weights multiplied by weight of input distribution bin
	  MC_ints[i][j][k] = MC_ints[i][j][k]+probi*probj*probk*xweight;
	}
      }
    }

  }

  int NDatabin = Data_distr_->GetNbinsX();

  for (int jbin=1;jbin<NDatabin+1;jbin++) {
    mean =  (Data_distr_->GetBinCenter(jbin))*ScaleFactor;
    xweight = Data_distr_->GetBinContent(jbin);

    // Generate poisson distribution for each value of the mean

    if(mean<0.) {
      throw cms::Exception("BadInputValue") << " Your histogram generates Data luminosity values less than zero!"
						<< " Please Check.  Terminating." << std::endl;
    }

    if(mean==0.){
      Expval = 1.;
    }
    else {
      Expval = exp(-1.*mean);
    }

    base = 1.;

    for (int i = 1; i<50; ++i) {
      base = base*mean;
      PowerSer[i] = base;
    }

    // compute poisson probability for each Nvtx in weight matrix

    for (int i=0; i<50; i++) {
      probi = PowerSer[i]/factorial[i]*Expval;
      for(int j=0; j<50; j++) {
	probj = PowerSer[j]/factorial[j]*Expval;
	for(int k=0; k<50; k++) {
	  probk = PowerSer[k]/factorial[k]*Expval;
	  // joint probability is product of event weights multiplied by weight of input distribution bin
	  Data_ints[i][j][k] = Data_ints[i][j][k]+probi*probj*probk*xweight;
	}
      }
    }

  }

  for (int i=0; i<50; i++) {
    //if(i<5) std::cout << "i = " << i << std::endl;
    for(int j=0; j<50; j++) {
      for(int k=0; k<50; k++) {
	if( (MC_ints[i][j][k])>0.) {
	  Weight3D_[i][j][k]  =  Data_ints[i][j][k]/MC_ints[i][j][k];
	}
	else {
	  Weight3D_[i][j][k]  = 0.;
	}
	WHist->SetBinContent( i+1,j+1,k+1,Weight3D_[i][j][k] );
	DHist->SetBinContent( i+1,j+1,k+1,Data_ints[i][j][k] );
	MHist->SetBinContent( i+1,j+1,k+1,MC_ints[i][j][k] );
	//	if(i<5 && j<5 && k<5) std::cout << Weight3D_[i][j][k] << " " ;
      }
      //      if(i<5 && j<5) std::cout << std::endl;
    }
  }

  if(! weightFileName_.empty() ) {
    std::cout << " 3D Weight Matrix initialized! " << std::endl;
    std::cout << " Writing weights to file " << weightFileName_ << " for re-use...  " << std::endl;

    TFile * outfile = new TFile(weightFileName_.c_str(),"RECREATE");
    WHist->Write();
    MHist->Write();
    DHist->Write();
    outfile->Write();
    outfile->Close();
    outfile->Delete();
  }

  return;

}

void Lumi3DReWeighting::weight3D_init( std::string WeightFileName ) {

  TFile *infile = new TFile(WeightFileName.c_str());
  TH1F *WHist = (TH1F*)infile->Get("WHist");

  // Check if the histogram exists
  if (!WHist) {
    throw cms::Exception("HistogramNotFound") << " Could not find the histogram WHist in the file "
					      << "in the file " << WeightFileName << "." << std::endl;
  }

  for (int i=0; i<50; i++) {
    //    if(i<5) std::cout << "i = " << i << std::endl;
    for(int j=0; j<50; j++) {
      for(int k=0; k<50; k++) {
	Weight3D_[i][j][k] = WHist->GetBinContent(i+1,j+1,k+1);
	//	if(i<5 && j<5 && k<5) std::cout << Weight3D_[i][j][k] << " ";
      }
      //      if(i<5 && j<5) std::cout << std::endl;

    }
  }

  std::cout << " 3D Weight Matrix initialized! " << std::endl;

  return;

}

void Lumi3DReWeighting::weight3D_init( std::string MCWeightFileName, std::string DataWeightFileName ) {

  TFile *infileMC = new TFile(MCWeightFileName.c_str());
  TH3D *MHist = (TH3D*)infileMC->Get("MHist");

  // Check if the histogram exists
  if (!MHist) {
    throw cms::Exception("HistogramNotFound") << " Could not find the histogram MHist in the file "
					      << "in the file " << MCWeightFileName << "." << std::endl;
  }

  TFile *infileD = new TFile(DataWeightFileName.c_str());
  TH3D *DHist = (TH3D*)infileD->Get("DHist");

  // Check if the histogram exists
  if (!DHist) {
    throw cms::Exception("HistogramNotFound") << " Could not find the histogram DHist in the file "
					      << "in the file " << DataWeightFileName << "." << std::endl;
  }

  for (int i=0; i<50; i++) {
    for(int j=0; j<50; j++) {
      for(int k=0; k<50; k++) {
	Weight3D_[i][j][k] = DHist->GetBinContent(i+1,j+1,k+1)/MHist->GetBinContent(i+1,j+1,k+1);
      }
    }
  }

  std::cout << " 3D Weight Matrix initialized! " << std::endl;

  return;

}

#endif


#include "../sdp_solve.hxx"

#include <boost/filesystem.hpp>
#include <boost/filesystem/fstream.hpp>

void write_timing(const boost::filesystem::path &checkpoint_out,
                  const Block_Info &block_info, const Timers &timers,
                  const bool &debug, El::Matrix<int32_t> &block_timings)
{
  if(debug)
    {
      timers.write_profile(checkpoint_out.string() + ".profiling."
                           + std::to_string(El::mpi::Rank()));
    }

  El::Zero(block_timings);
  for(auto &index : block_info.block_indices)
    {
      block_timings(index, 0) = timers.elapsed_milliseconds(
                                  "run.step.initializeSchurComplementSolver."
                                  "Q.syrk_"
                                  + std::to_string(index))
                                + timers.elapsed_milliseconds(
                                    "run.step.initializeSchurComplementSolver."
                                    "Q.solve_"
                                    + std::to_string(index))
                                + timers.elapsed_milliseconds(
                                    "run.step.initializeSchurComplementSolver."
                                    "Q.cholesky_"
                                    + std::to_string(index));
    }
  El::AllReduce(block_timings, El::mpi::COMM_WORLD);
  if(El::mpi::Rank() == 0)
    {
      boost::filesystem::create_directories(checkpoint_out);
      boost::filesystem::path block_timings_path(checkpoint_out
                                                 / "block_timings");
      boost::filesystem::ofstream block_timings_file(block_timings_path);
      for(int64_t row = 0; row < block_timings.Height(); ++row)
        {
          block_timings_file << block_timings(row, 0) << "\n";
        }
      if(!block_timings_file.good())
        {
          throw std::runtime_error("Error when writing to: "
                                   + block_timings_path.string());
        }
    }
}


// Copyright (C) 2016 Vicente J. Botet Escriba
//
//  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)

// <experimental/monad_error.hpp>
// <experimental/optional.hpp>

#include <boost/detail/lightweight_test.hpp>
#include <experimental/monad_error.hpp>
#include <experimental/optional.hpp>

std::experimental::optional<int> twice(int i) {
  if (i==0)
  {
    return std::experimental::nullopt;
  }
  return std::experimental::make_optional(2*i);
}

std::experimental::optional<int> recover( ) {
  return std::experimental::make_optional(-1);
}
std::experimental::optional<int> fwd() {
  return std::experimental::nullopt;
}

int main()
{
  namespace stde = std::experimental;

  static_assert(stde::is_type_constructible<stde::optional<int>>::value, "ERROR");
  static_assert(stde::is_monad_error<stde::optional<stde::_t>>::value, "ERROR");

  {
    stde::optional<int> x = stde::monad_error::make_error<stde::optional>();
    stde::optional<int> y = stde::monad_error::bind(x,twice);
    BOOST_TEST(! y);
    stde::optional<int> z = stde::monad_error::catch_error(y,recover);
    BOOST_TEST(z);
    BOOST_TEST_EQ(*z,-1);
  }
  {
    int v=1;
    stde::optional<int> x = stde::make<stde::optional>(v);
    stde::optional<int> y = stde::monad_error::bind(x,twice);
    stde::optional<int> z = stde::monad_error::catch_error(y,fwd);
    BOOST_TEST(z);
    BOOST_TEST(*z == 2);
  }
  {
    int v=0;
    stde::optional<int> x = stde::make<stde::optional>(v);
    stde::optional<int> y = stde::monad_error::bind(x,twice);
    BOOST_TEST(! y);
  }

  return ::boost::report_errors();
}


/*Basic includes.*/
#include <stdio.h>
#include <iostream>
#include <algorithm>
#include <math.h>
#include <cmath>
#include <vector>

/*Includes for ROS.*/
#include <ros/ros.h>

/*Includes for Markers.*/
#include <visualization_msgs/Marker.h>
#include <visualization_msgs/MarkerArray.h>

/*Includes for GUI.*/
#include <dynamic_reconfigure/server.h>
#include <urban_road_filter/LidarFiltersConfig.h>

/*Includes for PCL.*/
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_cloud.h>
#include <pcl_ros/point_cloud.h>
#include <pcl/point_types.h>

#include "starShapedSearch.cpp"

/*ramer-douglas-peucker*/
#include <boost/geometry.hpp>
#include <boost/geometry/geometries/linestring.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/assign.hpp>

using namespace boost::assign;

typedef boost::geometry::model::d2::point_xy<float> xy;
boost::geometry::model::linestring<xy> line;
boost::geometry::model::linestring<xy> simplified;

/*Global variables.*/
int channels = 32;                                    /* The number of channels of the LIDAR .*/
std::string fixedFrame;                               /* Fixed Frame.*/
std::string topicName;                                /* subscribed topic.*/
bool x_zero_method, y_zero_method, star_shaped_method ; /*Three methods of roadside detection*/
bool blind_spots;                                     /*Vakfolt javító algoritmus.*/
int xDirection;                                       /*A vakfolt levágás milyen irányú.*/
float interval;                                       /*A LIDAR vertikális szögfelbontásának, elfogadott intervalluma.*/
float curbHeight;                                     /*Becsült minimum szegély magasság.*/
int curbPoints;                                       /*A pontok becsült száma, a szegélyen.*/
float beamZone;                                       /*A vizsgált sugárzóna mérete.*/
float angleFilter1;                                   /*X = 0 érték mellett, három pont által bezárt szög.*/
float angleFilter2;                                   /*Z = 0 érték mellett, két vektor által bezárt szög.*/
float angleFilter3;                                   /*Csaplár László kódjához szükséges. Sugár irányú határérték (fokban).*/
float min_X, max_X, min_Y, max_Y, min_Z, max_Z;       /*A vizsgált terület méretei.*/

bool polysimp_allow = true;                           /*polygon-eygszerűsítés engedélyezése*/
bool zavg_allow = true;                               /*egyszerűsített polygon z-koordinátái átlagból (engedély)*/
float polysimp = 0.5;                                 /*polygon-egyszerűsítési tényező (Ramer-Douglas-Peucker)*/
float polyz = -1.5;                                   /*manuálisan megadott z-koordináta (polygon)*/

int ghostcount = 0;                                   /* segédváltozó az elavult markerek (ghost) eltávolításához */

/*A paraméterek beállítása.*/
void paramsCallback(lidar_filters_pkg::LidarFiltersConfig &config, uint32_t level)
{
    fixedFrame = config.fixed_frame;
    topicName = config.topic_name;
    x_zero_method = config.x_zero_method;
    y_zero_method = config.z_zero_method;
    star_shaped_method  = config.star_shaped_method ;
    blind_spots = config.blind_spots;
    xDirection = config.xDirection;
    interval = config.interval;
    curbHeight = config.curb_height;
    curbPoints = config.curb_points;
    beamZone = config.beamZone;
    angleFilter1 = config.cylinder_deg_x;
    angleFilter2 = config.cylinder_deg_z;
    angleFilter3 = config.sector_deg;
    min_X = config.min_x;
    max_X = config.max_x;
    min_Y = config.min_y;
    max_Y = config.max_y;
    min_Z = config.min_z;
    max_Z = config.max_z;
    dmin_param = config.dmin_param;
    kdev_param = config.kdev_param;
    kdist_param = config.kdist_param;
    polysimp_allow = config.simple_poly_allow;
    polysimp = config.poly_s_param;
    zavg_allow = config.poly_z_avg_allow;
    polyz = config.poly_z_manual;
    ROS_INFO("Updated params %s", ros::this_node::getName().c_str());
}

/*Publish.*/
ros::Publisher pub_road;        /*Filtered points (passable roadway).*/
ros::Publisher pub_high;        /*Filtered points (not roadway).*/
ros::Publisher pub_box;         /*Szűrt pontok (nem úttest).*/
ros::Publisher pub_pobroad;     /*All points in the study area.*/
ros::Publisher pub_marker;      /*Marker.*/

/*FÜGGVÉNYEK*/
/*Recursive, fast sorting function. (1/3)*/
void swap(float *a, float *b)
{
    float t = *a;
    *a = *b;
    *b = t;
}

/*Recursive, fast sorting function. (2/3)*/
int partition(float *array3D, int arc, int piece, int low, int high)
{
    float pivot = *(array3D + arc * piece * 7 + high * 7 + 4);
    int i = (low - 1);
    for (int j = low; j <= high - 1; j++)
    {
        if (*(array3D + arc * piece * 7 + j * 7 + 4) < pivot)
        {
            i++;
            for(int sw = 0; sw < 7; sw++){
                swap(&*(array3D + arc * piece * 7 + i * 7 + sw), &*(array3D + arc * piece * 7 + j * 7 + sw));
            }
        }
    }
    for(int sw = 0; sw < 7; sw++){
        swap(&*(array3D + arc * piece * 7 + (i + 1) * 7 + sw), &*(array3D + arc * piece * 7 + high * 7 + sw));
    }
    return (i + 1);
}

/*Recursive, fast sorting function. (3/3)*/
void quickSort(float *array3D, int arc, int piece, int low, int high)
{
    if (low < high)
    {
        int pi = partition(array3D, arc, piece, low, high);
        quickSort(array3D, arc, piece, low, pi - 1);
        quickSort(array3D, arc, piece, pi + 1, high);
    }
}

/*Ez a függvény végzi a szűrést.*/
void filtered(const pcl::PointCloud<pcl::PointXYZ> &cloud)
{
    /*Segédváltozók, a "for" ciklusokhoz.*/
    int i, j, k, l;

    pcl::PointXYZ pt;                                           /*Egy db pont tárolásához szükséges.*/
    pcl::PointCloud<pcl::PointXYZ> cloud_filtered_Road;         /*Szűrt pontok (járható úttest).*/
    pcl::PointCloud<pcl::PointXYZ> cloud_filtered_ProbablyRoad; /*Szűrt pontok (nem járható úttest).*/
    pcl::PointCloud<pcl::PointXYZ> cloud_filtered_High;         /*Szűrt pontok (nem úttest).*/
    pcl::PointCloud<pcl::PointXYZ> cloud_filtered_Box;          /*A vizsgált terület, összes pontja.*/

    /*Upload the "cloud_filtered_Box" topic with the points.
    We go through the input cloud and select the points that
    match the criteria, are added to the "cloud_filtered_Box" topic.*/
    for (i = 0; i <= cloud.size(); i++)
    {
        if (cloud.points[i].x >= min_X && cloud.points[i].x <= max_X &&
            cloud.points[i].y >= min_Y && cloud.points[i].y <= max_Y &&
            cloud.points[i].z >= min_Z && cloud.points[i].z <= max_Z &&
            cloud.points[i].x + cloud.points[i].y + cloud.points[i].z != 0)
        {
            pt.x = cloud.points[i].x;
            pt.y = cloud.points[i].y;
            pt.z = cloud.points[i].z;
            cloud_filtered_Box.push_back(pt);
        }
    }

    /*A pontok darabszáma, a vizsgált területen.*/
    int piece = cloud_filtered_Box.points.size();

    /*Minimum 30 pont legyen a vizsgált területen, különben programhibák lesznek.
    Illetve nincs is értelme vizsgálni ilyen kevés pontot.*/
    if (piece >= 30)
    {
        /*Csaplár László kódjának meghívása és a szükséges határérték beállítása.*/
        if (star_shaped_method )
        {
            slope_param = angleFilter3 * (M_PI / 180);
            callback(cloud_filtered_Box);
        }

        /*
        2D tömb:
        - A pontok értékei: (0: X, 1: Y, 2: Z),
        - A pontok origótól vett távolsága: (3: D),
        - A pontok szögfelbontása: (4: Alpha),
        - Csaplár László kódja alapján felismert szegély pontok: Csoportszámok (5: Road = 1, High = 2).
        */
        float *array2D = new float[piece * 6]();

        /*A szögfüggvények, zárójelbeli értékeinek tárolásához.*/
        float bracket;

        /*Egy 1D tömb, melyben a különböző szögfelbontások találhatóak.
        Ez a LIDAR csatornaszámával egyenlő.
        Fontos, hogy 0 értékekkel töltsük fel.*/
        float angle[channels] = {0};

        /*A szögfelbontásokat tároló 1D tömböt, segít feltölteni.*/
        int index = 0;

        /*Új körvonalhoz tartozik az adott szög?*/
        int newCircle;

        /*A 2D tömb feltöltése.*/
        for (i = 0; i < piece; i++)
        {
            /*--- Az első 4 oszlop feltöltése. ---*/
            *(array2D + i * 6 + 0) = cloud_filtered_Box.points[i].x;
            *(array2D + i * 6 + 1) = cloud_filtered_Box.points[i].y;
            *(array2D + i * 6 + 2) = cloud_filtered_Box.points[i].z;
            *(array2D + i * 6 + 3) = sqrt(pow(*(array2D + i * 6 + 0), 2) + pow(*(array2D + i * 6 + 1), 2) + pow(*(array2D + i * 6 + 2), 2));

            /*--- Az 5. oszlop feltöltése. ---*/
            bracket = abs(*(array2D + i * 6 + 2)) / *(array2D + i * 6 + 3);

            /*A kerekítési hibák miatt szükséges sorok.*/
            if (bracket < -1)
                bracket = -1;
            else if (bracket > 1)
                bracket = 1;

            /*Számolás és konvertálás fokba.*/
            if (*(array2D + i * 6 + 2) < 0)
            {
                *(array2D + i * 6 + 4) = acos(bracket) * 180 / M_PI;
            }
            else if (*(array2D + i * 6 + 2) >= 0)
            {
                *(array2D + i * 6 + 4) = (asin(bracket) * 180 / M_PI) + 90;
            }

            /*A megfelelő index beállítása.*/
            /*Azt vesszük alapul, hogy az adott szög, egy új körvonahoz tartozik (azaz newCircle = 1).*/
            newCircle = 1;

            /*Ha már korábban volt ilyen érték (a meghatározott intervallumon belül), akkor ez nem egy új körív.
            Azaz "newCircle = 0", a folyamatból kiléphetünk, nincs szükség további vizsgálatra.*/
            for (j = 0; j < channels; j++)
            {
                if (angle[j] == 0)
                    break;

                if (abs(angle[j] - *(array2D + i * 6 + 4)) <= interval)
                {
                    newCircle = 0;
                    break;
                }
            }

            /*Ha ilyen érték, nem szerepel még a tömbben, akkor ez egy új körív.*/
            if (newCircle == 1)
            {
                /*Feltétel, hogy a program ne lépjen ki szegmentálási hibával.
                Ha valamilyen okból, több körív keletkezne mint 64, hiba lépne fel.*/
                if (index < channels)
                {
                    angle[index] = *(array2D + i * 6 + 4);
                    index++;
                }
            }
        }

        /*--- A 6. oszlop feltöltése. ---*/
        /*Csaplár László kódja által, magaspontoknak jelölt pontok felvétele a 2D tömbbe.*/
        if (star_shaped_method )
        {
            for (i = 0; i < padkaIDs.size(); i++)
            {
                *(array2D + padkaIDs[i] * 6 + 5) = 2;
            }
        }

        /*A szögfelbontások növekvő sorrendbe rendezése.
        A legkisebb lesz az első körív és így tovább.*/
        std::sort(angle, angle + index);

        /*
        3D tömb:
        - A pontok értékei: (0: X, 1: Y, 2: Z),
        - A pontok origótól vett távolsága, z = 0 értékkel: (3: D),
        - A pontok helyzete egy körben (360°). (4: Alpha),
        - X = 0 érték mellett, az új Y koordináták (5: új Y),
        - Csoportszámok (6: Road = 1, High = 2).
        */
        float *array3D = new float[channels * piece * 7]();

        /*Az adott köríveket tartalmazó csoportok (azaz a "channels"),
        megfelelő sorindexeinek beállításához szükséges.
        Fontos, hogy 0 értékekkel töltsük fel.*/
        int indexArray[channels] = {0};

        /*Egy 1D tömb. Az adott köríven, az origótól, a legnagyobb távolsággal rendelkező pontok értékei.*/
        float maxDistance[channels] = {0};

        /*Hibás LIDAR csatornaszám esetén szükséges.*/
        int results;

        /*A 3D tömb feltöltése.*/
        for (i = 0; i < piece; i++)
        {
            results = 0;

            /*A megfelelő körív kiválasztása.*/
            for (j = 0; j < index; j++)
            {
                if (abs(angle[j] - *(array2D + i * 6 + 4)) <= interval)
                {
                    results = 1;
                    break;
                }
            }

            if (results == 1)
            {
                /*Az értékek hozzáadás, a 2D tömbből.*/
                *(array3D + j * piece * 7 + indexArray[j] * 7 + 0) = *(array2D + i * 6 + 0); /*X koordináta.*/
                *(array3D + j * piece * 7 + indexArray[j] * 7 + 1) = *(array2D + i * 6 + 1); /*Y koordináta.*/
                *(array3D + j * piece * 7 + indexArray[j] * 7 + 2) = *(array2D + i * 6 + 2); /*Z koordináta.*/

                /*A már ismert magaspontok.*/
                if (star_shaped_method )
                    *(array3D + j * piece * 7 + indexArray[j] * 7 + 6) = *(array2D + i * 6 + 5);

                /*Itt annyi különbség lesz, hogy a "z" érték nélkül adjuk hozzá a távolságot.*/
                *(array3D + j * piece * 7 + indexArray[j] * 7 + 3) = sqrt(pow(*(array2D + i * 6 + 0), 2) + pow(*(array2D + i * 6 + 1), 2));

                /*Az 5. oszlop feltöltése, a szögekkel. 360 fokban, minden pontnak van egy szöge.*/
                bracket = (abs(*(array3D + j * piece * 7 + indexArray[j] * 7 + 0))) / (*(array3D + j * piece * 7 + indexArray[j] * 7 + 3));
                if (bracket < -1)
                    bracket = -1;
                else if (bracket > 1)
                    bracket = 1;

                if (*(array3D + j * piece * 7 + indexArray[j] * 7 + 0) >= 0 && *(array3D + j * piece * 7 + indexArray[j] * 7 + 1) <= 0)
                {
                    *(array3D + j * piece * 7 + indexArray[j] * 7 + 4) = asin(bracket) * 180 / M_PI;
                }
                else if (*(array3D + j * piece * 7 + indexArray[j] * 7 + 0) >= 0 && *(array3D + j * piece * 7 + indexArray[j] * 7 + 1) > 0)
                {
                    *(array3D + j * piece * 7 + indexArray[j] * 7 + 4) = 180 - (asin(bracket) * 180 / M_PI);
                }
                else if (*(array3D + j * piece * 7 + indexArray[j] * 7 + 0) < 0 && *(array3D + j * piece * 7 + indexArray[j] * 7 + 1) >= 0)
                {
                    *(array3D + j * piece * 7 + indexArray[j] * 7 + 4) = 180 + (asin(bracket) * 180 / M_PI);
                }
                else
                {
                    *(array3D + j * piece * 7 + indexArray[j] * 7 + 4) = 360 - (asin(bracket) * 180 / M_PI);
                }

                if (*(array3D + j * piece * 7 + indexArray[j] * 7 + 3) > maxDistance[j])
                {
                    maxDistance[j] = *(array3D + j * piece * 7 + indexArray[j] * 7 + 3);
                }

                indexArray[j]++;
            }
        }

        /*A lefoglalt terület felszabadítása.*/
        delete[] array2D;

        /*-- 1. lépés: A NEM út pontok szűrése. --*/
        int p2, p3; /*p2, p3 - A három vizsgált pontból, a második és a harmadik.*/

        /*
        --> alpha - A három pont és a két vektor által bezárt szög.
        --> x1, x2, x3 - A három pont által bezárt háromszög oldalainak hossza.
        --> curbPoints - A pontok becsült száma, a szegélyen.
        --> va1, va2, vb1, vb2 - A két vektor.
        --> max1, max2 - Nem csak a szöget, hanem a magasságot is vizsgálni kell.
        --> d - A két szélső pont közötti távolság. A LIDAR forgása és a körív szakadások miatt.
            d - Ez a változó a program későbbi részein is felhasználásra kerül.
        */
        float alpha, x1, x2, x3, va1, va2, vb1, vb2, max1, max2, d;

        /*Az összes kör vizsgálata.*/
        for (i = 0; i < index; i++)
        {
            if (x_zero_method)
            {
                /*Új Y értékek beállítása, X = 0 értékek mellett.*/
                for (j = 1; j < indexArray[i]; j++)
                {
                    *(array3D + i * piece * 7 + j * 7 + 5) = *(array3D + i * piece * 7 + (j - 1) * 7 + 5) + 0.0100;
                }

                /*A kör pontjainak vizsgálata. X = 0 módszer.*/
                for (j = curbPoints; j <= (indexArray[i] - 1) - curbPoints; j++)
                {
                    p2 = j + curbPoints / 2;
                    p3 = j + curbPoints;

                    d = sqrt(
                        pow(*(array3D + i * piece * 7 + p3 * 7 + 0) - *(array3D + i * piece * 7 + j * 7 + 0), 2) +
                        pow(*(array3D + i * piece * 7 + p3 * 7 + 1) - *(array3D + i * piece * 7 + j * 7 + 1), 2));

                    /*A távolság, 5 méternél kisebb legyen.*/
                    if (d < 5.0000)
                    {
                        x1 = sqrt(
                            pow(*(array3D + i * piece * 7 + p2 * 7 + 5) - *(array3D + i * piece * 7 + j * 7 + 5), 2) +
                            pow(*(array3D + i * piece * 7 + p2 * 7 + 2) - *(array3D + i * piece * 7 + j * 7 + 2), 2));
                        x2 = sqrt(
                            pow(*(array3D + i * piece * 7 + p3 * 7 + 5) - *(array3D + i * piece * 7 + p2 * 7 + 5), 2) +
                            pow(*(array3D + i * piece * 7 + p3 * 7 + 2) - *(array3D + i * piece * 7 + p2 * 7 + 2), 2));
                        x3 = sqrt(
                            pow(*(array3D + i * piece * 7 + p3 * 7 + 5) - *(array3D + i * piece * 7 + j * 7 + 5), 2) +
                            pow(*(array3D + i * piece * 7 + p3 * 7 + 2) - *(array3D + i * piece * 7 + j * 7 + 2), 2));

                        bracket = (pow(x3, 2) - pow(x1, 2) - pow(x2, 2)) / (-2 * x1 * x2);
                        if (bracket < -1)
                            bracket = -1;
                        else if (bracket > 1)
                            bracket = 1;

                        alpha = acos(bracket) * 180 / M_PI;

                        /*Feltétel és csoporthoz adás.*/
                        if (alpha <= angleFilter1 &&
                            (abs(*(array3D + i * piece * 7 + j * 7 + 2) - *(array3D + i * piece * 7 + p2 * 7 + 2)) >= curbHeight ||
                             abs(*(array3D + i * piece * 7 + p3 * 7 + 2) - *(array3D + i * piece * 7 + p2 * 7 + 2)) >= curbHeight) &&
                            abs(*(array3D + i * piece * 7 + j * 7 + 2) - *(array3D + i * piece * 7 + p3 * 7 + 2)) >= 0.05)
                        {
                            *(array3D + i * piece * 7 + p2 * 7 + 6) = 2;
                        }
                    }
                }
            }

            if (y_zero_method)
            {
                /*A kör pontjainak vizsgálata. Z = 0 módszer.*/
                for (j = curbPoints; j <= (indexArray[i] - 1) - curbPoints; j++)
                {
                    d = sqrt(
                        pow(*(array3D + i * piece * 7 + (j + curbPoints) * 7 + 0) - *(array3D + i * piece * 7 + (j - curbPoints) * 7 + 0), 2) +
                        pow(*(array3D + i * piece * 7 + (j + curbPoints) * 7 + 1) - *(array3D + i * piece * 7 + (j - curbPoints) * 7 + 1), 2));

                    /*A távolság, 5 méternél kisebb legyen.*/
                    if (d < 5.0000)
                    {
                        /*Kezdeti értékek beállítása.*/
                        max1 = abs(*(array3D + i * piece * 7 + j * 7 + 2)), max2 = abs(*(array3D + i * piece * 7 + j * 7 + 2));
                        va1 = 0, va2 = 0, vb1 = 0, vb2 = 0;

                        /*Az 'a' vektor és a legnagyobb magasság beállítása.*/
                        for (k = j - 1; k >= j - curbPoints; k--)
                        {
                            va1 = va1 + (*(array3D + i * piece * 7 + k * 7 + 0) - *(array3D + i * piece * 7 + j * 7 + 0));
                            va2 = va2 + (*(array3D + i * piece * 7 + k * 7 + 1) - *(array3D + i * piece * 7 + j * 7 + 1));
                            if (abs(*(array3D + i * piece * 7 + k * 7 + 2)) > max1)
                                max1 = abs(*(array3D + i * piece * 7 + k * 7 + 2));
                        }

                        /*A 'b' vektor és a legnagyobb magasság beállítása.*/
                        for (k = j + 1; k <= j + curbPoints; k++)
                        {
                            vb1 = vb1 + (*(array3D + i * piece * 7 + k * 7 + 0) - *(array3D + i * piece * 7 + j * 7 + 0));
                            vb2 = vb2 + (*(array3D + i * piece * 7 + k * 7 + 1) - *(array3D + i * piece * 7 + j * 7 + 1));
                            if (abs(*(array3D + i * piece * 7 + k * 7 + 2)) > max2)
                                max2 = abs(*(array3D + i * piece * 7 + k * 7 + 2));
                        }

                        va1 = (1 / (float)curbPoints) * va1;
                        va2 = (1 / (float)curbPoints) * va2;
                        vb1 = (1 / (float)curbPoints) * vb1;
                        vb2 = (1 / (float)curbPoints) * vb2;

                        bracket = (va1 * vb1 + va2 * vb2) / (sqrt(pow(va1, 2) + pow(va2, 2)) * sqrt(pow(vb1, 2) + pow(vb2, 2)));
                        if (bracket < -1)
                            bracket = -1;
                        else if (bracket > 1)
                            bracket = 1;

                        alpha = acos(bracket) * 180 / M_PI;

                        /*Feltétel és csoporthoz adás.*/
                        if (alpha <= angleFilter2 &&
                            (max1 - abs(*(array3D + i * piece * 7 + j * 7 + 2)) >= curbHeight ||
                             max2 - abs(*(array3D + i * piece * 7 + j * 7 + 2)) >= curbHeight) &&
                            abs(max1 - max2) >= 0.05)
                        {
                            *(array3D + i * piece * 7 + j * 7 + 6) = 2;
                        }
                    }
                }
            }
        }

        /*-- 2. lépés: Az út pontok szűrése. --*/
        /*A tömb elemeinek rendezése oly módon, hogy körönként, a szögeknek megfelelő sorrendben legyenek.*/
        for (i = 0; i < index; i++)
        {
            quickSort(array3D, i, piece, 0, indexArray[i] - 1);
        }

        /*Vakfolt keresés:
        Megvizsgáljuk a második körvonalat. (Az elsővel pontatlan.)
        Főleg a [90°-180° --- 180°-270°] tartomány és a [0°-90° --- 270°-360°] tartomány a lényeg, az autó két oldalán.
        Mind a kettő tartományban keresünk 2db magaspontot. Ha az adott tartományban az első körvonalon van két magaspont,
        a közte lévő terület nagy valószínűséggel egy vakfolt lesz.*/
        float q1 = 0, q2 = 180, q3 = 180, q4 = 360; /*A kör négy része. (1. 2. 3. 4. negyed.)*/
        int c1 = -1, c2 = -1, c3 = -1, c4 = -1;     /*A talált pontok ID-ja az első körvonalon.*/

        if (blind_spots)
        {
            for (i = 0; i < indexArray[1]; i++)
            {
                if (*(array3D + 1 * piece * 7 + i * 7 + 6) == 2)
                {
                    if (*(array3D + 1 * piece * 7 + i * 7 + 4) >= 0 && *(array3D + 1 * piece * 7 + i * 7 + 4) < 90)
                    {
                        if (*(array3D + 1 * piece * 7 + i * 7 + 4) > q1)
                        {
                            q1 = *(array3D + 1 * piece * 7 + i * 7 + 4);
                            c1 = i;
                        }
                    }
                    else if (*(array3D + 1 * piece * 7 + i * 7 + 4) >= 90 && *(array3D + 1 * piece * 7 + i * 7 + 4) < 180)
                    {
                        if (*(array3D + 1 * piece * 7 + i * 7 + 4) < q2)
                        {
                            q2 = *(array3D + 1 * piece * 7 + i * 7 + 4);
                            c2 = i;
                        }
                    }
                    else if (*(array3D + 1 * piece * 7 + i * 7 + 4) >= 180 && *(array3D + 1 * piece * 7 + i * 7 + 4) < 270)
                    {
                        if (*(array3D + 1 * piece * 7 + i * 7 + 4) > q3)
                        {
                            q3 = *(array3D + 1 * piece * 7 + i * 7 + 4);
                            c3 = i;
                        }
                    }
                    else
                    {
                        if (*(array3D + 1 * piece * 7 + i * 7 + 4) < q4)
                        {
                            q4 = *(array3D + 1 * piece * 7 + i * 7 + 4);
                            c4 = i;
                        }
                    }
                }
            }
        }

        float arcDistance;   /*Körív mérete, a megadott foknál. Fontos minden körön, ugyanakkora körív méretet vizsgálni.*/
        int notRoad;         /*Ha az adott köríven, az adott szakaszon, található magaspont, akkor 1-es értéket vesz fel, amúgy 0-át.*/
        int blindSpot;       /*Vakfoltok az autó mellett.*/
        float currentDegree; /*Az aktuális köríven, a szög nagysága.*/

        /*A körív méret meghatározása.*/
        arcDistance = ((maxDistance[0] * M_PI) / 180) * beamZone;

        /*0°-tól 360° - beamZone-ig.*/
        for (i = 0; i <= 360 - beamZone; i++)
        {
            blindSpot = 0;

            if (blind_spots)
            {
                /*Ha ezek a feltételek teljesülnek, akkor egy vakfoltba léptünk és itt nem vizsgálódunk.*/
                if (xDirection == 0)
                {
                    /*+-X irányba is vizsgáljuk a pontokat.*/
                    if ((q1 != 0 && q4 != 360 && (i <= q1 || i >= q4)) || (q2 != 180 && q3 != 180 && i >= q2 && i <= q3))
                    {
                        blindSpot = 1;
                    }
                }
                else if (xDirection == 1)
                {
                    /*+X irányba vizsgáljuk a pontokat.*/
                    if ((q2 != 180 && i >= q2 && i <= 270) || (q1 != 0 && (i <= q1 || i >= 270)))
                    {
                        blindSpot = 1;
                    }
                }
                else
                {
                    /*-X vizsgáljuk a pontokat.*/
                    if ((q4 != 360 && (i >= q4 || i <= 90)) || (q3 != 180 && i <= q3 && i >= 90))
                    {
                        blindSpot = 1;
                    }
                }
            }

            if (blindSpot == 0)
            {
                /*Alap beállítás, hogy az adott szakaszon nincs magaspont.*/
                notRoad = 0;

                /*Az első kör adott szakaszának vizsgálata.*/
                for (j = 0; *(array3D + 0 * piece * 7 + j * 7 + 4) <= i + beamZone && j < indexArray[0]; j++)
                {
                    if (*(array3D + 0 * piece * 7 + j * 7 + 4) >= i)
                    {
                        /*Nem vizsgáljuk tovább az adott szakaszt, ha találunk benne magaspontot.*/
                        if (*(array3D + 0 * piece * 7 + j * 7 + 6) == 2)
                        {
                            notRoad = 1;
                            break;
                        }
                    }
                }

                /*Ha nem találtunk az első kör, adott szakaszán magaspontot, továbbléphetünk a következő körre.*/
                if (notRoad == 0)
                {
                    /*Az első kör szakaszát elfogadjuk.*/
                    for (j = 0; *(array3D + 0 * piece * 7 + j * 7 + 4) <= i + beamZone && j < indexArray[0]; j++)
                    {
                        if (*(array3D + 0 * piece * 7 + j * 7 + 4) >= i)
                        {
                            *(array3D + 0 * piece * 7 + j * 7 + 6) = 1;
                        }
                    }

                    /*A további körök vizsgálata.*/
                    for (k = 1; k < index; k++)
                    {
                        /*Új szöget kell meghatározni, hogy a távolabbi körvonalakon is, ugyanakkora körív hosszt vizsgáljunk.*/
                        if (i == 360 - beamZone)
                        {
                            currentDegree = 360;
                        }
                        else
                        {
                            currentDegree = i + arcDistance / ((maxDistance[k] * M_PI) / 180);
                        }

                        /*Az új kör pontjait vizsgáljuk.*/
                        for (l = 0; *(array3D + k * piece * 7 + l * 7 + 4) <= currentDegree && l < indexArray[k]; l++)
                        {
                            if (*(array3D + k * piece * 7 + l * 7 + 4) >= i)
                            {
                                /*Nem vizsgáljuk tovább az adott szakaszt, ha találunk benne magaspontot.*/
                                if (*(array3D + k * piece * 7 + l * 7 + 6) == 2)
                                {
                                    notRoad = 1;
                                    break;
                                }
                            }
                        }

                        /*A többi kört nem vizsgáljuk, ha a sugár, elakadt egy magasponton.*/
                        if (notRoad == 1)
                            break;

                        /*Egyébként, elfogadjuk az adott kör, adott szakaszát.*/
                        for (l = 0; *(array3D + k * piece * 7 + l * 7 + 4) <= currentDegree && l < indexArray[k]; l++)
                        {
                            if (*(array3D + k * piece * 7 + l * 7 + 4) >= i)
                            {
                                *(array3D + k * piece * 7 + l * 7 + 6) = 1;
                            }
                        }
                    }
                }
            }
        }

        /*Ugyanaz, mint az előző, csak itt 360°-tól 0° + beamZone-ig vizsgáljuk a pontokat.*/
        for (i = 360; i >= 0 + beamZone; --i)
        {
            blindSpot = 0;

            if (blind_spots)
            {
                /*Ha ezek a feltételek teljesülnek, akkor egy vakfoltba léptünk és itt nem vizsgálódunk.*/
                if (xDirection == 0)
                {
                    /*+-X irányba is vizsgáljuk a pontokat.*/
                    if ((q1 != 0 && q4 != 360 && (i <= q1 || i >= q4)) || (q2 != 180 && q3 != 180 && i >= q2 && i <= q3))
                    {
                        blindSpot = 1;
                    }
                }
                else if (xDirection == 1)
                {
                    /*+X irányba vizsgáljuk a pontokat.*/
                    if ((q2 != 180 && i >= q2 && i <= 270) || (q1 != 0 && (i <= q1 || i >= 270)))
                    {
                        blindSpot = 1;
                    }
                }
                else
                {
                    /*-X vizsgáljuk a pontokat.*/
                    if ((q4 != 360 && (i >= q4 || i <= 90)) || (q3 != 180 && i <= q3 && i >= 90))
                    {
                        blindSpot = 1;
                    }
                }
            }

            if (blindSpot == 0)
            {
                /*Alap beállítás, hogy az adott szakaszon nincs magaspont.*/
                notRoad = 0;

                /*Az első kör adott szakaszának vizsgálata.*/
                for (j = indexArray[0] - 1; *(array3D + 0 * piece * 7 + j * 7 + 4) >= i - beamZone && j >= 0; --j)
                {
                    if (*(array3D + 0 * piece * 7 + j * 7 + 4) <= i)
                    {
                        /*Nem vizsgáljuk tovább az adott szakaszt, ha találunk benne magaspontot.*/
                        if (*(array3D + 0 * piece * 7 + j * 7 + 6) == 2)
                        {
                            notRoad = 1;
                            break;
                        }
                    }
                }

                /*Ha nem találtunk az első kör, adott szakaszán magaspontot, továbbléphetünk a következő körre.*/
                if (notRoad == 0)
                {
                    /*Az első kör szakaszát elfogadjuk.*/
                    for (j = indexArray[0] - 1; *(array3D + 0 * piece * 7 + j * 7 + 4) >= i - beamZone && j >= 0; --j)
                    {
                        if (*(array3D + 0 * piece * 7 + j * 7 + 4) <= i)
                        {
                            *(array3D + 0 * piece * 7 + j * 7 + 6) = 1;
                        }
                    }

                    /*A további körök vizsgálata.*/
                    for (k = 1; k < index; k++)
                    {
                        /*Új szöget kell meghatározni, hogy a távolabbi körvonalakon is, ugyanakkora körív hosszt vizsgáljunk.*/
                        if (i == 0 + beamZone)
                        {
                            currentDegree = 0;
                        }
                        else
                        {
                            currentDegree = i - arcDistance / ((maxDistance[k] * M_PI) / 180);
                        }

                        /*Az új kör pontjait vizsgáljuk.*/
                        for (l = indexArray[k] - 1; *(array3D + k * piece * 7 + l * 7 + 4) >= currentDegree && l >= 0; --l)
                        {
                            if (*(array3D + k * piece * 7 + l * 7 + 4) <= i)
                            {
                                /*Nem vizsgáljuk tovább az adott szakaszt, ha találunk benne magaspontot.*/
                                if (*(array3D + k * piece * 7 + l * 7 + 6) == 2)
                                {
                                    notRoad = 1;
                                    break;
                                }
                            }
                        }

                        /*A többi kört nem vizsgáljuk, ha a sugár, elakadt egy magasponton.*/
                        if (notRoad == 1)
                            break;

                        /*Egyébként, elfogadjuk az adott kör, adott szakaszát.*/
                        for (l = indexArray[k] - 1; *(array3D + k * piece * 7 + l * 7 + 4) >= currentDegree && l >= 0; --l)
                        {
                            if (*(array3D + k * piece * 7 + l * 7 + 4) <= i)
                            {
                                *(array3D + k * piece * 7 + l * 7 + 6) = 1;
                            }
                        }
                    }
                }
            }
        }

        /*-- 3. lépés: A marker pontjainak keresése. Adott fokban a legtávolabbi zöld pont. --*/
        /*A marker pontjait tartalmazza. Az első három oszlopban az X - Y - Z koordinátát,
        a negyedikben pedig 0 - 1 érték szerepel attól függően, hogy az adott fokban található-e olyan pont, ami nincs útnak jelölve.*/
        float markerPointsArray[piece][4];
        float maxDistanceRoad; /*Adott fokban, a legtávolabbi zöld pont távolsága.*/
        int cM = 0;            /*Segédváltozó, a marker pontok feltöltéséhez (c - counter, M - Marker).*/
        int ID1, ID2;          /*Az adott pont melyik körvonalon van (ID1) és hányadik pont (ID2).*/
        int redPoints;         /*A vizsgált sávban van-e magaspont, vagy olyan pont, amit nem jelölt a program útnak, se magaspontnak.*/

        /*360 fokban megvizsgáljuk a pontokat, 1 fokonként.*/
        for (i = 0; i <= 360; i++)
        {
            ID1 = -1;
            ID2 = -1;
            maxDistanceRoad = 0;
            redPoints = 0;

            /*Itt végigmegyünk az összes körvonal, összes pontján.*/
            for (j = 0; j < index; j++)
            {
                for (k = 0; k < indexArray[j]; k++)
                {
                    /*Ha találunk az adott fokban nem út pontot, akkor kilépünk, mert utána úgyse lesz már út pont és a "redPoints" változó 1-es érétket kap.*/
                    if (*(array3D + j * piece * 7 + k * 7 + 6) != 1 && *(array3D + j * piece * 7 + k * 7 + 4) >= i && *(array3D + j * piece * 7 + k * 7 + 4) < i + 1)
                    {
                        redPoints = 1;
                        break;
                    }

                    /*A talált zöld pont távolságának vizsgálata.*/
                    if (*(array3D + j * piece * 7 + k * 7 + 6) == 1 && *(array3D + j * piece * 7 + k * 7 + 4) >= i && *(array3D + j * piece * 7 + k * 7 + 4) < i + 1)
                    {
                        d = sqrt(pow(0 - *(array3D + j * piece * 7 + k * 7 + 0), 2) + pow(0 - *(array3D + j * piece * 7 + k * 7 + 1), 2));

                        if (d > maxDistanceRoad)
                        {
                            maxDistanceRoad = d;
                            ID1 = j;
                            ID2 = k;
                        }
                    }
                }
                /*A korábbi "break", kilépett az adott körvonalból, ez a továbbiakból is kilép, és jön a következő fok vizsgálata.*/
                if (redPoints == 1)
                    break;
            }

            /*A marker pontok hozzáadása a tömbhöz.*/
            if (ID1 != -1 && ID2 != -1)
            {
                markerPointsArray[cM][0] = *(array3D + ID1 * piece * 7 + ID2 * 7 + 0);
                markerPointsArray[cM][1] = *(array3D + ID1 * piece * 7 + ID2 * 7 + 1);
                markerPointsArray[cM][2] = *(array3D + ID1 * piece * 7 + ID2 * 7 + 2);
                markerPointsArray[cM][3] = redPoints;
                cM++;
            }
        }

        /*-- 4. lépés: A csoportok feltöltése. --*/
        for (i = 0; i < index; i++)
        {
            for (j = 0; j < indexArray[i]; j++)
            {
                /*Az út pontok.*/
                if (*(array3D + i * piece * 7 + j * 7 + 6) == 1)
                {
                    pt.x = *(array3D + i * piece * 7 + j * 7 + 0);
                    pt.y = *(array3D + i * piece * 7 + j * 7 + 1);
                    pt.z = *(array3D + i * piece * 7 + j * 7 + 2);
                    cloud_filtered_Road.push_back(pt);
                }

                /*A magas pontok.*/
                else if (*(array3D + i * piece * 7 + j * 7 + 6) == 2)
                {
                    pt.x = *(array3D + i * piece * 7 + j * 7 + 0);
                    pt.y = *(array3D + i * piece * 7 + j * 7 + 1);
                    pt.z = *(array3D + i * piece * 7 + j * 7 + 2);
                    cloud_filtered_High.push_back(pt);
                }
            }
        }

        /*-- 5. lépés: A marker beállítása. --*/
        /*Legyen minimum 3 pont, amit össze lehet kötni, különben hibák lépnének fel.*/
        if (cM > 2)
        {
            /*Lehet olyan eset, hogy piros - zöld - prios (vagy fordítva) pont van egymás mellett.
            Ez azért rossz, mert a zöld / piros marker (line strip) ebben az esetben csak 1 pont lesz.
            Ez nem javasolt, ezért minden pontnak lennie kell egy azonos színű párjának.
            A "markerPointsArray" tömb 3. oszlopában ha 1-es szerepel, akkor az a piros line strip-hez tartozik,
            ellenkező esetben a zöldhöz.*/

            /*Ha az első pont zöld, de a második piros, akkor az első is a piros line strip-be kerül.*/
            if (markerPointsArray[0][3] == 0 && markerPointsArray[1][3] == 1)
                markerPointsArray[0][3] = 1;

            /*Ha az utolsó pont zöld, de az utolsó előtti piros, akkor az utolsó is a piros line strip-be kerül.*/
            if (markerPointsArray[cM - 1][3] == 0 && markerPointsArray[cM - 2][3] == 1)
                markerPointsArray[cM - 1][3] = 1;

            /*Ha az első pont piros, de a második zöld, akkor az első is a zöld line strip-be kerül.*/
            if (markerPointsArray[0][3] == 1 && markerPointsArray[1][3] == 0)
                markerPointsArray[0][3] = 0;

            /*Ha az utolsó pont piros, de az utolsó előtti zöld, akkor az utolsó is a zöld line strip-be kerül.*/
            if (markerPointsArray[cM - 1][3] == 1 && markerPointsArray[cM - 2][3] == 0)
                markerPointsArray[cM - 1][3] = 0;

            /*Itt végig megyünk a pontokon, ha egy zöld pontot két piros fog közre, akkor az is a piros line strip-be kerül.
            Az első kettő és az utolsó kettő pontot nem vizsgáljuk, ezek már be vannak állítva.*/
            for (i = 2; i <= cM - 3; i++)
            {
                if (markerPointsArray[i][3] == 0 && markerPointsArray[i - 1][3] == 1 && markerPointsArray[i + 1][3] == 1)
                    markerPointsArray[i][3] = 1;
            }

            /*Itt végig megyünk az összes ponton, ha egy piros pontot két zöld fog közre, akkor az is a zöld line strip-be kerül.
            Az első kettő és az utolsó kettő pontot nem vizsgáljuk, ezek már be vannak állítva.*/
            for (i = 2; i <= cM - 3; i++)
            {
                if (markerPointsArray[i][3] == 1 && markerPointsArray[i - 1][3] == 0 && markerPointsArray[i + 1][3] == 0)
                    markerPointsArray[i][3] = 0;
            }

            visualization_msgs::MarkerArray ma;    /*Egy marker array, amiben a zöld / piros line strip-ek kerülnek.*/
            visualization_msgs::Marker line_strip; /*Az adott zöld vagy piros szakasz / line strip.*/
            geometry_msgs::Point point;            /*Az adott pont értékei. Ez tölti fel az adott line stip-et.*/
float zavg = 0.0;                      /*Átlagos z-érték (egyszerűsített polygonhoz)*/

            int lineStripID = 0; /*Az adott line strip ID-ja.*/

            line_strip.header.frame_id = fixedFrame;
            line_strip.header.stamp = ros::Time();
            line_strip.type = visualization_msgs::Marker::LINE_STRIP;
            line_strip.action = visualization_msgs::Marker::ADD;

            /*Végigmegyünk a pontokon, amik a markert fogják alkotni.*/
            for (i = 0; i < cM; i++)
            {
                /*Az adott pont hozzáadása egy "geometry_msgs::Point" típusú változóhoz.*/
                point.x = markerPointsArray[i][0];
                point.y = markerPointsArray[i][1];
                point.z = markerPointsArray[i][2];
                zavg *= i;
                zavg += point.z;
                zavg /= i+1;

                /*Az első pont hozzáadása az adott line strip-hez.
                Az első pontnál semmilyen feltétel nem kell.*/
                if (i == 0)
                {
                    line_strip.points.push_back(point);
                    line += xy(point.x,point.y);
                }

                /*Ha a következő pont is ugyanabba a csoportba (piros vagy zöld) tartozik, mint az előző,
                akkor ezt is hozzáadjuk az adott line strip-hez.*/
                else if (markerPointsArray[i][3] == markerPointsArray[i - 1][3])
                {
                    line_strip.points.push_back(point);
                    line += xy(point.x,point.y);

                    /*Ebben az "else if" feltételben fogjuk elérni az utolsó pontot, itt elkészül az utolsó line strip.*/
                    if (i == cM - 1)
                    {
                        line_strip.id = lineStripID;

                        line_strip.pose.position.x = 0;
                        line_strip.pose.position.y = 0;
                        line_strip.pose.position.z = 0;

                        line_strip.pose.orientation.x = 0.0;
                        line_strip.pose.orientation.y = 0.0;
                        line_strip.pose.orientation.z = 0.0;
                        line_strip.pose.orientation.w = 1.0;

                        line_strip.scale.x = 0.5;
                        line_strip.scale.y = 0.5;
                        line_strip.scale.z = 0.5;

                        /*A line strip színének a beállítása.*/
                        if (markerPointsArray[i][3] == 0)
                        {
                            line_strip.color.a = 1.0;
                            line_strip.color.r = 0.0;
                            line_strip.color.g = 1.0;
                            line_strip.color.b = 0.0;
                        }
                        else
                        {
                            line_strip.color.a = 1.0;
                            line_strip.color.r = 1.0;
                            line_strip.color.g = 0.0;
                            line_strip.color.b = 0.0;
                        }

                        if (polysimp_allow)
                        {
                            line_strip.points.clear();
                            boost::geometry::clear(simplified);
                            boost::geometry::simplify(line, simplified, polysimp);
                            for(boost::geometry::model::linestring<xy>::const_iterator it = simplified.begin(); it != simplified.end(); it++)
                            {
                                geometry_msgs::Point p;
                                p.x = boost::geometry::get<0>(*it);
                                p.y = boost::geometry::get<1>(*it);
                                p.z = polyz;

                                line_strip.points.push_back(p);
                            }
                        }

                        ma.markers.push_back(line_strip); /*A line strip hozzáadása a marker array-hez.*/
                        line_strip.points.clear();        /*Az utolsó line strip-ből is töröljük a pontokat, feleslegesen ne tárolódjon.*/
                        boost::geometry::clear(line);
                    }
                }

                /*Csoportváltozás --> pirosról - zöldre.
                Ilyenkor még piros marker köti össze a két pontot, szóval hozzáadjuk a pontot az adott line strip-hez.*/
                else if (markerPointsArray[i][3] != markerPointsArray[i - 1][3] && markerPointsArray[i][3] == 0)
                {
                    line_strip.points.push_back(point);
                    line += xy(point.x,point.y);

                    /*A következő pontok már új line strip-hez fognak tartozni, szóval itt elkészül az egyik piros.*/
                    line_strip.id = lineStripID;
                    lineStripID++;

                    line_strip.pose.position.x = 0;
                    line_strip.pose.position.y = 0;
                    line_strip.pose.position.z = 0;

                    line_strip.pose.orientation.x = 0.0;
                    line_strip.pose.orientation.y = 0.0;
                    line_strip.pose.orientation.z = 0.0;
                    line_strip.pose.orientation.w = 1.0;

                    line_strip.scale.x = 0.5;
                    line_strip.scale.y = 0.5;
                    line_strip.scale.z = 0.5;

                    line_strip.color.a = 1.0;
                    line_strip.color.r = 1.0;
                    line_strip.color.g = 0.0;
                    line_strip.color.b = 0.0;

                    if (polysimp_allow)
                    {
                        line_strip.points.clear();
                        boost::geometry::clear(simplified);
                        boost::geometry::simplify(line, simplified, polysimp);
                        for(boost::geometry::model::linestring<xy>::const_iterator it = simplified.begin(); it != simplified.end(); it++)
                        {
                            geometry_msgs::Point p;
                            p.x = boost::geometry::get<0>(*it);
                            p.y = boost::geometry::get<1>(*it);
                            p.z = polyz;

                            line_strip.points.push_back(p);
                        }
                    }

                    ma.markers.push_back(line_strip);   /*A line strip hozzáadása a marker array-hez.*/
                    line_strip.points.clear();          /*A benne lévő pontok már nem kellenek.*/
                    boost::geometry::clear(line);
                    line_strip.points.push_back(point); /*A következő zöld line strip-nél is szükség van erre a pontra, szóval hozzáadjuk.*/
                    line += xy(point.x,point.y);
                }

                /*Csoportváltozás --> zöldről - pirosra.
                Előszőr beállítjuk a zöld line stip-et, majd az utolsó pontot hozzáadjuk a piroshoz is,
                mivel zöld és piros pont között mindig piros line srip van.*/
                else if (markerPointsArray[i][3] != markerPointsArray[i - 1][3] && markerPointsArray[i][3] == 1)
                {
                    /*A zöld marker.*/
                    line_strip.id = lineStripID;
                    lineStripID++;

                    line_strip.pose.position.x = 0;
                    line_strip.pose.position.y = 0;
                    line_strip.pose.position.z = 0;

                    line_strip.pose.orientation.x = 0.0;
                    line_strip.pose.orientation.y = 0.0;
                    line_strip.pose.orientation.z = 0.0;
                    line_strip.pose.orientation.w = 1.0;

                    line_strip.scale.x = 0.5;
                    line_strip.scale.y = 0.5;
                    line_strip.scale.z = 0.5;

                    line_strip.color.a = 1.0;
                    line_strip.color.r = 0.0;
                    line_strip.color.g = 1.0;
                    line_strip.color.b = 0.0;

                    if (polysimp_allow)
                    {
                        line_strip.points.clear();
                        boost::geometry::clear(simplified);
                        boost::geometry::simplify(line, simplified, polysimp);
                        for(boost::geometry::model::linestring<xy>::const_iterator it = simplified.begin(); it != simplified.end(); it++)
                        {
                            geometry_msgs::Point p;
                            p.x = boost::geometry::get<0>(*it);
                            p.y = boost::geometry::get<1>(*it);
                            p.z = polyz;

                            line_strip.points.push_back(p);
                        }
                    }

                    ma.markers.push_back(line_strip); /*A line strip hozzáadása a marker array-hez.*/
                    line_strip.points.clear();        /*A benne lévő pontok már nem kellenek.*/
                    boost::geometry::clear(line);

                    /*A követkető piros line srip-hez szükség van az előző pontra is.*/
                    point.x = markerPointsArray[i - 1][0];
                    point.y = markerPointsArray[i - 1][1];
                    point.z = markerPointsArray[i - 1][2];
                    line_strip.points.push_back(point);
                    line += xy(point.x,point.y);

                    /*A követkető piros line srip-hez szükség van a jelenlegi pontra.*/
                    point.x = markerPointsArray[i][0];
                    point.y = markerPointsArray[i][1];
                    point.z = markerPointsArray[i][2];
                    line_strip.points.push_back(point);
                    line += xy(point.x,point.y);
                }
                line_strip.lifetime = ros::Duration(0);
            }
            if (zavg_allow)
            {
                for (int seg=0; seg < ma.markers.size(); seg++)
                {
                    for (int mz = 0; mz < ma.markers[seg].points.size(); mz++) /*Egyszerűsített polygon z-koordinátáinak megadása átlagból. */
                    {
                        ma.markers[seg].points[mz].z = zavg;
                    }
                }
                /* polyz = zavg; /* Be- és kikapcsolással a konstans z-érték az átlagérték alapján beállítja magát. (kell?) */
            }

            /*érvényét vesztett markerek eltávolítása*/
            line_strip.action = visualization_msgs::Marker::DELETE;
            for (int del = lineStripID; del<ghostcount; del++)
            {
                line_strip.id++;
                ma.markers.push_back(line_strip);
            }
            ghostcount = lineStripID;

            /*A marker array hírdetése.*/
            pub_marker.publish(ma);
        }

        for (j = 0; j < indexArray[10]; j++)
        {
            pt.x = *(array3D + 10 * piece * 7 + j * 7 + 0);
            pt.y = *(array3D + 10 * piece * 7 + j * 7 + 1);
            pt.z = *(array3D + 10 * piece * 7 + j * 7 + 2);
            cloud_filtered_ProbablyRoad.push_back(pt);
        }

        /*A lefoglalt terület felszabadítása.*/
        delete[] array3D;
    }

    /*Road és High topic header.*/
    cloud_filtered_Road.header = cloud.header;
    cloud_filtered_ProbablyRoad.header = cloud.header;
    cloud_filtered_High.header = cloud.header;
    cloud_filtered_Box.header = cloud.header;

    /*Publish.*/
    pub_road.publish(cloud_filtered_Road); /*Szűrt pontok (úttest).*/
    pub_high.publish(cloud_filtered_High); /*Szűrt pontok (nem úttest).*/
    pub_box.publish(cloud_filtered_Box);  /*A vizsgált terület, összes pontja.*/
    pub_pobroad.publish(cloud_filtered_ProbablyRoad);
}

/*MAIN*/
int main(int argc, char **argv)
{

    /*Az ROS inicializálása.*/
    ros::init(argc, argv, "lidarSegmentation");
    ROS_INFO("Initializing %s", ros::this_node::getName().c_str());

    /*A GUI felülethez szükséges sorok.*/
    dynamic_reconfigure::Server<lidar_filters_pkg::LidarFiltersConfig> server;
    dynamic_reconfigure::Server<lidar_filters_pkg::LidarFiltersConfig>::CallbackType f;
    f = boost::bind(&paramsCallback, _1, _2);
    server.setCallback(f);

    /*NodeHandle*/
    ros::NodeHandle nh;

    /*Subscribe to the topic.*/
    // trigger the callback function (filter) when new cloud points arrives
    ros::Subscriber sub = nh.subscribe(topicName, 1, filtered);

    /*Announcement of filtered data.*/
    pub_road = nh.advertise<pcl::PCLPointCloud2>("cloud_filtered_Road", 1);
    pub_high = nh.advertise<pcl::PCLPointCloud2>("cloud_filtered_High", 1);
    pub_box = nh.advertise<pcl::PCLPointCloud2>("cloud_filtered_Box", 1);
    pub_pobroad = nh.advertise<pcl::PCLPointCloud2>("cloud_filtered_ProbablyRoad", 1);
    pub_marker = nh.advertise<visualization_msgs::MarkerArray>("visualization_MarkerArray", 1);

    /*Required for the code of László Csaplár.*/
    beam_init();

    ROS_INFO("Ready");

    ros::spin();
    return 0;
}


//==================================================================================================
/*!
  @file

  Aggregate SIMD numerical and type limits for X86 AVX2

  @copyright 2012 - 2015 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)

**/
//==================================================================================================
#ifndef BOOST_SIMD_ARCH_X86_AVX2_LIMITS_HPP_INCLUDED
#define BOOST_SIMD_ARCH_X86_AVX2_LIMITS_HPP_INCLUDED

#include <boost/simd/arch/x86/tags.hpp>
#include <boost/simd/arch/common/limits.hpp>
#include <boost/simd/arch/x86/avx/limits.hpp>

namespace boost { namespace simd
{
  template<> struct limits<boost::simd::avx2_> : limits<boost::simd::avx_>
  {
    struct largest_integer
    {
      template<typename Sign> struct apply : boost::dispatch::make_integer<8,Sign> {};
    };

    struct smallest_integer
    {
      template<typename Sign> struct apply : boost::dispatch::make_integer<1,Sign> {};
    };
  };
} }

#endif


#include <ros/ros.h>
#include <geometry_msgs/PoseStamped.h>
#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#include <boost/algorithm/string.hpp>
#include <tf/transform_datatypes.h>
#include <math.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <sys/ioctl.h>
#include <fcntl.h>
#include <termios.h>
#include <unistd.h>
#include <time.h>
#include "kalman-Ndof.hpp"
#include <boost/date_time/posix_time/posix_time.hpp>
#include <nav_msgs/Odometry.h>

#define DEVNAME "/dev/ttyACM0"
#define BAUDRATE B115200
#define BUFSIZE 256
#define PI 3.14159265
#define DEG2RAD(x) ((x)*(PI/180))

using namespace std;
using namespace boost;

Kalman *kalman;

int main(int argc, char **argv)
{
  ros::init(argc, argv, "pozyx");
  ros::NodeHandle n;
  ros::Publisher pos_pub = n.advertise<geometry_msgs::PoseStamped> ("pozyx", 1000);
  ros::Publisher ninebot_pos_pub = n.advertise<nav_msgs::Odometry> ("ninebot_measured_pos", 1000);
  std::string frame_id("/world_link");

	int fd = open(DEVNAME, O_RDWR | O_NOCTTY);
	if(fd < 0){
		ROS_ERROR("cannot open device");
		return 0;
	}
	ROS_INFO("device opened");

	struct termios tio;
	memset(&tio, 0, sizeof(tio));
	tio.c_cflag = CS8 | CLOCAL | CREAD;
	tio.c_cc[VTIME] = 0;
	tio.c_lflag = ICANON;
	tio.c_iflag = IGNPAR | ICRNL;
	cfsetspeed(&tio, BAUDRATE);
	tcsetattr(fd, TCSANOW, &tio);

  kalman = new Kalman(6, 6, 3);
  ros::Time last_time, now;
  now = ros::Time::now();

	int len;
	char buf[BUFSIZE];

  double X, Y, Z, qx, qy, qz, qw;
  double preX, preY, preZ, preqx, preqy, preqz, preqw;
  double kfX = 0;
  double kfY = 0;
  double kfZ = 0;
  bool GetInitRPY = true;
  double init_roll, init_pitch, init_yaw;

	while(ros::ok()){
		memset(&buf, 0, sizeof(buf));
		len = read(fd, buf, BUFSIZE);
		if(len == 0) continue;
	  std::string data(buf);
	  std::vector<std::string> v_data;
	  v_data.clear();
    boost::split(v_data, data, boost::is_any_of(","));

    if(v_data.size() == 8){

      preX = X;
      preY = Y;
      preZ = Z;
      preqx = qx;
      preqy = qy;
      preqz = qz;
      preqw = qw;

      int id = atoi(v_data.at(0).c_str());
      X = 0.001 * atoi(v_data.at(1).c_str());
      Y = 0.001 * atoi(v_data.at(2).c_str());
      Z = 0.001 * atoi(v_data.at(3).c_str());
      qx = atof(v_data.at(4).c_str());
      qy = atof(v_data.at(5).c_str());
      qz = atof(v_data.at(6).c_str());
      qw = atof(v_data.at(7).c_str());

      // get init eular angle as offset (called once)
      if(GetInitRPY){
        tf::Quaternion init_q(qx,qy,qz,qw);
        tf::Matrix3x3 init_mat(init_q);
        init_mat.getRPY(init_roll,init_pitch,init_yaw);
        GetInitRPY = false;
      }

      if(X == 0 && Y==0 && Z==0){
        X = preX;
        Y = preY;
        Z = preZ;
      }

      if(qx == 0 && qy == 0 && qz == 0 && qw == 0){
        qx = preqx;
        qy = preqy;
        qz = preqz;
        qw = preqw;
      }

      ROS_INFO("%d,%f,%f,%f,%f,%f,%f,%f",id, X, Y, Z, qx, qy, qz, qw);

      //calc orientation offset applied
      double ninebot_roll, ninebot_pitch, ninebot_yaw;
      tf::Quaternion temp_q(qx, qy, qz, qw);
      tf::Matrix3x3 temp_mat(temp_q);
      temp_mat.getRPY(ninebot_roll, ninebot_pitch, ninebot_yaw);

      ninebot_yaw -= init_yaw;

      tf::Matrix3x3 offset_mat;
      offset_mat.setEulerYPR(ninebot_yaw, ninebot_pitch, ninebot_roll);
      tf::Quaternion ninebot_quat;
      offset_mat.getRotation(ninebot_quat);

      //kalman_filter
      last_time = now;
      now = ros::Time::now();
      double delta_t = (now.sec + now.nsec * 0.000000001) - (last_time.sec + last_time.nsec * 0.000000001);
      double velX = (X - preX) / delta_t;
      double velY = (Y - preY) / delta_t;
      double velZ = (Z - preZ) / delta_t;

      static int count = 0;

      if(kalman == NULL) return 0;
      if(count == 0){
        count++;
        continue;
      }else if(count == 1){
        // 初期化 (システム雑音，観測雑音，積分時間)
        kalman->init(0.15,0.7,0.08);
        double C[6][6];
        memset(C, 0, sizeof(C));
        C[0][0] = C[1][1] = C[2][2] = 1;
        kalman->setC((double *)&C);
        kalman->setX(0, X);
        kalman->setX(1, Y);
        kalman->setX(2, Z);
        kalman->setX(3, velX);
        kalman->setX(4, velY);
        kalman->setX(5, velZ);
        count++;
      }

      double A[3][6];
      memset(A, 0, sizeof(A));
      A[0][0] = A[1][1] = A[2][2] = 1;
      A[0][3] = A[1][4] = A[2][5] = delta_t;
      double obs[6]={X, Y, Z, velX, velY, velZ};
      double input[3]={0, 0, 0};

      kalman->update(obs, input);

      kfX = kalman->getX(0);
      kfY = kalman->getX(1);
      kfZ = kalman->getX(2);

      ROS_INFO("kfX=%f kfY=%f kfZ=%f",kfX,kfY,kfZ);

      //----------------

      geometry_msgs::PoseStamped msg;
      msg.header.frame_id = frame_id;
      msg.header.stamp = ros::Time::now();
      msg.pose.position.x = kfX;
      msg.pose.position.y = kfY;
      msg.pose.position.z = kfZ;
      msg.pose.orientation.x = qx;
      msg.pose.orientation.y = qy;
      msg.pose.orientation.z = qz;
      msg.pose.orientation.w = qw;
      pos_pub.publish(msg);

      nav_msgs::Odometry odom;
      odom.header.frame_id = frame_id;
      odom.header.stamp = ros::Time::now();
      odom.pose.pose.position.x = kfX;
      odom.pose.pose.position.y = kfY;
      odom.pose.pose.position.z = kfZ;
      odom.pose.pose.orientation.x = qx - ninebot_quat.getX();
      odom.pose.pose.orientation.y = qy - ninebot_quat.getY();
      odom.pose.pose.orientation.z = qz - ninebot_quat.getZ();
      odom.pose.pose.orientation.w = qw - ninebot_quat.getW();
      ninebot_pos_pub.publish(odom);

    }
  }
  return 0;
}


/*    Copyright (c) 2010-2019, Delft University of Technology
 *    All rigths reserved
 *
 *    This file is part of the Tudat. Redistribution and use in source and
 *    binary forms, with or without modification, are permitted exclusively
 *    under the terms of the Modified BSD license. You should have received
 *    a copy of the license with this file. If not, please or visit:
 *    http://tudat.tudelft.nl/LICENSE.
 */

#include <boost/lambda/lambda.hpp>

#include "tudat/interface/spice/spiceEphemeris.h"
#include "tudat/astro/ephemerides/customEphemeris.h"
#include "tudat/astro/ephemerides/keplerEphemeris.h"
#include "tudat/astro/ephemerides/multiArcEphemeris.h"
#include "tudat/astro/ephemerides/tabulatedEphemeris.h"
#include "tudat/astro/ephemerides/approximatePlanetPositions.h"
#include "tudat/astro/ephemerides/approximatePlanetPositionsCircularCoplanar.h"
#include "tudat/astro/ephemerides/constantEphemeris.h"
#include "tudat/math/interpolators/lagrangeInterpolator.h"
#include "tudat/simulation/environment_setup/createEphemeris.h"

namespace tudat
{

namespace simulation_setup
{

using namespace ephemerides;

//! Function to create an ephemeris model.
std::shared_ptr< ephemerides::Ephemeris > createBodyEphemeris(
        const std::shared_ptr< EphemerisSettings > ephemerisSettings,
        const std::string& bodyName )
{
    // Declare return object.
    std::shared_ptr< ephemerides::Ephemeris > ephemeris;

    if( ephemerisSettings->getMakeMultiArcEphemeris( ) )
    {
        std::map< double, std::shared_ptr< Ephemeris > > singleArcEphemerides;
        ephemerisSettings->resetMakeMultiArcEphemeris( false );

        singleArcEphemerides[ -std::numeric_limits< double >::lowest( ) ] = createBodyEphemeris(
                    ephemerisSettings, bodyName );

        ephemeris = std::make_shared< MultiArcEphemeris >(
                    singleArcEphemerides, ephemerisSettings->getFrameOrigin( ), ephemerisSettings->getFrameOrientation( ) );
    }
    else
    {

        // Check which type of ephemeris model is to be created.
        switch( ephemerisSettings->getEphemerisType( ) )
        {
        case direct_spice_ephemeris:
        {
            // Check consistency of type and class.
            std::shared_ptr< DirectSpiceEphemerisSettings > directEphemerisSettings =
                    std::dynamic_pointer_cast< DirectSpiceEphemerisSettings >( ephemerisSettings );
            if( directEphemerisSettings == nullptr )
            {
                throw std::runtime_error(
                            "Error, expected direct spice ephemeris settings for body " + bodyName );
            }
            else
            {
                std::string inputName = ( directEphemerisSettings->getBodyNameOverride( ) == "" ) ?
                            bodyName : directEphemerisSettings->getBodyNameOverride( );

                // Create corresponding ephemeris object.
                ephemeris = std::make_shared< SpiceEphemeris >(
                            inputName,
                            directEphemerisSettings->getFrameOrigin( ),
                            directEphemerisSettings->getCorrectForStellarAberration( ),
                            directEphemerisSettings->getCorrectForLightTimeAberration( ),
                            directEphemerisSettings->getConvergeLighTimeAberration( ),
                            directEphemerisSettings->getFrameOrientation( ) );
            }
            break;
        }
        case interpolated_spice:
        {
            // Check consistency of type and class.
            std::shared_ptr< InterpolatedSpiceEphemerisSettings > interpolatedEphemerisSettings =
                    std::dynamic_pointer_cast< InterpolatedSpiceEphemerisSettings >(
                        ephemerisSettings );
            if( interpolatedEphemerisSettings == nullptr )
            {
                throw std::runtime_error(
                            "Error, expected tabulated spice ephemeris settings for body " + bodyName );
            }
            else
            {
                // Since only the barycenters of planetary systems are included in the standard DE
                // ephemerides, append 'Barycenter' to body name.
                std::string inputName = ( interpolatedEphemerisSettings->getBodyNameOverride( ) == "" ) ?
                            bodyName : interpolatedEphemerisSettings->getBodyNameOverride( );
                // Create corresponding ephemeris object.
                if( !interpolatedEphemerisSettings->getUseLongDoubleStates( ) )
                {
                    ephemeris = createTabulatedEphemerisFromSpice< double, double >(
                                inputName,
                                interpolatedEphemerisSettings->getInitialTime( ),
                                interpolatedEphemerisSettings->getFinalTime( ),
                                interpolatedEphemerisSettings->getTimeStep( ),
                                interpolatedEphemerisSettings->getFrameOrigin( ),
                                interpolatedEphemerisSettings->getFrameOrientation( ),
                                interpolatedEphemerisSettings->getInterpolatorSettings( ) );
                }
                else
                {
#if( TUDAT_BUILD_WITH_EXTENDED_PRECISION_PROPAGATION_TOOLS )

                    ephemeris = createTabulatedEphemerisFromSpice< long double, double >(
                                inputName,
                                static_cast< long double >( interpolatedEphemerisSettings->getInitialTime( ) ),
                                static_cast< long double >( interpolatedEphemerisSettings->getFinalTime( ) ),
                                static_cast< long double >( interpolatedEphemerisSettings->getTimeStep( ) ),
                                interpolatedEphemerisSettings->getFrameOrigin( ),
                                interpolatedEphemerisSettings->getFrameOrientation( ),
                                interpolatedEphemerisSettings->getInterpolatorSettings( ) );
#else
                    throw std::runtime_error( "Error, long double compilation is turned off; requested long double tabulated ephemeris" );
#endif
                }
            }
            break;
        }
        case tabulated_ephemeris:
        {
            // Check consistency of type and class.
            std::shared_ptr< TabulatedEphemerisSettings > tabulatedEphemerisSettings =
                    std::dynamic_pointer_cast< TabulatedEphemerisSettings >( ephemerisSettings );
            if( tabulatedEphemerisSettings == nullptr )
            {
                throw std::runtime_error(
                            "Error, expected tabulated ephemeris settings for body " + bodyName );
            }
            else
            {
                // Create corresponding ephemeris object.
                if( !tabulatedEphemerisSettings->getUseLongDoubleStates( ) )
                {
                    if( tabulatedEphemerisSettings->getBodyStateHistory( ).size( ) != 0 )
                    {
                        ephemeris = std::make_shared< TabulatedCartesianEphemeris< > >(
                                    std::make_shared<
                                    interpolators::LagrangeInterpolator< double, Eigen::Vector6d > >
                                    ( tabulatedEphemerisSettings->getBodyStateHistory( ), 6,
                                      interpolators::huntingAlgorithm,
                                      interpolators::lagrange_cubic_spline_boundary_interpolation ),
                                    tabulatedEphemerisSettings->getFrameOrigin( ),
                                    tabulatedEphemerisSettings->getFrameOrientation( ) );
                    }
                    else
                    {
                        ephemeris = std::make_shared< TabulatedCartesianEphemeris< > >(
                                    std::shared_ptr< interpolators::OneDimensionalInterpolator< double, Eigen::Vector6d > >( ),
                                      tabulatedEphemerisSettings->getFrameOrigin( ),
                                      tabulatedEphemerisSettings->getFrameOrientation( ) );
                    }
                }
                else
                {
#if( TUDAT_BUILD_WITH_EXTENDED_PRECISION_PROPAGATION_TOOLS )

                    // Cast input history to required type.
                    if( tabulatedEphemerisSettings->getBodyStateHistory( ).size( ) != 0 )
                    {
                        std::map< double, Eigen::Vector6d > originalStateHistory =
                                tabulatedEphemerisSettings->getBodyStateHistory( );
                        std::map< double, Eigen::Matrix< long double, 6, 1 > > longStateHistory;

                        for( std::map< double, Eigen::Vector6d >::const_iterator stateIterator =
                             originalStateHistory.begin( ); stateIterator != originalStateHistory.end( ); stateIterator++ )
                        {
                            longStateHistory[ stateIterator->first ] = stateIterator->second.cast< long double >( );
                            ephemeris =
                                    std::make_shared< TabulatedCartesianEphemeris< long double, double > >(
                                        std::make_shared< interpolators::LagrangeInterpolator<
                                        double, Eigen::Matrix< long double, 6, 1 > > >
                                        ( longStateHistory, 6,
                                          interpolators::huntingAlgorithm,
                                          interpolators::lagrange_cubic_spline_boundary_interpolation ),
                                        tabulatedEphemerisSettings->getFrameOrigin( ),
                                        tabulatedEphemerisSettings->getFrameOrientation( ) );
                        }
                    }
                    else
                    {
                        ephemeris = std::make_shared< TabulatedCartesianEphemeris< long double, double > >(
                                    std::shared_ptr< interpolators::OneDimensionalInterpolator<
                                    double, Eigen::Matrix< long double, 6, 1 > > >( ),
                                      tabulatedEphemerisSettings->getFrameOrigin( ),
                                      tabulatedEphemerisSettings->getFrameOrientation( ) );
                    }
#else
                    throw std::runtime_error( "Error, long double compilation is turned off; requested long double tabulated ephemeris" );
#endif
                }
            }
            break;
        }
        case auto_generated_tabulated_ephemeris:
        {
            // Check consistency of type and class.
            std::shared_ptr< AutoGeneratedTabulatedEphemerisSettings > tabulatedEphemerisSettings =
                    std::dynamic_pointer_cast< AutoGeneratedTabulatedEphemerisSettings >( ephemerisSettings );
            if( tabulatedEphemerisSettings == nullptr )
            {
                throw std::runtime_error( "Error, expected auto-generate tabulated ephemeris settings for " + bodyName );
            }
            else
            {
                // Create ephemeris
                ephemeris = getTabulatedEphemeris(
                            createBodyEphemeris( tabulatedEphemerisSettings->getEphemerisSettings( ), bodyName ),
                            tabulatedEphemerisSettings->getStartTime( ), tabulatedEphemerisSettings->getEndTime( ),
                            tabulatedEphemerisSettings->getTimeStep( ), tabulatedEphemerisSettings->getInterpolatorSettings( ) );
            }
            break;
        }
        case constant_ephemeris:
        {
            // Check consistency of type and class.
            std::shared_ptr< ConstantEphemerisSettings > constantEphemerisSettings =
                    std::dynamic_pointer_cast< ConstantEphemerisSettings >( ephemerisSettings );
            if( constantEphemerisSettings == nullptr )
            {
                throw std::runtime_error( "Error, expected constant ephemeris settings for " + bodyName );
            }
            else
            {
                // Create ephemeris
                ephemeris = std::make_shared< ConstantEphemeris >(
                            [ = ]( ){ return constantEphemerisSettings->getConstantState( ); },
                            constantEphemerisSettings->getFrameOrigin( ),
                            constantEphemerisSettings->getFrameOrientation( ) );
            }
            break;
        }
        case custom_ephemeris:
        {
            // Check consistency of type and class.
            std::shared_ptr< CustomEphemerisSettings > customEphemerisSettings =
                    std::dynamic_pointer_cast< CustomEphemerisSettings >( ephemerisSettings );
            if( customEphemerisSettings == nullptr )
            {
                throw std::runtime_error( "Error, expected custom ephemeris settings for " + bodyName );
            }
            else
            {
                // Create ephemeris
                ephemeris = std::make_shared< CustomEphemeris >(
                            customEphemerisSettings->getCustomStateFunction( ),
                            customEphemerisSettings->getFrameOrigin( ),
                            customEphemerisSettings->getFrameOrientation( ) );
            }
            break;
        }

        case kepler_ephemeris:
        {
            // Check consistency of type and class.
            std::shared_ptr< KeplerEphemerisSettings > keplerEphemerisSettings =
                    std::dynamic_pointer_cast< KeplerEphemerisSettings >( ephemerisSettings );
            if( keplerEphemerisSettings == nullptr )
            {
                throw std::runtime_error( "Error, expected Kepler ephemeris settings for " + bodyName );
            }
            else
            {
                // Create ephemeris
                ephemeris = std::make_shared< KeplerEphemeris >(
                            keplerEphemerisSettings->getInitialStateInKeplerianElements( ),
                            keplerEphemerisSettings->getEpochOfInitialState( ),
                            keplerEphemerisSettings->getCentralBodyGravitationalParameter( ),
                            keplerEphemerisSettings->getFrameOrigin( ),
                            keplerEphemerisSettings->getFrameOrientation( ),
                            keplerEphemerisSettings->getRootFinderAbsoluteTolerance( ),
                            keplerEphemerisSettings->getRootFinderMaximumNumberOfIterations( ) );
            }
            break;
        }
        case approximate_planet_positions:
        {
            // Check consistency of type and class.
            std::shared_ptr< ApproximateJplEphemerisSettings > approximateEphemerisSettings =
                    std::dynamic_pointer_cast< ApproximateJplEphemerisSettings >(
                        ephemerisSettings );
            if( approximateEphemerisSettings == nullptr )
            {
                throw std::runtime_error(
                            "Error, expected approximate ephemeris settings for body " + bodyName );
            }
            else
            {

//                ephemerides::ApproximatePlanetPositionsBase::BodiesWithEphemerisData bodyToUse;
//                if( approximateEphemerisSettings->getBodyIdentifier( ) ==
//                        ephemerides::ApproximatePlanetPositionsBase::BodiesWithEphemerisData::undefined )
//                {
//                    try
//                    {
//                        bodyToUse = ephemerides::ApproximatePlanetPositionsBase::getBodiesWithEphemerisDataId( bodyName );
//                    }
//                    catch( std::runtime_error const& )

//                    {
//                        throw std::runtime_error( "Error, approximate ephemeris not available for body: " + bodyName + " when creating ephemeris." );
//                    }
//                }
//                else
//                {
//                    bodyToUse = approximateEphemerisSettings->getBodyIdentifier( );
//                }
//=======
//>>>>>>> origin/feature/mga_estimation_refactor_merge

                // Create corresponding ephemeris object.
                if( approximateEphemerisSettings->getUseCircularCoplanarApproximation( ) )
                {
                    ephemeris = std::make_shared< ApproximateJplCircularCoplanarEphemeris >(
                                approximateEphemerisSettings->getBodyName( ) );
                }
                else
                {
                    ephemeris = std::make_shared< ApproximateJplEphemeris >(
                                approximateEphemerisSettings->getBodyName( ) );
                }
            }
            break;
        }
		case direct_tle_ephemeris:
		{
			// Check consistency of type and class.
			std::shared_ptr< DirectTleEphemerisSettings > directTleEphemerisSettings =
					std::dynamic_pointer_cast< DirectTleEphemerisSettings >( ephemerisSettings );
			if( directTleEphemerisSettings == nullptr )
			{
				throw std::runtime_error(
						"Error, expected direct TLE ephemeris settings for body " + bodyName );
			}
			// Check if the Earth is present in the simulation
			else
			{
				//std::string inputName = bodyName;

				// Check period of the satellite for correct SDP setting

				// Create corresponding ephemeris object.
				ephemeris = std::make_shared< TleEphemeris >(
						directTleEphemerisSettings->getFrameOrigin(),
						directTleEphemerisSettings->getFrameOrientation(),
						directTleEphemerisSettings->getTle()
						);
			}
			break;
		}
		case interpolated_tle_ephemeris:
		{
			// Check consistency of type and class.
			std::shared_ptr< InterpolatedTleEphemerisSettings > interpolatedTleEphemerisSettings =
					std::dynamic_pointer_cast< InterpolatedTleEphemerisSettings >(
							ephemerisSettings );
			if( interpolatedTleEphemerisSettings == nullptr )
			{
				throw std::runtime_error(
						"Error, expected interpolated TLE ephemeris settings for body " + bodyName );
			}
			else
			{

				ephemeris = createTabulatedEphemerisFromTLE< double, double >(
						bodyName,
						interpolatedTleEphemerisSettings->getInitialTime( ),
						interpolatedTleEphemerisSettings->getFinalTime( ),
						interpolatedTleEphemerisSettings->getTimeStep( ),
						interpolatedTleEphemerisSettings->getFrameOrigin( ),
						interpolatedTleEphemerisSettings->getFrameOrientation( ),
						interpolatedTleEphemerisSettings->getTle( ),
						interpolatedTleEphemerisSettings->getInterpolatorSettings( )
						);
			}
			break;
		}
        case scaled_ephemeris:
        {
            // Check consistency of type and class.
            std::shared_ptr< ScaledEphemerisSettings > scaledEphemeriSettings =
                    std::dynamic_pointer_cast< ScaledEphemerisSettings >(
                        ephemerisSettings );
            if( scaledEphemeriSettings == nullptr )
            {
                throw std::runtime_error(
                            "Error, expected scaled ephemeris settings for body " + bodyName );
            }
            else
            {
                std::shared_ptr< Ephemeris > baseEphemeris = createBodyEphemeris(
                            scaledEphemeriSettings->getBaseSettings( ), bodyName );
                ephemeris = std::make_shared< ScaledEphemeris >(
                            baseEphemeris, scaledEphemeriSettings->getScaling( ), scaledEphemeriSettings->getIsScalingAbsolute( ) );
            }
            break;
        }
        default:
        {
            throw std::runtime_error(
                        "Error, did not recognize ephemeris model settings type " +
                        std::to_string( ephemerisSettings->getEphemerisType( ) ) );
        }
        }
    }
    return ephemeris;

}

//! Function that retrieves the time interval at which an ephemeris can be safely interrogated
std::pair< double, double > getSafeInterpolationInterval( const std::shared_ptr< ephemerides::Ephemeris > ephemerisModel )
{
    // Make default output pair
    std::pair< double, double > safeInterval = std::make_pair(
                std::numeric_limits< double >::lowest( ),  std::numeric_limits< double >::max( ) );

    // Check if model is tabulated, and retrieve safe interval from model
    if( isTabulatedEphemeris( ephemerisModel ) )
    {
        safeInterval = getTabulatedEphemerisSafeInterval( ephemerisModel );
    }
    // Check if model is multi-arc, and retrieve safe intervals from first and last arc.
    else if( std::dynamic_pointer_cast< ephemerides::MultiArcEphemeris >( ephemerisModel ) != nullptr )
    {
        std::shared_ptr< ephemerides::MultiArcEphemeris > multiArcEphemerisModel  =
                std::dynamic_pointer_cast< ephemerides::MultiArcEphemeris >( ephemerisModel );
        safeInterval.first = getSafeInterpolationInterval( multiArcEphemerisModel->getSingleArcEphemerides( ).at( 0 ) ).first;
        safeInterval.second = getSafeInterpolationInterval(
                    multiArcEphemerisModel->getSingleArcEphemerides( ).at(
                        multiArcEphemerisModel->getSingleArcEphemerides( ).size( ) - 1 ) ).second;
    }
    return safeInterval;
}

} // namespace simulation_setup

} // namespace tudat


#pragma once

#include <string>
#include <array>

#include <Eigen/Dense>

namespace hebi {

struct HexapodParameters
{
  // Body parameters
  float mass_;

  std::array<float, 6> leg_angle_;
  std::array<float, 6> leg_offset_;
  std::array<bool, 6> leg_flip_;

  // Stance
  float stance_radius_;
  float default_body_height_;
  float min_z_;
  float max_z_;
  float max_r_; // max radius (for stance mode body weight shifting)

  // Step threshold
  float step_threshold_rotate_; // rad
  float step_threshold_shift_; // m

  // Logging
  bool logging_enabled_;
  float high_log_frequency_hz_;
  float low_log_frequency_hz_;

  // true on success, false on failure
  bool loadFromFile(const std::string& file);
  bool saveToFile(const std::string& file) const;
  // Index must be in [0,5]
  Eigen::Matrix4d getLegTransform(int index) const;
  void resetToDefaults();
};

} // namespace hebi