// The MIT License (MIT)
//
// Copyright (c) 2014-2016 Darrell Wright
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files( the "Software" ), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and / or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in all
// copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.

#include <boost/scoped_array.hpp>
#include <iostream>
#include <limits>
#include <stdexcept>
#include <string>
#include <Wt/WApplication>

#include "channelcombiner.h"
#include <daw/grayscale_filter/genericimage.h>
#include "largepixeldata.h"

namespace daw { namespace imaging {
	namespace {
		float too_gs_small( float const & R, float const & G, float const & B ) {
			return 0.299f*R + 0.587f*G + 0.114f*B;
		}

		float round( float const & value ) {
			return floor( value + 0.5f );
		}

		uint8_t max( rgb3 lpd ) {
			int ret = lpd.red;
			if( lpd.green > ret ) {
				ret = lpd.green;
			}
			if( lpd.blue > ret ) {
				ret = lpd.blue;
			}
			return ret;
		}

		template<typename T>
		auto max3( T value1, T const & value2, T const & value3 ) {
			if( value2 > value1 ) {
				value1 = value2;
			}
			if( value3 > value1 ) {
				value1 = value3;
			}
			return value1;
		}

		auto max3( LargePixelData const & pd ) {
			return max3( pd.R, pd.G, pd.B );
		}

		template<typename T>
		auto min3( T value1, T const & value2, T const & value3 ) {
			if( value2 < value1 ) {
				value1 = value2;
			}
			if( value3 < value1 ) {
				value1 = value3;
			}
			return value1;
		}

		auto min3( LargePixelData const & pd ) {
			return min3( pd.R, pd.G, pd.B );
		}

		void clampvalue( int &value, int const min, int const max ) {
			if( value < min ) {
				Wt::log( "info" ) << "Had to clamp value from from " << value << " to " << min;
				value = min;
			} else if( value > max ) {
				Wt::log( "info" ) << "Had to clamp value from from " << value << " to " << max;
				value = max;
			}
		}

		void clampvalue( LargePixelData &value, int const min, int const max ) {
			clampvalue( value.R, min, max );
			clampvalue( value.G, min, max );
			clampvalue( value.B, min, max );
		}

		float colform( const rgb3 c, float const R, float const G, float const B ) {
			return R*(float)c.red + G*(float)c.green + B*(float)c.blue;
		}
	}
	GenericImage<rgb3> ChannelCombiner::runfilter( GenericImage<rgb3> const & image_y, GenericImage<rgb3> const & image_u, GenericImage<rgb3> const & image_v ) const {
		if( !((image_y.size( ) == image_v.size( )) && (image_y.size( ) == image_v.size( ))) ) {
			const std::string msg = "Images are not the same size in channel filter";
			Wt::log( "error" ) << msg;
			throw std::runtime_error( msg );
		}

		boost::scoped_array<LargePixelData> output_lpdimg( new LargePixelData[image_y.size( )] );	//output_lpdimg
		for( size_t n=0; n<image_y.size( ); ++n ) {
			float const Y = (float)image_y[n].red;
			float const U = (float)image_u[n].red;
			float const V = (float)image_v[n].red;
			output_lpdimg[n] = LargePixelData( (int)(Y + 1.14f*V), (int)(Y - 0.395f*U - 0.581f*V), (int)(Y + 2.032f*U) );
		}

		LargePixelData pd_min( std::numeric_limits<int>::max( ), std::numeric_limits<int>::max( ), std::numeric_limits<int>::max( ) );
		LargePixelData pd_max( std::numeric_limits<int>::min( ), std::numeric_limits<int>::min( ), std::numeric_limits<int>::min( ) );

		// Do not parallelize without accounting for shared data
		for( size_t n=0; n<image_y.size( ); ++n ) {
			LargePixelData::min( output_lpdimg[n], pd_min );
			LargePixelData::max( output_lpdimg[n], pd_max );
		}

		auto const max_all = max3( pd_max );
		auto const min_all = min3( pd_min );
		auto const range_all = static_cast<float>(max_all - min_all);

		GenericImage<rgb3> output_image{ image_y.width( ), image_y.height( ) };

		//#pragma omp parallel for
		for( int n=0; n<(int)image_y.size( ); ++n ) {
			LargePixelData cur_value;
			cur_value.R = (int)((float)(output_lpdimg[n].R-pd_min.R)*(255.0f/range_all));
			cur_value.G = (int)((float)(output_lpdimg[n].G-pd_min.G)*(255.0f/range_all));
			cur_value.B = (int)((float)(output_lpdimg[n].B-pd_min.B)*(255.0f/range_all));
			clampvalue( cur_value, 0, 255 );
			output_image[n] = rgb3( (unsigned char)cur_value.R, (unsigned char)cur_value.G, (unsigned char)cur_value.B );

		}

		return output_image;
	}
}}


// Boost.Geometry - gis-projections (based on PROJ4)

// Copyright (c) 2008-2015 Barend Gehrels, Amsterdam, the Netherlands.

// This file was modified by Oracle on 2017, 2018.
// Modifications copyright (c) 2017-2018, Oracle and/or its affiliates.
// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle.

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

// This file is converted from PROJ4, http://trac.osgeo.org/proj
// PROJ4 is originally written by Gerald Evenden (then of the USGS)
// PROJ4 is maintained by Frank Warmerdam
// PROJ4 is converted to Boost.Geometry by Barend Gehrels

// Last updated version of proj: 5.0.0

// Original copyright notice:

// Permission is hereby granted, free of charge, to any person obtaining a
// copy of this software and associated documentation files (the "Software"),
// to deal in the Software without restriction, including without limitation
// the rights to use, copy, modify, merge, publish, distribute, sublicense,
// and/or sell copies of the Software, and to permit persons to whom the
// Software is furnished to do so, subject to the following conditions:

// The above copyright notice and this permission notice shall be included
// in all copies or substantial portions of the Software.

// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
// OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
// DEALINGS IN THE SOFTWARE.

#ifndef BOOST_GEOMETRY_PROJECTIONS_SOMERC_HPP
#define BOOST_GEOMETRY_PROJECTIONS_SOMERC_HPP

#include <boost/geometry/util/math.hpp>

#include <boost/geometry/srs/projections/impl/base_static.hpp>
#include <boost/geometry/srs/projections/impl/base_dynamic.hpp>
#include <boost/geometry/srs/projections/impl/projects.hpp>
#include <boost/geometry/srs/projections/impl/factory_entry.hpp>
#include <boost/geometry/srs/projections/impl/aasincos.hpp>

namespace boost { namespace geometry
{

namespace srs { namespace par4
{
    struct somerc {}; // Swiss. Obl. Mercator

}} //namespace srs::par4

namespace projections
{
    #ifndef DOXYGEN_NO_DETAIL
    namespace detail { namespace somerc
    {
            static const double epsilon = 1.e-10;
            static const int n_iter = 6;

            template <typename T>
            struct par_somerc
            {
                T K, c, hlf_e, kR, cosp0, sinp0;
            };

            // template class, using CRTP to implement forward/inverse
            template <typename T, typename Parameters>
            struct base_somerc_ellipsoid
                : public base_t_fi<base_somerc_ellipsoid<T, Parameters>, T, Parameters>
            {
                par_somerc<T> m_proj_parm;

                inline base_somerc_ellipsoid(const Parameters& par)
                    : base_t_fi<base_somerc_ellipsoid<T, Parameters>, T, Parameters>(*this, par)
                {}

                // FORWARD(e_forward)
                // Project coordinates from geographic (lon, lat) to cartesian (x, y)
                inline void fwd(T& lp_lon, T& lp_lat, T& xy_x, T& xy_y) const
                {
                    static const T fourth_pi = detail::fourth_pi<T>();
                    static const T half_pi = detail::half_pi<T>();

                    T phip, lamp, phipp, lampp, sp, cp;

                    sp = this->m_par.e * sin(lp_lat);
                    phip = 2.* atan( exp( this->m_proj_parm.c * (
                        log(tan(fourth_pi + 0.5 * lp_lat)) - this->m_proj_parm.hlf_e * log((1. + sp)/(1. - sp)))
                        + this->m_proj_parm.K)) - half_pi;
                    lamp = this->m_proj_parm.c * lp_lon;
                    cp = cos(phip);
                    phipp = aasin(this->m_proj_parm.cosp0 * sin(phip) - this->m_proj_parm.sinp0 * cp * cos(lamp));
                    lampp = aasin(cp * sin(lamp) / cos(phipp));
                    xy_x = this->m_proj_parm.kR * lampp;
                    xy_y = this->m_proj_parm.kR * log(tan(fourth_pi + 0.5 * phipp));
                }

                // INVERSE(e_inverse)  ellipsoid & spheroid
                // Project coordinates from cartesian (x, y) to geographic (lon, lat)
                inline void inv(T& xy_x, T& xy_y, T& lp_lon, T& lp_lat) const
                {
                    static const T fourth_pi = detail::fourth_pi<T>();

                    T phip, lamp, phipp, lampp, cp, esp, con, delp;
                    int i;

                    phipp = 2. * (atan(exp(xy_y / this->m_proj_parm.kR)) - fourth_pi);
                    lampp = xy_x / this->m_proj_parm.kR;
                    cp = cos(phipp);
                    phip = aasin(this->m_proj_parm.cosp0 * sin(phipp) + this->m_proj_parm.sinp0 * cp * cos(lampp));
                    lamp = aasin(cp * sin(lampp) / cos(phip));
                    con = (this->m_proj_parm.K - log(tan(fourth_pi + 0.5 * phip)))/this->m_proj_parm.c;
                    for (i = n_iter; i ; --i) {
                        esp = this->m_par.e * sin(phip);
                        delp = (con + log(tan(fourth_pi + 0.5 * phip)) - this->m_proj_parm.hlf_e *
                            log((1. + esp)/(1. - esp)) ) *
                            (1. - esp * esp) * cos(phip) * this->m_par.rone_es;
                        phip -= delp;
                        if (fabs(delp) < epsilon)
                            break;
                    }
                    if (i) {
                        lp_lat = phip;
                        lp_lon = lamp / this->m_proj_parm.c;
                    } else {
                        BOOST_THROW_EXCEPTION( projection_exception(error_tolerance_condition) );
                    }
                }

                static inline std::string get_name()
                {
                    return "somerc_ellipsoid";
                }

            };

            // Swiss. Obl. Mercator
            template <typename Parameters, typename T>
            inline void setup_somerc(Parameters& par, par_somerc<T>& proj_parm)
            {
                static const T fourth_pi = detail::fourth_pi<T>();

                T cp, phip0, sp;

                proj_parm.hlf_e = 0.5 * par.e;
                cp = cos(par.phi0);
                cp *= cp;
                proj_parm.c = sqrt(1 + par.es * cp * cp * par.rone_es);
                sp = sin(par.phi0);
                proj_parm.cosp0 = cos( phip0 = aasin(proj_parm.sinp0 = sp / proj_parm.c) );
                sp *= par.e;
                proj_parm.K = log(tan(fourth_pi + 0.5 * phip0)) - proj_parm.c * (
                    log(tan(fourth_pi + 0.5 * par.phi0)) - proj_parm.hlf_e *
                    log((1. + sp) / (1. - sp)));
                proj_parm.kR = par.k0 * sqrt(par.one_es) / (1. - sp * sp);
            }

    }} // namespace detail::somerc
    #endif // doxygen

    /*!
        \brief Swiss. Obl. Mercator projection
        \ingroup projections
        \tparam Geographic latlong point type
        \tparam Cartesian xy point type
        \tparam Parameters parameter type
        \par Projection characteristics
         - Cylindrical
         - Ellipsoid
         - For CH1903
        \par Example
        \image html ex_somerc.gif
    */
    template <typename T, typename Parameters>
    struct somerc_ellipsoid : public detail::somerc::base_somerc_ellipsoid<T, Parameters>
    {
        inline somerc_ellipsoid(const Parameters& par) : detail::somerc::base_somerc_ellipsoid<T, Parameters>(par)
        {
            detail::somerc::setup_somerc(this->m_par, this->m_proj_parm);
        }
    };

    #ifndef DOXYGEN_NO_DETAIL
    namespace detail
    {

        // Static projection
        BOOST_GEOMETRY_PROJECTIONS_DETAIL_STATIC_PROJECTION(srs::par4::somerc, somerc_ellipsoid, somerc_ellipsoid)

        // Factory entry(s)
        template <typename T, typename Parameters>
        class somerc_entry : public detail::factory_entry<T, Parameters>
        {
            public :
                virtual base_v<T, Parameters>* create_new(const Parameters& par) const
                {
                    return new base_v_fi<somerc_ellipsoid<T, Parameters>, T, Parameters>(par);
                }
        };

        template <typename T, typename Parameters>
        inline void somerc_init(detail::base_factory<T, Parameters>& factory)
        {
            factory.add_to_factory("somerc", new somerc_entry<T, Parameters>);
        }

    } // namespace detail
    #endif // doxygen

} // namespace projections

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_PROJECTIONS_SOMERC_HPP


/*
 * Copyright (c) 2011, Mattia Penati <mattia.penati@gmail.com>
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without modification,
 * are permitted provided that the following conditions are met:
 *
 *     * Redistributions of source code must retain the above copyright notice,
 *       this list of conditions and the following disclaimer.
 *     * Redistributions in binary form must reproduce the above copyright notice,
 *       this list of conditions and the following disclaimer in the documentation
 *       and/or other materials provided with the distribution.
 *     * Neither the name of the Politecnico di Milano nor the names of its
 *       contributors may be used to endorse or promote products derived from
 *       this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR
 * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON
 * ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */

#ifndef AMA_TENSOR_MUL_MUL_TEMPORARY_HPP
#define AMA_TENSOR_MUL_MUL_TEMPORARY_HPP 1

#include <ama/tensor/iexp/index_reorder.hpp>
#include <ama/tensor/iexp/iexp_base.hpp>
#include <ama/tensor/mul/mul_calculator.hpp>
#include <boost/mpl/bool.hpp>
#include <boost/mpl/plus.hpp>

/* this class is used to store a temporary expression before the reduction */

namespace ama
{
  namespace tensor_
  {

    /* forward declaration */
    template <typename LEFT, typename RIGHT> class mul_temporary;

    /* specialization of iexp traits */
    template <typename LEFT, typename RIGHT>
    struct iexp_traits< mul_temporary<LEFT, RIGHT> >
    {
      typedef typename LEFT::value_type value_type;

      typedef typename LEFT::dimension_type dimension_type;

      typedef typename mul_index<LEFT, RIGHT>::controvariant_list controvariant_list;
      typedef typename mul_index<LEFT, RIGHT>::covariant_list covariant_list;

      typedef ::boost::mpl::false_ is_assignable;
    };

    /* class declaration */
    template <typename LEFT, typename RIGHT>
    class mul_temporary:
        public iexp_base< mul_temporary<LEFT, RIGHT> >
    {
    protected:
      typedef iexp_base< mul_temporary<LEFT, RIGHT> > base_type;

      typedef LEFT left_operand;
      typedef RIGHT right_operand;

    public:
      /* constructor */
      mul_temporary(left_operand const & left,
                    right_operand const & right)
          : m_left(left),
            m_right(right) { }

    public:
      typedef typename base_type::value_type value_type;

      /* retrieve the value */
      template <typename IMAP>
      value_type at() const
      {
        return m_left.template at<IMAP>() * m_right.template at<IMAP>();
      }

    protected:
      left_operand const m_left;
      right_operand const m_right;
    };

  }
}

#endif /* AMA_TENSOR_MUL_MUL_TEMPORARY_HPP */


#ifndef STAN_MATH_PRIM_PROB_BERNOULLI_LOGIT_GLM_RNG_HPP
#define STAN_MATH_PRIM_PROB_BERNOULLI_LOGIT_GLM_RNG_HPP

#include <stan/math/prim/fun/Eigen.hpp>
#include <stan/math/prim/meta.hpp>
#include <stan/math/prim/err.hpp>
#include <stan/math/prim/fun/as_column_vector_or_scalar.hpp>
#include <stan/math/prim/fun/as_array_or_scalar.hpp>
#include <stan/math/prim/fun/inv_logit.hpp>
#include <stan/math/prim/fun/scalar_seq_view.hpp>
#include <boost/random/bernoulli_distribution.hpp>
#include <boost/random/variate_generator.hpp>
#include <vector>

namespace stan {
namespace math {

/** \ingroup multivar_dists
 * Returns a draw from the Generalized Linear Model (GLM)
 * with Bernoulli distribution and logit link function.
 *
 * This is a convenience wrapper around
 * bernoulli_logit_rng(alpha + x * beta, rng).
 * @tparam T_x type of the matrix of independent variables (features);
 * this should be an Eigen::Matrix whose number of columns should
 * match the length of beta; the number of rows is the number of
 * samples.
 * @tparam T_alpha type of the intercept(s); this can be a vector of
 * the same length as y) of intercepts or a single value (for models
 * with constant intercept); if a vector its length should match x's
 * row-count;
 * @tparam T_beta type of the weight vector;
 * @tparam RNG Type of pseudo-random number generator.
 * @param x design matrix
 * @param alpha intercept (in log odds)
 * @param beta weight vector
 * @param rng Pseudo-random number generator.
 * @return Bernoulli logit glm random variate
 * @throw std::domain_error if x, beta or alpha is infinite.
 * @throw std::invalid_argument if container sizes mismatch.
 */
template <typename T_x, typename T_alpha, typename T_beta, class RNG>
inline typename VectorBuilder<true, int, T_alpha>::type bernoulli_logit_glm_rng(
    const T_x& x, const T_alpha& alpha, const T_beta& beta, RNG& rng) {
  using boost::bernoulli_distribution;
  using boost::variate_generator;
  using T_x_ref = ref_type_t<T_x>;
  using T_alpha_ref = ref_type_t<T_alpha>;
  using T_beta_ref = ref_type_t<T_beta>;

  const size_t N = x.cols();
  const size_t M = x.rows();

  static const char* function = "bernoulli_logit_glm_rng";
  check_consistent_size(function, "Weight vector", beta, N);
  check_consistent_size(function, "Vector of intercepts", alpha, M);
  T_x_ref x_ref = x;
  T_alpha_ref alpha_ref = alpha;
  T_beta_ref beta_ref = beta;
  check_finite(function, "Matrix of independent variables", x_ref);
  check_finite(function, "Weight vector", beta_ref);
  check_finite(function, "Intercept", alpha_ref);

  const auto& beta_vector = as_column_vector_or_scalar(beta_ref);

  Eigen::VectorXd x_beta;
  if (is_vector<T_beta>::value) {
    x_beta = x_ref * beta_vector;
  } else {
    x_beta = (x_ref.array() * forward_as<double>(beta_vector)).rowwise().sum();
  }

  scalar_seq_view<T_alpha> alpha_vec(alpha_ref);

  VectorBuilder<true, int, T_alpha> output(M);

  for (size_t m = 0; m < M; ++m) {
    double theta_m = alpha_vec[m] + x_beta(m);
    variate_generator<RNG&, bernoulli_distribution<>> bernoulli_rng(
        rng, bernoulli_distribution<>(inv_logit(theta_m)));
    output[m] = bernoulli_rng();
  }

  return output.data();
}
}  // namespace math
}  // namespace stan
#endif


//=======================================================================
// Copyright 2001 Jeremy G. Siek, Andrew Lumsdaine, Lie-Quan Lee,
//
// 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)
//=======================================================================
#include <iostream>
#include <boost/property_map/property_map.hpp>

int main()
{
    using namespace boost;
    double x[] = { 0.2, 4.5, 3.2 };
    iterator_property_map< double*, identity_property_map, double, double& >
        pmap(x);
    std::cout << "x[1] = " << get(pmap, 1) << std::endl;
    put(pmap, 0, 1.7);
    std::cout << "x[0] = " << pmap[0] << std::endl;
    return 0;
}


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

#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/passthrough.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/segmentation/extract_clusters.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/filters/statistical_outlier_removal.h>

#include <pcl/filters/crop_box.h>
#include <tf/tf.h>
#include <tf/transform_broadcaster.h>
#include <tf_conversions/tf_eigen.h>
#include <pcl/segmentation/extract_polygonal_prism_data.h>

#include <tf/transform_listener.h>

#include "std_msgs/String.h"
#include <sstream>

int n_cluster, big_cluster, cluster_size;

std::vector<ros::Publisher> pub_vec;

std::string cluster_name;

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

sensor_msgs::PointCloud2::Ptr original_cloud (new sensor_msgs::PointCloud2);
sensor_msgs::PointCloud2::Ptr voxel_cloud (new sensor_msgs::PointCloud2);
sensor_msgs::PointCloud2::Ptr passthrough_cloud (new sensor_msgs::PointCloud2);
sensor_msgs::PointCloud2::Ptr segmentation_cloud (new sensor_msgs::PointCloud2);
sensor_msgs::PointCloud2::Ptr cluster_cloud (new sensor_msgs::PointCloud2);
sensor_msgs::PointCloud2::Ptr outlier_cloud (new sensor_msgs::PointCloud2);
std::vector<sensor_msgs::PointCloud2::Ptr> pc2_clusters;

void callback(const PointCloud::ConstPtr& msg)
{

  pc2_clusters.clear();

  std::string world_frame="camera_depth_optical_frame";

  // Downsample points (voxel grid filter)
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_ptr(new pcl::PointCloud<pcl::PointXYZ>(*msg));
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_voxel_filtered(new pcl::PointCloud<pcl::PointXYZ>());
  pcl::VoxelGrid<pcl::PointXYZ> voxel_filter;
  voxel_filter.setInputCloud(cloud_ptr);
  voxel_filter.setLeafSize(float(0.005), float(0.005), float(0.005)); //In Meters It has to be modificated
  voxel_filter.filter(*cloud_voxel_filtered);

  //passthrough filter
  pcl::PointCloud<pcl::PointXYZ>xf_cloud, yf_cloud, zf_cloud;

  //filter in x (Lateral)
  pcl::PassThrough<pcl::PointXYZ> pass_x;
  pass_x.setInputCloud(cloud_voxel_filtered);
  pass_x.setFilterFieldName("x");
  pass_x.setFilterLimits(-0.5,0.5);
  pass_x.filter(xf_cloud);

  //filter in y (Amunt)
  pcl::PointCloud<pcl::PointXYZ>::Ptr xf_cloud_ptr(new pcl::PointCloud<pcl::PointXYZ>(xf_cloud));
  pcl::PassThrough<pcl::PointXYZ> pass_y;
  pass_y.setInputCloud(xf_cloud_ptr);
  pass_y.setFilterFieldName("y");
  pass_y.setFilterLimits(-0.5,0.3);
  pass_y.filter(yf_cloud);

    //filter in z (Endavant)
  pcl::PointCloud<pcl::PointXYZ>::Ptr yf_cloud_ptr(new pcl::PointCloud<pcl::PointXYZ>(yf_cloud));
  pcl::PassThrough<pcl::PointXYZ> pass_z;
  pass_z.setInputCloud(yf_cloud_ptr);
  pass_z.setFilterFieldName("z");
  pass_z.setFilterLimits(0.3,1.3);
  pass_z.filter(zf_cloud);

  //plane segmentation
  pcl::PointCloud<pcl::PointXYZ>::Ptr cropped_cloud(new pcl::PointCloud<pcl::PointXYZ>(zf_cloud));
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_f(new pcl::PointCloud<pcl::PointXYZ>);
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZ>);
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_plane(new pcl::PointCloud<pcl::PointXYZ>);

  pcl::SACSegmentation<pcl::PointXYZ> seg;
  pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
  pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
  seg.setOptimizeCoefficients(true);
  seg.setModelType(pcl::SACMODEL_PLANE);
  seg.setMethodType(pcl::SAC_RANSAC);
  seg.setMaxIterations(400);
  seg.setDistanceThreshold(0.001);

  seg.setInputCloud(cropped_cloud);
  seg.segment(*inliers, *coefficients);
  if (inliers->indices.size() == 0) {
    ROS_WARN_STREAM("Could not estimate a planar model for the given dataset.");
  }

  pcl::ExtractIndices<pcl::PointXYZ> extract;
  extract.setInputCloud(cropped_cloud);
  extract.setIndices(inliers);
  extract.setNegative(false);

  extract.filter(*cloud_plane);

  extract.setNegative(true);
  extract.filter(*cloud_f);

  //euclidean cluster extraction
  pcl::PointCloud<pcl::PointXYZ>::Ptr cluster_cloud(new pcl::PointCloud<pcl::PointXYZ>);

  pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>);
  *cloud_filtered = *cloud_f;
  tree->setInputCloud(cloud_filtered);
  std::vector<pcl::PointIndices> cluster_indices;
  pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;
  ec.setClusterTolerance(0.08); //Cluster tolerance
  ec.setMinClusterSize(50);
  ec.setMaxClusterSize(1000);
  ec.setSearchMethod(tree);
  ec.setInputCloud(cloud_filtered);
  ec.extract(cluster_indices);

  ros::NodeHandle nh;

  //Create a publisher for each cluster
  for (int i=0; i<cluster_indices.size(); ++i) {
    std::string topicName = "/cluster_" + boost::lexical_cast<std::string>(i);
    ros::Publisher pub = nh.advertise<sensor_msgs::PointCloud2> (topicName, 1);
    pub_vec.push_back(pub);
    }

  std::vector<pcl::PointIndices>::const_iterator it;
  std::vector<int>::const_iterator pit;

  int j = 0;
  std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> clusters;

  for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin(); it != cluster_indices.end(); ++it) {
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cluster (new pcl::PointCloud<pcl::PointXYZ>);

    for (std::vector<int>::const_iterator pit = it->indices.begin(); pit != it->indices.end(); pit++)
      cloud_cluster->points.push_back(cloud_filtered->points[*pit]);

  cloud_cluster->width = cloud_cluster->points.size();
  cloud_cluster->height = 1;
  cloud_cluster->is_dense = true;
  clusters.push_back(cloud_cluster);

  sensor_msgs::PointCloud2::Ptr tempROSMsg(new sensor_msgs::PointCloud2);
  pcl::toROSMsg(*cloud_cluster, *tempROSMsg);
  pc2_clusters.push_back(tempROSMsg);
  tempROSMsg->header.frame_id = world_frame;
  tempROSMsg-> header.stamp = ros::Time::now();
  pub_vec[j].publish(tempROSMsg);
  ++j;

  }

  //statical outlier removal
  pcl::PointCloud<pcl::PointXYZ>::Ptr cluster_cloud_ptr(new pcl::PointCloud<pcl::PointXYZ>);
  *cluster_cloud_ptr = *cloud_f;
  pcl::PointCloud<pcl::PointXYZ>::Ptr sor_cloud_filtered(new pcl::PointCloud<pcl::PointXYZ>);

  pcl::StatisticalOutlierRemoval<pcl::PointXYZ>sor;
  sor.setInputCloud(cluster_cloud_ptr);
  sor.setMeanK(100);
  sor.setStddevMulThresh(1.0);
  sor.filter(*sor_cloud_filtered);

  //broadcast transform
  for(int i=0; i<j; i++){
    pcl::PointCloud<pcl::PointXYZ>::Ptr cluster_vec= clusters.at(i);

    Eigen::Vector4f centroid;
    pcl::compute3DCentroid(*cluster_vec, centroid);
    Eigen::Vector3f vec3 = centroid.head<3>();

    static tf::TransformBroadcaster br;
    tf::Transform part_transform;

    part_transform.setOrigin(tf::Vector3(vec3(0),vec3(1),vec3(2)));
    tf::Quaternion q;
    q.setRPY(0 ,0, 0);
    part_transform.setRotation(q);

    std::stringstream ss;
    ss << "cluster_" << i;

    ROS_INFO_STREAM(vec3);

    br.sendTransform(tf::StampedTransform(part_transform, ros::Time::now(), world_frame, ss.str()));

  }

 //Calculate de biggest cluster
  n_cluster = 0;
  big_cluster = 0;
  cluster_size = 0;

 for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin(); it != cluster_indices.end(); ++it) {
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cluster (new pcl::PointCloud<pcl::PointXYZ>);

    for (std::vector<int>::const_iterator pit = it->indices.begin(); pit != it->indices.end(); pit++)
      cloud_cluster->points.push_back(cloud_filtered->points[*pit]);

    if (cloud_cluster->points.size() > cluster_size){

	cluster_size = cloud_cluster->points.size();
	big_cluster = n_cluster;

    }

    n_cluster++;
  }

  std::cout << "El cluster mes gran te" << cluster_size << " punts \n";
  std::cout << "I es el cluster_" << big_cluster << "\n";
  cluster_name = "/cluster_" + boost::lexical_cast<std::string>(big_cluster);
  std::cout << cluster_name << "\n";

  //Convert point cloud PCL->ROS
  pcl::toROSMsg(*msg, *original_cloud);
  original_cloud->header.frame_id=world_frame;
  original_cloud->header.stamp=ros::Time::now();

}

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

  //Subscribe to the kinect point cloud topic
  ros::Subscriber sub = nh.subscribe<PointCloud>("/camera/depth/points", 1, callback);

  //output point cloud
  ros::Publisher pub_original = nh.advertise<PointCloud>("original_cloud", 1);

  //Biggest cluster publisher
  ros::Publisher pub_big_cluster = nh.advertise<std_msgs::String>("big_cluster", 100);

  ros::Rate loop_rate(10);

  while(ros::ok())
  {
    //Publish the original point cloud
    pub_original.publish(original_cloud);

    //Publish the biggest cluster
    std_msgs::String msg;
    std::stringstream ss;
    ss << cluster_name;
    msg.data = ss.str();
    pub_big_cluster.publish(msg);

    ros::spinOnce();
    loop_rate.sleep();
  }

  ros::spin();

return 0;
}


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

#ifndef BOOST_NUMERIC_BINDINGS_AMOS_AMOS_OVERLOADS_HPP
#define BOOST_NUMERIC_BINDINGS_AMOS_AMOS_OVERLOADS_HPP

#include <boost/numeric/bindings/amos/amos.h>
#include <boost/numeric/bindings/traits/type.hpp>
#include <boost/numeric/bindings/traits/type_traits.hpp>

namespace boost { namespace numeric { namespace bindings { namespace amos { namespace detail {

  using namespace ::boost::numeric::bindings::traits ;

  //
  // BESI
  //

  //inline
  //void besi(const fcomplex * z, const fcomplex * fnu, const integer_t * kode, const integer_t * n, fcomplex* cy, integer_t * nz, integer_t * error) ;

  inline
  void besi(const double& z, const double& fnu, const integer_t& kode, const integer_t& n, double* cy, integer_t& nz, integer_t& error)
  { AMOS_DBESI( &z, &fnu, &kode, &n, cy, &nz ) ; }

  inline
  void besi(const complex_f& z, const float&  fnu, const integer_t& kode, const integer_t& n, complex_f* cy, integer_t & nz, integer_t & error)
  { AMOS_CBESI( complex_ptr( &z ), &fnu, &kode, &n, complex_ptr( cy ), &nz, &error ) ; }

  // inline
  // void besi(const complex_d* z, const double* fnu, const integer_t * kode, const integer_t * n, complex_d* cy, integer_t * nz, integer_t * error)  ;

  //
  // BESJ
  //

  inline
  void besj(const double& z, const double& fnu, const integer_t& kode, const integer_t& n, double* cy, integer_t& nz, integer_t& error)
  { AMOS_DBESJ( &z, &fnu, &n, cy, &nz ) ; }

  inline
  void besj(const complex_f& z, const float&  fnu, const integer_t& kode, const integer_t& n, complex_f* cy, integer_t & nz, integer_t & error)
  { AMOS_CBESJ( complex_ptr( &z ), &fnu, &kode, &n, complex_ptr( cy ), &nz, &error ) ; }

  //
  // BESY
  //

  inline
  void besy(const double& z, const double& fnu, const integer_t& kode, const integer_t& n, double* cy, integer_t& nz, double* wrk, integer_t& error)
  { AMOS_DBESY( &z, &fnu, &n, cy ) ; }

}}}}}

#endif // BOOST_NUMERIC_BINDINGS_AMOS_AMOS_OVERLOADS_HPP


// Copyright © 2016-2019 Thomas Nagler and Thibault Vatter
//
// This file is part of the vinecopulib library and licensed under the terms of
// the MIT license. For a copy, see the LICENSE file in the root directory of
// vinecopulib or https://vinecopulib.github.io/vinecopulib/.

#pragma once

#include <boost/graph/adjacency_list.hpp>
#include <vinecopulib/bicop/class.hpp>
#include <vinecopulib/vinecop/fit_controls.hpp>
#include <vinecopulib/vinecop/rvine_structure.hpp>
#include <vinecopulib/misc/tools_interface.hpp>

// to allow for (auto e : boost::edges(g)) notation
namespace std {
template<class T>
T begin(const std::pair<T, T> &eItPair)
{
    return eItPair.first;
}

template<class T>
T end(const std::pair<T, T> &eItPair)
{
    return eItPair.second;
}
}
namespace vinecopulib {

namespace tools_select {

double calculate_criterion(const Eigen::Matrix<double, Eigen::Dynamic, 2>& data,
                           std::string tree_criterion,
                           Eigen::VectorXd weights);

Eigen::MatrixXd calculate_criterion_matrix(const Eigen::MatrixXd &data,
                                           std::string tree_criterion,
                                           const Eigen::VectorXd& weights);

// boost::graph represenation of a vine tree
struct VertexProperties
{
    std::vector<size_t> conditioning;
    std::vector<size_t> conditioned;
    std::vector<size_t> all_indices;
    std::vector<size_t> prev_edge_indices;
    Eigen::VectorXd hfunc1;
    Eigen::VectorXd hfunc2;
};
struct EdgeProperties
{
    std::vector<size_t> conditioning;
    std::vector<size_t> conditioned;
    std::vector<size_t> all_indices;
    Eigen::Matrix<double, Eigen::Dynamic, 2> pc_data;
    Eigen::VectorXd hfunc1;
    Eigen::VectorXd hfunc2;
    double weight;
    double crit;
    vinecopulib::Bicop pair_copula;
    double loglik;
    double npars;
    double fit_id;
};
typedef boost::adjacency_list <
boost::vecS,
boost::vecS,
boost::undirectedS,
VertexProperties,
boost::property<boost::edge_weight_t, double, EdgeProperties>> VineTree;

typedef boost::graph_traits<VineTree>::edge_descriptor EdgeIterator;
typedef std::pair<EdgeIterator, bool> FoundEdge;

class VinecopSelector
{
public:
    VinecopSelector(const Eigen::MatrixXd& data,
                    const FitControlsVinecop& controls);

    std::vector<std::vector<Bicop>> get_pair_copulas() const;

    RVineStructure get_rvine_structure() const;

    static std::vector<std::vector<Bicop>> make_pair_copula_store(
        size_t d,
        size_t trunc_lvl);

    void select_all_trees(const Eigen::MatrixXd &data);

    void sparse_select_all_trees(const Eigen::MatrixXd &data);

    double get_loglik() const;

    double get_threshold() const;

    size_t get_nobs() const;

    std::vector<VineTree> get_trees() const {return trees_;};
    std::vector<VineTree> get_trees_opt() const {return trees_opt_;};

protected:
    virtual void select_tree(size_t t);

    virtual void finalize(size_t trunc_lvl) = 0;

    double get_mbicv_of_tree(size_t t, double loglik);

    double get_loglik_of_tree(size_t t);

    double get_npars_of_tree(size_t t);

    void set_tree_to_indep(size_t t);

    void print_pair_copulas_of_tree(size_t);

    std::vector<double> get_thresholded_crits();

    void initialize_new_fit(const Eigen::MatrixXd &data);

    void set_current_fit_as_opt(const double& loglik);

    virtual void add_allowed_edges(VineTree &tree) = 0;

    Eigen::MatrixXd get_pc_data(size_t v0, size_t v1, const VineTree &tree);

    ptrdiff_t find_common_neighbor(size_t v0, size_t v1,
                                   const VineTree &tree);

    virtual double compute_fit_id(const EdgeProperties& e);

    size_t n_;
    size_t d_;
    FitControlsVinecop controls_;
    tools_thread::ThreadPool pool_;
    std::vector<VineTree> trees_;
    RVineStructure vine_struct_;
    std::vector<std::vector<Bicop>> pair_copulas_;
    // for sparse selction
    std::vector<VineTree> trees_opt_;
    double loglik_;
    double threshold_;
    double psi0_; // initial prior probability for mbicv

    double get_next_threshold(std::vector<double> &thresholded_crits);

    // functions for manipulation of trees ----------------
    VineTree make_base_tree(const Eigen::MatrixXd &data);

    VineTree edges_as_vertices(const VineTree &prev_tree);

    void min_spanning_tree(VineTree &tree);

    void add_edge_info(VineTree &tree);

    void remove_edge_data(VineTree &tree);

    void remove_vertex_data(VineTree &tree);

    void select_pair_copulas(VineTree &tree,
                             const VineTree &tree_opt = VineTree());

    FoundEdge find_old_fit(double fit_id, const VineTree &old_graph);

    double get_tree_loglik(const VineTree &tree);

    double get_tree_npars(const VineTree &tree);

    size_t get_num_non_indeps_of_tree(size_t t);

    std::string get_pc_index(const EdgeIterator &e, const VineTree &tree);
};

class StructureSelector : public VinecopSelector
{
public:
    StructureSelector(const Eigen::MatrixXd &data,
                      const FitControlsVinecop &controls);

    ~StructureSelector()
    {
    }

protected:
    void add_allowed_edges(VineTree &tree);

    void finalize(size_t trunc_lvl);
};

class FamilySelector : public VinecopSelector
{
public:
    FamilySelector(const Eigen::MatrixXd &data,
                   const RVineStructure &vine_struct,
                   const FitControlsVinecop &controls);

    ~FamilySelector()
    {
    }

protected:
    void add_allowed_edges(VineTree &tree);

    void finalize(size_t trunc_lvl);

};

}

}

#include <vinecopulib/vinecop/implementation/tools_select.ipp>


#include <algorithm>
#include <functional>
#include <string>
#include <random>

#include <boost/math/constants/constants.hpp>
#include <Euclid/Util/Assert.h>

namespace Euclid
{

namespace _impl
{

inline void mask_filter(const RTCFilterFunctionNArguments* args)
{
    EASSERT(args->N == 1);
    auto mask = reinterpret_cast<uint8_t*>(args->geometryUserPtr);
    EASSERT(mask != nullptr);

    auto primid = RTCHitN_primID(args->hit, 1, 0);
    if (mask[primid] != 1) { args->valid[0] = 0; }
}

} // namespace _impl

inline RayCamera::RayCamera(const Vec3& position,
                            const Vec3& focus,
                            const Vec3& up,
                            float tnear,
                            float tfar)
    : Camera(position, focus, up, tnear, tfar)
{}

inline PerspRayCamera::PerspRayCamera(const Vec3& position,
                                      const Vec3& focus,
                                      const Vec3& up,
                                      float vfov,
                                      float aspect,
                                      float tnear,
                                      float tfar)
    : RayCamera(position, focus, up, tnear, tfar)
{
    auto fov = vfov * boost::math::float_constants::degree;
    this->film.height = 2.0f * std::tan(fov * 0.5f);
    this->film.width = aspect * this->film.height;
}

inline PerspRayCamera::PerspRayCamera(const Vec3& position,
                                      const Vec3& focus,
                                      const Vec3& up,
                                      float vfov,
                                      unsigned width,
                                      unsigned height,
                                      float tnear,
                                      float tfar)
    : RayCamera(position, focus, up, tnear, tfar)
{
    auto fov = vfov * boost::math::float_constants::degree;
    auto aspect = static_cast<float>(width) / height;
    this->film.height = 2.0f * std::tan(fov * 0.5f);
    this->film.width = aspect * film.height;
}

inline PerspRayCamera::~PerspRayCamera() = default;

inline void PerspRayCamera::set_aspect(float aspect)
{
    this->film.width = aspect * this->film.height;
}

inline void PerspRayCamera::set_aspect(unsigned width, unsigned height)
{
    auto aspect = static_cast<float>(width) / height;
    this->film.width = aspect * this->film.height;
}

inline void PerspRayCamera::set_fov(float vfov)
{
    auto fov = vfov * boost::math::float_constants::degree;
    auto aspect = this->film.width / this->film.height;
    this->film.height = 2.0f * std::tan(fov * 0.5f);
    this->film.width = aspect * this->film.height;
}

inline RTCRayHit PerspRayCamera::gen_ray(float s, float t) const
{
    RTCRayHit rayhit;
    Eigen::Vector3f view = -this->dir + (s - 0.5f) * this->film.width * u +
                           (t - 0.5f) * this->film.height * v;
    EASSERT(view.dot(this->dir) < 0.0f);

    rayhit.ray.org_x = this->pos(0);
    rayhit.ray.org_y = this->pos(1);
    rayhit.ray.org_z = this->pos(2);
    rayhit.ray.dir_x = view(0);
    rayhit.ray.dir_y = view(1);
    rayhit.ray.dir_z = view(2);
    rayhit.ray.tnear = this->tnear;
    rayhit.ray.tfar = this->tfar;
    rayhit.ray.flags = 0;
    rayhit.hit.geomID = RTC_INVALID_GEOMETRY_ID;
    return rayhit;
}

inline OrthoRayCamera::OrthoRayCamera(const Vec3& position,
                                      const Vec3& focus,
                                      const Vec3& up,
                                      float xextent,
                                      float yextent,
                                      float tnear,
                                      float tfar)
    : RayCamera(position, focus, up, tnear, tfar)
{
    this->film.width = xextent;
    this->film.height = yextent;
}

inline OrthoRayCamera::~OrthoRayCamera() = default;

inline void OrthoRayCamera::set_extent(float xextent, float yextent)
{
    this->film.width = xextent;
    this->film.height = yextent;
}

inline RTCRayHit OrthoRayCamera::gen_ray(float s, float t) const
{
    RTCRayHit rayhit;
    Eigen::Vector3f origin = this->pos + (s - 0.5f) * this->film.width * u +
                             (t - 0.5f) * this->film.height * v;

    rayhit.ray.org_x = origin(0);
    rayhit.ray.org_y = origin(1);
    rayhit.ray.org_z = origin(2);
    rayhit.ray.dir_x = -this->dir(0);
    rayhit.ray.dir_y = -this->dir(1);
    rayhit.ray.dir_z = -this->dir(2);
    rayhit.ray.tnear = this->tnear;
    rayhit.ray.tfar = this->tfar;
    rayhit.ray.flags = 0;
    rayhit.hit.geomID = RTC_INVALID_GEOMETRY_ID;
    return rayhit;
}

inline RayTracer::RayTracer(int threads)
{
    std::string cfg("threads=");
    cfg.append(std::to_string(threads));
    _device = rtcNewDevice(cfg.c_str());
    if (!_device) {
        auto err = rtcGetDeviceError(_device);
        std::string err_str("Embree device creation error: ");
        err_str.append(std::to_string(err));
        throw std::runtime_error(err_str);
    }

    _scene = rtcNewScene(_device);
    if (!_scene) {
        auto err = rtcGetDeviceError(_device);
        std::string err_str("Embree scene creation error: ");
        err_str.append(std::to_string(err));
        throw std::runtime_error(err_str);
    }

    _material.ambient << 0.1f, 0.1f, 0.1f;
    _material.diffuse << 0.7f, 0.7f, 0.7f;
}

inline RayTracer::~RayTracer()
{
    release_buffers();
    rtcReleaseScene(_scene);
    rtcReleaseDevice(_device);
}

// TODO: add generic type support
inline void RayTracer::attach_geometry_buffers(
    const std::vector<float>& positions,
    const std::vector<unsigned>& indices)
{
    if (positions.empty() || indices.empty()) {
        EWARNING("Input geometry is empty.");
        return;
    }
    if (positions.size() % 3 != 1) {
        throw std::invalid_argument("The last element of the positions buffer "
                                    "is not padded to 16 bytes. Add "
                                    "one more 0.0f to your positions buffer.");
    }
    if (indices.size() % 3 != 0) {
        throw std::invalid_argument("Size of input indices is not divisible by "
                                    "3, thus not a valid triangle mesh.");
    }

    release_buffers();

    _geometry = rtcNewGeometry(_device, RTC_GEOMETRY_TYPE_TRIANGLE);
    rtcSetSharedGeometryBuffer(_geometry,
                               RTC_BUFFER_TYPE_VERTEX,
                               0,
                               RTC_FORMAT_FLOAT3,
                               positions.data(),
                               0,
                               3 * sizeof(float),
                               positions.size() / 3);
    rtcSetSharedGeometryBuffer(_geometry,
                               RTC_BUFFER_TYPE_INDEX,
                               0,
                               RTC_FORMAT_UINT3,
                               indices.data(),
                               0,
                               3 * sizeof(unsigned),
                               indices.size() / 3);
    rtcCommitGeometry(_geometry);
    _geom_id = rtcAttachGeometry(_scene, _geometry);
    rtcCommitScene(_scene);
}

inline void RayTracer::attach_color_buffer(const std::vector<float>* colors,
                                           bool vertex_color)
{
    if (colors && colors->empty()) {
        EWARNING("Input geometry is empty.");
        return;
    }
    if (colors && vertex_color && colors->size() % 3 != 1) {
        throw std::invalid_argument("The last element of the colors buffer "
                                    "is not padded to 16 bytes. Add "
                                    "one more 0.0f to your colors buffer.");
    }

    // ! vertex color -> vertex color
    if (!(_colors && _vertex_color) && (colors && vertex_color)) {
        rtcSetGeometryVertexAttributeCount(_geometry, 1);
        rtcSetSharedGeometryBuffer(_geometry,
                                   RTC_BUFFER_TYPE_VERTEX_ATTRIBUTE,
                                   0,
                                   RTC_FORMAT_FLOAT3,
                                   colors->data(),
                                   0,
                                   3 * sizeof(float),
                                   colors->size() / 3);
        rtcCommitGeometry(_geometry);
    }
    // vertex color -> ! vertex color
    else if ((_colors && _vertex_color) && !(colors && vertex_color)) {
        rtcSetGeometryVertexAttributeCount(_geometry, 0);
        rtcCommitGeometry(_geometry);
    }
    // vertex color -> vertex color
    else if ((_colors && _vertex_color) && (colors && vertex_color)) {
        rtcSetSharedGeometryBuffer(_geometry,
                                   RTC_BUFFER_TYPE_VERTEX_ATTRIBUTE,
                                   0,
                                   RTC_FORMAT_FLOAT3,
                                   colors->data(),
                                   0,
                                   3 * sizeof(float),
                                   colors->size() / 3);
        rtcCommitGeometry(_geometry);
    }

    _colors = colors;
    _vertex_color = vertex_color;
}

inline void RayTracer::attach_face_mask_buffer(const std::vector<uint8_t>* mask)
{
    // no mask -> mask
    if (!_face_mask && mask) {
        rtcSetGeometryIntersectFilterFunction(_geometry, _impl::mask_filter);
    }
    // mask -> no mask
    else if (_face_mask && !mask) {
        rtcSetGeometryIntersectFilterFunction(_geometry, nullptr);
    }

    uint8_t* buffer = nullptr;
    if (mask) { buffer = const_cast<uint8_t*>(mask->data()); }
    rtcSetGeometryUserData(_geometry, buffer);
    rtcCommitGeometry(_geometry);
    _face_mask = mask;
}

inline void RayTracer::release_buffers()
{
    if (_geom_id != -1) {
        rtcDetachGeometry(_scene, _geom_id);
        _geom_id = -1;
        _colors = nullptr;
        _face_mask = nullptr;
        rtcReleaseGeometry(_geometry);
    }
}

inline void RayTracer::set_material(const Material& material)
{
    _material = material;
}

inline void RayTracer::set_background(
    const Eigen::Ref<const Eigen::Array3f>& color)
{
    _background = color;
}

inline void RayTracer::set_background(float r, float g, float b)
{
    _background << r, g, b;
}

inline void RayTracer::enable_light(bool on)
{
    _lighting = on;
}

inline void RayTracer::render_shaded(std::vector<uint8_t>& pixels,
                                     const RayCamera& camera,
                                     int width,
                                     int height,
                                     bool interleaved)
{
    pixels.resize(3 * width * height);
    auto diffuse_color = _select_diffuse_color();

    RTCIntersectContext context;
    rtcInitIntersectContext(&context);

#pragma omp parallel for schedule(dynamic)
    for (int y = 0; y < height; ++y) {
        for (int x = 0; x < width; ++x) {
            Eigen::Array3f color(0.0f, 0.0f, 0.0f);
            auto u = static_cast<float>(x) / width;
            auto v = static_cast<float>(y) / height;
            auto rayhit = camera.gen_ray(u, v);
            rtcIntersect1(_scene, &context, &rayhit);

            if (rayhit.hit.geomID != RTC_INVALID_GEOMETRY_ID) {
                Eigen::Array3f ambient = _material.ambient;
                Eigen::Array3f diffuse = diffuse_color(rayhit.hit);
                if (_lighting) {
                    Eigen::Vector3f normal = Eigen::Vector3f(rayhit.hit.Ng_x,
                                                             rayhit.hit.Ng_y,
                                                             rayhit.hit.Ng_z)
                                                 .normalized();
                    // // Point light at the view position
                    Eigen::Vector3f lightdir = Eigen::Vector3f(rayhit.ray.dir_x,
                                                               rayhit.ray.dir_y,
                                                               rayhit.ray.dir_z)
                                                   .normalized();
                    diffuse *= std::abs(normal.dot(-lightdir));
                }
                color += ambient + diffuse;
            }
            else {
                color += _background;
            }
            color(0) = std::min(color(0), 1.0f);
            color(1) = std::min(color(1), 1.0f);
            color(2) = std::min(color(2), 1.0f);
            color *= 255;
            auto r = static_cast<uint8_t>(color(0));
            auto g = static_cast<uint8_t>(color(1));
            auto b = static_cast<uint8_t>(color(2));
            if (interleaved) {
                pixels[3 * ((height - y - 1) * width + x) + 0] = r;
                pixels[3 * ((height - y - 1) * width + x) + 1] = g;
                pixels[3 * ((height - y - 1) * width + x) + 2] = b;
            }
            else {
                pixels[(height - y - 1) * width + x] = r;
                pixels[width * height + (height - y - 1) * width + x] = g;
                pixels[2 * width * height + (height - y - 1) * width + x] = b;
            }
        }
    }
}

inline void RayTracer::render_shaded(std::vector<uint8_t>& pixels,
                                     const RayCamera& camera,
                                     int width,
                                     int height,
                                     int samples,
                                     bool interleaved)
{
    pixels.resize(3 * width * height);
    auto diffuse_color = _select_diffuse_color();

    RTCIntersectContext context;
    rtcInitIntersectContext(&context);
    std::random_device rd;
    std::minstd_rand rd_gen(rd());
    std::uniform_real_distribution<> rd_number(0.0f, 1.0f);
    const float rcpr_samples = 1.0f / samples;

#pragma omp parallel for schedule(dynamic)
    for (int y = 0; y < height; ++y) {
        for (int x = 0; x < width; ++x) {
            Eigen::Array3f color(0.0f, 0.0f, 0.0f);
            for (int s = 0; s < samples; ++s) {
                auto u = static_cast<float>(x + rd_number(rd_gen)) / width;
                auto v = static_cast<float>(y + rd_number(rd_gen)) / height;
                auto rayhit = camera.gen_ray(u, v);
                rtcIntersect1(_scene, &context, &rayhit);

                if (rayhit.hit.geomID != RTC_INVALID_GEOMETRY_ID) {
                    Eigen::Array3f ambient = _material.ambient;
                    Eigen::Array3f diffuse = diffuse_color(rayhit.hit);
                    if (_lighting) {
                        Eigen::Vector3f normal =
                            Eigen::Vector3f(rayhit.hit.Ng_x,
                                            rayhit.hit.Ng_y,
                                            rayhit.hit.Ng_z)
                                .normalized();
                        // // Point light at the view position
                        Eigen::Vector3f lightdir =
                            Eigen::Vector3f(rayhit.ray.dir_x,
                                            rayhit.ray.dir_y,
                                            rayhit.ray.dir_z)
                                .normalized();
                        diffuse *= std::abs(normal.dot(-lightdir));
                    }
                    color += ambient + diffuse;
                }
                else {
                    color += _background;
                }
            }
            color *= rcpr_samples;
            color(0) = std::min(color(0), 1.0f);
            color(1) = std::min(color(1), 1.0f);
            color(2) = std::min(color(2), 1.0f);
            color *= 255;
            auto r = static_cast<uint8_t>(color(0));
            auto g = static_cast<uint8_t>(color(1));
            auto b = static_cast<uint8_t>(color(2));
            if (interleaved) {
                pixels[3 * ((height - y - 1) * width + x) + 0] = r;
                pixels[3 * ((height - y - 1) * width + x) + 1] = g;
                pixels[3 * ((height - y - 1) * width + x) + 2] = b;
            }
            else {
                pixels[(height - y - 1) * width + x] = r;
                pixels[width * height + (height - y - 1) * width + x] = g;
                pixels[2 * width * height + (height - y - 1) * width + x] = b;
            }
        }
    }
}

inline void RayTracer::render_depth(std::vector<uint8_t>& pixels,
                                    const RayCamera& camera,
                                    int width,
                                    int height)
{
    pixels.resize(width * height);
    std::vector<float> depths(width * height, -1.0f);
    float min_depth = std::numeric_limits<float>::max();
    float max_depth = -1.0f;
    RTCIntersectContext context;
    rtcInitIntersectContext(&context);
#pragma omp parallel for schedule(dynamic)
    for (int y = 0; y < height; ++y) {
        for (int x = 0; x < width; ++x) {
            auto u = static_cast<float>(x) / width;
            auto v = static_cast<float>(y) / height;
            auto rayhit = camera.gen_ray(u, v);
            rtcIntersect1(_scene, &context, &rayhit);
            if (rayhit.hit.geomID != RTC_INVALID_GEOMETRY_ID) {
                auto depth = rayhit.ray.tfar * camera.dir.norm();
                depths[(height - y - 1) * width + x] = depth;
                if (depth < min_depth) min_depth = depth;
                if (depth > max_depth) max_depth = depth;
            }
        }
    }
    float denom = 1.0f / (max_depth - min_depth);
    for (size_t i = 0; i < depths.size(); ++i) {
        if (depths[i] == -1.0f) { pixels[i] = 0; }
        else {
            pixels[i] =
                static_cast<uint8_t>((max_depth - depths[i]) * denom * 255);
        }
    }
}

inline void RayTracer::render_depth(std::vector<float>& values,
                                    const RayCamera& camera,
                                    int width,
                                    int height)
{
    values.resize(width * height);
    RTCIntersectContext context;
    rtcInitIntersectContext(&context);
#pragma omp parallel for schedule(dynamic)
    for (int y = 0; y < height; ++y) {
        for (int x = 0; x < width; ++x) {
            auto u = static_cast<float>(x) / width;
            auto v = static_cast<float>(y) / height;
            auto rayhit = camera.gen_ray(u, v);
            rtcIntersect1(_scene, &context, &rayhit);
            if (rayhit.hit.geomID != RTC_INVALID_GEOMETRY_ID) {
                auto depth = rayhit.ray.tfar * camera.dir.norm();
                values[(height - y - 1) * width + x] = depth;
            }
            else {
                values[(height - y - 1) * width + x] = -1.0f;
            }
        }
    }
}

inline void RayTracer::render_silhouette(std::vector<uint8_t>& pixels,
                                         const RayCamera& camera,
                                         int width,
                                         int height)
{
    pixels.resize(width * height);
    RTCIntersectContext context;
    rtcInitIntersectContext(&context);
#pragma omp parallel for schedule(dynamic)
    for (int y = 0; y < height; ++y) {
        for (int x = 0; x < width; ++x) {
            auto u = static_cast<float>(x) / width;
            auto v = static_cast<float>(y) / height;
            auto rayhit = camera.gen_ray(u, v);
            rtcOccluded1(_scene, &context, &(rayhit.ray));
            if (rayhit.ray.tfar <= 0.0f) {
                pixels[(height - y - 1) * width + x] = 255;
            }
            else {
                pixels[(height - y - 1) * width + x] = 0;
            }
        }
    }
}

inline void RayTracer::render_index(std::vector<uint8_t>& pixels,
                                    const RayCamera& camera,
                                    int width,
                                    int height,
                                    bool interleaved)
{
    pixels.resize(3 * width * height);
    RTCIntersectContext context;
    rtcInitIntersectContext(&context);
#pragma omp parallel for schedule(dynamic)
    for (int y = 0; y < height; ++y) {
        for (int x = 0; x < width; ++x) {
            auto u = static_cast<float>(x) / width;
            auto v = static_cast<float>(y) / height;
            auto rayhit = camera.gen_ray(u, v);
            rtcIntersect1(_scene, &context, &rayhit);
            uint32_t index;
            if (rayhit.hit.geomID != RTC_INVALID_GEOMETRY_ID) {
                index = rayhit.hit.primID + 1;
            }
            else {
                index = 0;
            }
            uint8_t r = index & 0x000000FF;
            uint8_t g = (index >> 8) & 0x000000FF;
            uint8_t b = (index >> 16) & 0x000000FF;
            if (interleaved) {
                pixels[3 * ((height - y - 1) * width + x) + 0] = r;
                pixels[3 * ((height - y - 1) * width + x) + 1] = g;
                pixels[3 * ((height - y - 1) * width + x) + 2] = b;
            }
            else {
                pixels[(height - y - 1) * width + x] = r;
                pixels[width * height + (height - y - 1) * width + x] = g;
                pixels[2 * width * height + (height - y - 1) * width + x] = b;
            }
        }
    }
}

inline void RayTracer::render_index(std::vector<uint32_t>& indices,
                                    const RayCamera& camera,
                                    int width,
                                    int height)
{
    indices.resize(width * height);
    RTCIntersectContext context;
    rtcInitIntersectContext(&context);
#pragma omp parallel for schedule(dynamic)
    for (int y = 0; y < height; ++y) {
        for (int x = 0; x < width; ++x) {
            auto u = static_cast<float>(x) / width;
            auto v = static_cast<float>(y) / height;
            auto rayhit = camera.gen_ray(u, v);
            rtcIntersect1(_scene, &context, &rayhit);
            uint32_t index;
            if (rayhit.hit.geomID != RTC_INVALID_GEOMETRY_ID) {
                index = rayhit.hit.primID + 1;
            }
            else {
                index = 0;
            }
            indices[(height - y - 1) * width + x] = index;
        }
    }
}

inline std::function<Eigen::Array3f(const RTCHit&)>
RayTracer::_select_diffuse_color()
{
    if (_colors && _vertex_color) {
        return std::bind(
            &RayTracer::_diffuse_vertex_color, this, std::placeholders::_1);
    }
    else if (_colors && !_vertex_color) {
        return std::bind(
            &RayTracer::_diffuse_face_color, this, std::placeholders::_1);
    }
    else {
        return std::bind(
            &RayTracer::_diffuse_material, this, std::placeholders::_1);
    }
}

inline Eigen::Array3f RayTracer::_diffuse_material(const RTCHit& hit)
{
    (void)hit;
    return _material.diffuse;
}

inline Eigen::Array3f RayTracer::_diffuse_face_color(const RTCHit& hit)
{
    Eigen::Array3f diffuse;
    diffuse << (*_colors)[3 * hit.primID], (*_colors)[3 * hit.primID + 1],
        (*_colors)[3 * hit.primID + 2];
    return diffuse;
}

inline Eigen::Array3f RayTracer::_diffuse_vertex_color(const RTCHit& hit)
{
    float buffer[4];
    RTCInterpolateArguments args;
    args.geometry = _geometry;
    args.primID = hit.primID;
    args.u = hit.u;
    args.v = hit.v;
    args.bufferType = RTC_BUFFER_TYPE_VERTEX_ATTRIBUTE;
    args.bufferSlot = 0;
    args.valueCount = 3;
    args.P = buffer;
    args.dPdu = nullptr;
    args.dPdv = nullptr;
    args.ddPdudu = nullptr;
    args.ddPdvdv = nullptr;
    args.ddPdudv = nullptr;
    rtcInterpolate(&args);

    Eigen::Array3f diffuse;
    diffuse << buffer[0], buffer[1], buffer[2];
    return diffuse;
}

} // namespace Euclid


//==================================================================================================
/*!

  Copyright 2016 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)
*/
//==================================================================================================
#include <boost/simd/constant/twenty.hpp>
#include <boost/simd/as.hpp>
#include <stf.hpp>

STF_CASE_TPL( "Check twenty behavior for all types"
            , (std::uint8_t)(std::uint16_t)(std::uint32_t)(std::uint64_t)
              (std::int8_t)(std::int16_t)(std::int32_t)(std::int64_t)
              (double)(float)
            )
{
  using boost::simd::as;
  using boost::simd::detail::twenty;
  using boost::simd::Twenty;

  STF_TYPE_IS(decltype(Twenty<T>()), T);
  STF_EQUAL(Twenty<T>(), T(20));
  STF_EQUAL(twenty( as(T{}) ),T(20));
}


// Copyright  (C)  2007  Ruben Smits <ruben dot smits at mech dot kuleuven dot be>

// Version: 1.0
// Author: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
// Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
// URL: http://www.orocos.org/kdl

// This library is free software; you can redistribute it and/or
// modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation; either
// version 2.1 of the License, or (at your option) any later version.

// This library is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
// Lesser General Public License for more details.

// You should have received a copy of the GNU Lesser General Public
// License along with this library; if not, write to the Free Software
// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA

#ifndef KDL_JACOBIAN_HPP
#define KDL_JACOBIAN_HPP

#include "frames.hpp"
#include <Eigen/Core>

namespace KDL
{
    // Equal is friend function, but default arguments for friends are forbidden (§8.3.6.4)
    class Jacobian;
    bool Equal( const Jacobian& a, const Jacobian& b, double eps = epsilon );
    void SetToZero( Jacobian& jac );

    class Jacobian
    {
    public:
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
        Eigen::Matrix< double, 6, Eigen::Dynamic > data;
        Jacobian( );
        explicit Jacobian( unsigned int nr_of_columns );
        Jacobian( const Jacobian& arg );

        ///Allocates memory for new size (can break realtime behavior)
        void resize( unsigned int newNrOfColumns );

        ///Allocates memory if size of this and argument is different
        Jacobian& operator=( const Jacobian& arg );

        bool operator==( const Jacobian& arg ) const;
        bool operator!=( const Jacobian& arg ) const;

        friend bool Equal( const Jacobian& a, const Jacobian& b, double eps );

        ~Jacobian( );

        double operator( )( unsigned int i, unsigned int j ) const;
        double& operator( )( unsigned int i, unsigned int j );
        unsigned int rows( ) const;
        unsigned int columns( ) const;

        friend void SetToZero( Jacobian& jac );

        friend bool changeRefPoint( const Jacobian& src1, const Vector& base_AB, Jacobian& dest );
        friend bool changeBase( const Jacobian& src1, const Rotation& rot, Jacobian& dest );
        friend bool changeRefFrame( const Jacobian& src1, const Frame& frame, Jacobian& dest );

        Twist getColumn( unsigned int i ) const;
        void setColumn( unsigned int i, const Twist& t );

        void changeRefPoint( const Vector& base_AB );
        void changeBase( const Rotation& rot );
        void changeRefFrame( const Frame& frame );
    };

    bool changeRefPoint( const Jacobian& src1, const Vector& base_AB, Jacobian& dest );
    bool changeBase( const Jacobian& src1, const Rotation& rot, Jacobian& dest );
    bool changeRefFrame( const Jacobian& src1, const Frame& frame, Jacobian& dest );

}  // namespace KDL

#endif


#pragma once
//===----------------------------------------------------------------------===//
#include <dtl/dtl.hpp>
#include <dtl/math.hpp>

#include <boost/dynamic_bitset.hpp>

#include <cstddef>
#include <string>
#include <vector>
//===----------------------------------------------------------------------===//
namespace dtl {
//===----------------------------------------------------------------------===//
/// Position list.
template<typename _position_t = $u32>
struct position_list {
  using position_t = _position_t;

  /// The ordered list of positions.
  std::vector<position_t> positions_;

  /// The universe [0, n_).
  $u64 n_;

  // TODO make private
  position_list() = default;

  explicit position_list(const boost::dynamic_bitset<$u32>& in)
      : positions_(), n_(in.size()) {
    std::size_t current_pos = in.find_first();
    while (current_pos < in.size()) {
      positions_.push_back(static_cast<position_t>(current_pos));
      current_pos = in.find_next(current_pos);
    }
  }

  ~position_list() = default;
  position_list(const position_list& other) = default;
  position_list(position_list&& other) noexcept = default;

  __forceinline__ position_list&
  operator=(const position_list& other) = default;

  __forceinline__ position_list&
  operator=(position_list&& other) noexcept = default;

  /// Return the size in bytes.
  std::size_t
  size_in_bytes() const {
    return positions_.size() * sizeof(position_t) /* positions */
        + sizeof(position_t) /* number of positions */
        + sizeof(n_) /* bit-length of the original bitmap */;
  }

  /// Return the length of the bitmap.
  std::size_t
  size() const {
    return n_;
  }

  /// Conversion to an boost::dynamic_bitset.
  boost::dynamic_bitset<$u32>
  to_bitset() {
    boost::dynamic_bitset<$u32> ret(n_);
    for (position_t p : positions_) {
      ret[p] = true;
    }
    return ret;
  }

  /// Bitwise AND
  position_list __forceinline__
  operator&(const position_list& other) const {
    position_list ret;
    ret.n_ = n_;
    auto this_it = positions_.begin();
    const auto this_it_end = positions_.end();
    auto other_it = other.positions_.begin();
    const auto other_it_end = other.positions_.end();
    while (!(this_it == this_it_end || other_it == other_it_end)) {
      if (*this_it == *other_it) {
        ret.positions_.push_back(*this_it);
        ++this_it;
        ++other_it;
      }
      else {
        if (*this_it < *other_it) {
          ++this_it;
        }
        else {
          ++other_it;
        }
      }
    }
    return ret;
  }

  /// Bitwise AND (range encoding)
  position_list __forceinline__
  and_re(const position_list& other) const {
    return *this & other; // nothing special here. fall back to AND
  }

  /// Bitwise XOR
  position_list __forceinline__
  operator^(const position_list& other) const {
    position_list ret;
    ret.n_ = n_;

    auto this_it = positions_.begin();
    const auto this_it_end = positions_.end();
    auto other_it = other.positions_.begin();
    const auto other_it_end = other.positions_.end();
    while (!(this_it == this_it_end || other_it == other_it_end)) {
      if (*this_it < *other_it) {
        ret.positions_.push_back(*this_it);
        ++this_it;
      }
      else if (*other_it < *this_it) {
        ret.positions_.push_back(*other_it);
        ++other_it;
      }
      else {
        ++this_it;
        ++other_it;
      }
    }
    if (this_it != this_it_end) {
      while (this_it != this_it_end) {
        ret.positions_.push_back(*this_it);
        ++this_it;
      }
    }
    if (other_it != other_it_end) {
      while (other_it != other_it_end) {
        ret.positions_.push_back(*other_it);
        ++other_it;
      }
    }
    return ret;
  }

  /// Bitwise XOR (range encoding)
  position_list __forceinline__
  xor_re(const position_list& other) const {
    return *this ^ other; // nothing special here. fall back to XOR
  }

  /// Computes (a XOR b) & this
  /// Note: this, a and b must be different instances. Otherwise, the behavior
  /// is undefined.
  __forceinline__ position_list&
  fused_xor_and(const position_list& a, const position_list& b) {
    const auto x = a ^ b;
    auto y = *this & x;
    std::swap(positions_, y.positions_);
    return *this;
  }

  static std::string
  name() {
    return "position_list_"
        + std::to_string(sizeof(position_t) * 8);
    ;
  }

  /// Returns the value of the bit at the position pos.
  u1
  test(const std::size_t pos) const {
    auto search = std::lower_bound(positions_.begin(), positions_.end(), pos);
    if (search == positions_.end()) {
      return false;
    }
    return *search == pos;
  }

  /// Set the i-th bit to the given value.
  void __forceinline__
  set(std::size_t i, u1 val) noexcept {
    auto search = std::lower_bound(positions_.begin(), positions_.end(), i);
    if (search == positions_.end()) {
      if (val) {
        // Append the value to the list.
        positions_.push_back(i);
      }
      return;
    }

    auto found_pos = *search;
    if (found_pos == i) {
      if (!val) {
        // Remove the value from the list.
        positions_.erase(search);
      }
      return;
    }
    else {
      if (val) {
        positions_.insert(search, i);
      }
    }
  }

  //===--------------------------------------------------------------------===//
  /// Iterator, with skip support.
  class iter {
    const position_list& outer_;

    //===------------------------------------------------------------------===//
    // Iterator state
    //===------------------------------------------------------------------===//
    /// Read position within the list.
    $u64 read_pos_;
    /// Points to the beginning of the current range.
    $u64 range_begin_;
    /// The length of the current range.
    $u64 range_length_;
    //===------------------------------------------------------------------===//

  public:
    explicit __forceinline__
    iter(const position_list& outer)
        : outer_(outer),
          read_pos_(outer.positions_.empty() ? outer_.positions_.size() : 0),
          range_begin_(read_pos_ < outer.positions_.size()
                  ? outer_.positions_[0]
                  : outer_.n_),
          range_length_(read_pos_ < outer.positions_.size() ? 1 : 0) {
      // Determine the length of the current range.
      if (read_pos_ == 0) {
        ++read_pos_;
      }
      while (read_pos_ < outer_.positions_.size()
          && outer_.positions_[read_pos_] == range_begin_ + range_length_) {
        ++read_pos_;
        ++range_length_;
      }
    }

    void __forceinline__
    next() {
      if (read_pos_ < outer_.positions_.size()) {
        range_begin_ = outer_.positions_[read_pos_];
        range_length_ = 1;
        ++read_pos_;
        while (read_pos_ < outer_.positions_.size()
            && outer_.positions_[read_pos_] == range_begin_ + range_length_) {
          ++read_pos_;
          ++range_length_;
        }
      }
      else {
        range_begin_ = outer_.n_;
        range_length_ = 0;
      }
    }

    void __forceinline__
    skip_to(const std::size_t to_pos) {
      if (to_pos < (range_begin_ + range_length_)) {
        range_length_ -= to_pos - range_begin_;
        range_begin_ = to_pos;
        return;
      }
      auto search = std::lower_bound(
          outer_.positions_.begin(), outer_.positions_.end(), to_pos);
      if (search != outer_.positions_.end()) {
        range_begin_ = *search;
        read_pos_ = std::distance(outer_.positions_.begin(), search) + 1ull;
        range_length_ = 1;
        while (read_pos_ < outer_.positions_.size()
            && outer_.positions_[read_pos_] == range_begin_ + range_length_) {
          ++read_pos_;
          ++range_length_;
        }
      }
      else {
        range_begin_ = outer_.n_;
        range_length_ = 0;
      }
    }

    u1 __forceinline__
    end() const noexcept {
      return range_length_ == 0;
    }

    u64 __forceinline__
    pos() const noexcept {
      return range_begin_;
    }

    u64 __forceinline__
    length() const noexcept {
      return range_length_;
    }
  };
  //===--------------------------------------------------------------------===//

  using skip_iter_type = iter;
  using scan_iter_type = iter;

  skip_iter_type __forceinline__
  it() const {
    return skip_iter_type(*this);
  }

  scan_iter_type __forceinline__
  scan_it() const {
    return scan_iter_type(*this);
  }

  /// Returns the name of the instance including the most important parameters
  /// in JSON.
  std::string
  info() const {
    return "{\"name\":\"" + name() + "\""
        + ",\"n\":" + std::to_string(n_)
        + ",\"size\":" + std::to_string(size_in_bytes())
        + ",\"positions\":" + std::to_string(positions_.size())
        + "}";
  }

  // For debugging purposes.
  void
  print(std::ostream& os) const {
    os << "[";
    if (!positions_.empty()) {
      os << positions_[0];
      for (std::size_t i = 1; i < positions_.size(); ++i) {
        os << "," << positions_[i];
      }
    }
    os << "]";
  }
};
//===----------------------------------------------------------------------===//
} // namespace dtl


#include <fstream>
#include <iostream>
#include <string>
#include <Eigen/Core>
#include <mav_msgs/conversions.h>
#include <mav_msgs/default_topics.h>
#include <ros/ros.h>
#include <trajectory_msgs/MultiDOFJointTrajectory.h>
#include <nav_msgs/Odometry.h>
#include <sensor_msgs/LaserScan.h>

// Globals
nav_msgs::Odometry current_odom;
sensor_msgs::LaserScan current_scan;
trajectory_msgs::MultiDOFJointTrajectory trajectory_msg;
const float DEG_2_RAD = M_PI / 180.0;
const int DEBUG = 0;
int new_room = 1;

void odom_callback(const nav_msgs::Odometry::ConstPtr& msg)
{
	current_odom.header.seq = msg->header.seq;
	current_odom.header.frame_id = "world";
	current_odom.child_frame_id = "firefly/base_link";
	current_odom.pose.pose.position.x = msg->pose.pose.position.x;
	current_odom.pose.pose.position.y = msg->pose.pose.position.y;
	current_odom.pose.pose.position.z = msg->pose.pose.position.z;
	current_odom.pose.pose.orientation.x = msg->pose.pose.orientation.x;
	current_odom.pose.pose.orientation.y = msg->pose.pose.orientation.y;
	current_odom.pose.pose.orientation.z = msg->pose.pose.orientation.z;
	current_odom.pose.pose.orientation.w = msg->pose.pose.orientation.w;
	current_odom.twist.twist.linear.x = msg->twist.twist.linear.x;
	current_odom.twist.twist.linear.y = msg->twist.twist.linear.y;
	current_odom.twist.twist.linear.z = msg->twist.twist.linear.z;
	current_odom.twist.twist.angular.x = msg->twist.twist.angular.x;
	current_odom.twist.twist.angular.y = msg->twist.twist.angular.y;
	current_odom.twist.twist.angular.z = msg->twist.twist.angular.z;

	if(DEBUG)
	{
		ROS_INFO("Seq: [%d]", current_odom.header.seq);
		ROS_INFO("Position-> x: [%f], y: [%f], z: [%f]", current_odom.pose.pose.position.x,current_odom.pose.pose.position.y, current_odom.pose.pose.position.z);
		ROS_INFO("Orientation-> x: [%f], y: [%f], z: [%f], w: [%f]", current_odom.pose.pose.orientation.x, current_odom.pose.pose.orientation.y, current_odom.pose.pose.orientation.z, current_odom.pose.pose.orientation.w);
		ROS_INFO("Vel-> Linear: [%f], Angular: [%f]", current_odom.twist.twist.linear.x,current_odom.twist.twist.angular.z);
	}

}

void laser_callback(const sensor_msgs::LaserScan::ConstPtr& msg)
{

	double min_angle = msg->angle_min;
	double max_angle = msg->angle_max;
	double increment = msg->angle_increment;
	int size = msg->ranges.size();
	int start = size/2 + min_angle/increment;
	int end = size/2 + max_angle/increment;

	current_scan.scan_time = msg->scan_time;
	current_scan.angle_increment = increment;
	current_scan.time_increment = msg->time_increment;
	current_scan.range_min = msg->range_min;
	current_scan.range_max = msg->range_max;
	current_scan.ranges.resize(end-start);
	int count = 0;

	for (int i = start; i <= end; i++)
	{
		current_scan.ranges[count] = msg->ranges[i];
		count++;
		if(DEBUG)
		{
			ROS_INFO("Ranges: [%f]", current_scan.ranges[i]);
		}
	}
}

void waypoint_calculator(void)
{
	/* Find the distances between */

	// Publish message here
}

int main(int argc, char** argv)
{
	// Init Sequence
	ros::init(argc, argv, "waypoint_generator");
	ros::NodeHandle nh;
	ros::Rate loop_rate(200);

	// Subscriber(s)
	ros::Subscriber odom_sub = nh.subscribe("/firefly/odom_fused", 1000, odom_callback);
	ros::Subscriber laser_sub = nh.subscribe("/firefly/scan", 1000, laser_callback);

	// Publisher(s)
	ros::Publisher trajectory_pub = nh.advertise<trajectory_msgs::MultiDOFJointTrajectory>("/firefly/test_trajectory", 10);// mav_msgs::default_topics::COMMAND_TRAJECTORY, 10);

	ROS_INFO("Started waypoint_generator.");

	while(ros::ok())
	{

		// TODO:Solve Core Dumped

		/*
		// If we are in a new room, publish the new waypoints
		if(new_room)
		{

			ros::Duration(2.0).sleep();

			double front, right, back, left;

			ROS_INFO("Calculating waypoints");
			int count = 0;
			for (int i = 0; i <= 359; i++)
			{
				ROS_INFO("Ranges: [%f], Count [%d]", current_scan.ranges[i], count);
				count++;
			}
			ROS_INFO("Exiting Loop");

			for (int i = 0; i < 45; i++)
			{
				front += current_scan.ranges[i]/45.0;
			}

			for (int i = 45; i < 135; i++)
			{
				right += current_scan.ranges[i]/90.0;
			}

			for (int i = 135; i < 225; i++)
			{
				back += current_scan.ranges[i]/90.0;
			}

			for (int i = 225; i < 315; i++)
			{
				left += current_scan.ranges[i]/90.0;
			}

			for (int i = 315; i < 360; i++)
			{
				front += current_scan.ranges[i]/45.0;
			}

			ROS_INFO("Waypoint calculated");
			// Calculate waypoints
			// TODO: Put waypoint Generation algorithm here!
			Eigen::Vector3d desired_position(((front+back)/2.0), ((left+right)/2.0), 1.0);
			double desired_yaw = 0.0 * DEG_2_RAD;

			mav_msgs::msgMultiDofJointTrajectoryFromPositionYaw(desired_position, desired_yaw, &trajectory_msg);

			ROS_INFO("Publishing waypoint on namespace %s: [%f, %f, %f].", nh.getNamespace().c_str(), desired_position.x(), desired_position.y(), desired_position.z());
			trajectory_pub.publish(trajectory_msg);
			// Drop the flag
			new_room = 0;
		}
		*/

		ros::spinOnce();
		loop_rate.sleep();
	}
}


//==================================================================================================
/**
  Copyright 2016 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_AVX_SIMD_FUNCTION_HMSB_HPP_INCLUDED
#define BOOST_SIMD_ARCH_X86_AVX_SIMD_FUNCTION_HMSB_HPP_INCLUDED

#include <boost/simd/detail/overload.hpp>
#include <boost/simd/function/slice.hpp>
#include <boost/simd/function/bitwise_cast.hpp>
#include <boost/simd/detail/dispatch/meta/as_floating.hpp>
#include <bitset>

namespace boost { namespace simd { namespace ext
{
  namespace bd = boost::dispatch;
  namespace bs = boost::simd;

  BOOST_DISPATCH_OVERLOAD ( hmsb_
                          , (typename A0)
                          , bs::avx_
                          , bs::pack_<bd::type32_<A0>, bs::avx_>
                          )
  {
    BOOST_FORCEINLINE std::bitset<8> operator()(A0 const& a0) const
    {
      return _mm256_movemask_ps(bitwise_cast<bd::as_floating_t<A0>>(a0));
    }
  };

  BOOST_DISPATCH_OVERLOAD ( hmsb_
                          , (typename A0)
                          , bs::avx_
                          , bs::pack_<bd::type64_<A0>, bs::avx_>
                          )
  {
    BOOST_FORCEINLINE std::bitset<4> operator()(A0 const& a0) const
    {
      return _mm256_movemask_pd(bitwise_cast<bd::as_floating_t<A0>>(a0));
    }
  };

  BOOST_DISPATCH_OVERLOAD ( hmsb_
                          , (typename A0)
                          , bs::avx_
                          , bs::pack_<bd::ints8_<A0>, bs::avx_>
                          )
  {
    BOOST_FORCEINLINE std::bitset<32> operator()(A0 const& a0) const
    {
      auto h = _mm_movemask_epi8(slice_high(a0));
      auto l = _mm_movemask_epi8(slice_low (a0));
      return l | (h << 16);
    }
  };

  BOOST_DISPATCH_OVERLOAD( hmsb_
                      , (typename A0)
                      , bs::avx_
                      , bs::pack_<bd::ints16_<A0>, bs::avx_>
                      )
  {
    BOOST_FORCEINLINE std::bitset<16> operator()(A0 const& a0) const
    {
      using s8type = typename A0::template retype<int8_t, 16>;
      s8type mask = {1,3,5,7,9,11,13,15,-128,-128,-128,-128,-128,-128,-128,-128};

      auto l = _mm_movemask_epi8(_mm_shuffle_epi8(bitwise_cast<s8type>(slice_low(a0)), mask));
      auto h = _mm_movemask_epi8(_mm_shuffle_epi8(bitwise_cast<s8type>(slice_high(a0)), mask));

      return l | (h << 8);
    }
  };
} } }

#endif


//
// Copyright (c) 2018 James A. Chappell
//
// METAR decoder example
//

#include <iostream>
#include <iomanip>
#include <string>
#include <vector>

#include <cmath>
#include <getopt.h>

#include <boost/algorithm/string.hpp>

#include "Fetch.h"

#include "Metar.h"
#include "Convert.h"
#include "Utils.h"

#include "Phenom2String.h"

using namespace std;
using namespace Storage_B::Weather;
using namespace Storage_B::Curlpp;

static const string URL =
  "https://tgftp.nws.noaa.gov/data/observations/metar/stations/";

static void usage(const string& command)
{
  cerr << "usage: " << command << " [options..] <stationString>\n";
  cerr << " -f, --fahrenheit Print temperature in Fahrenheit\n";
}

static const char *DEG_SIM = "\u00B0";

static const char *speed_units[]
{
  "KT",
  "MPS",
  "KPH"
};

static const char *sky_conditions[] =
{
  "SKC",
  "CLR",
  "NSC",
  "FEW",
  "SCT",
  "BKN",
  "OVC"
};

static const char *cloud_types[] =
{
  "TCU",
  "CB",
  "ACC"
};

static optional<double> Temp(const optional<double>& td,
                             const optional<int>& ti)
{
  if (td.has_value())
    return *td;
  else if (ti.has_value())
    return static_cast<double>(*ti);
  else
    return {};
}

static void print_temp(const optional<double>& temp, bool fahrenheit_flag)
{
  if (temp.has_value())
  {
    cout << (fahrenheit_flag ? Convert::c2f(*temp) : *temp);
  }
  else
  {
    cout << "???";
  }
  cout << DEG_SIM << (fahrenheit_flag ? 'F' : 'C');
}

int main(int argc, char **argv)
{
  static struct option long_options[] =
  {
    {"help", no_argument, NULL, 'h'},
    {"fahrenheit", no_argument, NULL, 'f'},
    {0, 0, 0, 0}
  };

  bool fahrenheit_flag(false);
  int option_index(0);
  int c;
  bool dflag(false);

  string metar_str;

  string command = basename(argv[0]);

  opterr = 0;
  while((c = getopt_long(argc, argv, "hfd:", long_options,
             &option_index)) != -1)
  {
    switch(c)
    {
      case 'h':
        usage(command);
        return 1;

      case 'f':
        fahrenheit_flag = true;
        break;

      case 'd':
        dflag = true;
        metar_str = optarg;
        break;

      default:
        usage(command);
        return 1;
    }
  }

  if (argv[optind] && !dflag)
  {
    string url(URL + argv[optind] + ".TXT");

    Fetch fetch(url.c_str());
    string data;

    auto result = fetch(data);

    if (!Curl::httpStatusOK(result))
    {
      cerr << "http_status = " << result << endl;
      return 1;
    }

    //
    // This METAR source returns 2 lines of text.
    //   * The first line is the observation time and date (UTC)
    //   * The second line is the METAR string
    //
    vector<string> list;
    boost::split(list, data, boost::is_any_of("\n"));

    metar_str = list[1];
  }

  if (!metar_str.empty())
  {
    cout << metar_str << endl;

    auto metar = Metar::Create(metar_str.c_str());

    auto temp =
        Temp(metar->TemperatureNA(), metar->Temperature());
    auto dew =
        Temp(metar->DewPointNA(), metar->DewPoint());

    cout << metar->ICAO().value() << endl;
    cout << setprecision(1) << fixed;

    cout <<   "Temperature: ";
    print_temp(temp, fahrenheit_flag);

    optional<double> feels_like;
    if (metar->WindSpeed().has_value() && temp.has_value())
    {
      double wind_kph;
      switch(metar->WindSpeedUnits().value())
      {
        case Metar::speed_units::KT:
          wind_kph = Convert::Kts2Kph(metar->WindSpeed().value());
          break;

        case Metar::speed_units::MPS:
          wind_kph = metar->WindSpeed().value() / 1000.0;
          break;

        default:
          wind_kph = metar->WindSpeed().value();
          break;
      }

      feels_like = Utils::WindChill(*temp, wind_kph);
      if (feels_like == temp) feels_like.reset();
    }

    optional<double> humidity;
    if (temp.has_value() && dew.has_value())
    {
      humidity = Utils::Humidity(*temp, *dew);
      if (!feels_like.has_value())
      {
        feels_like = Utils::HeatIndex(*temp, *humidity, true);
        if (feels_like == temp) feels_like.reset();
      }
    }

    if (feels_like.has_value())
    {
      cout <<   "\nFeels Like:  ";
      print_temp(feels_like, fahrenheit_flag);
    }

    cout << "\nDew Point:   ";
    print_temp(dew, fahrenheit_flag);

    if (humidity.has_value())
    {
      cout << "\nHumidity:    " << *humidity << "%";
    }

    cout << "\nPressure:    ";
    if (metar->AltimeterA().has_value())
      cout << metar->AltimeterA().value() << " inHg" << endl;
    else if (metar->AltimeterQ().has_value())
      cout << metar->AltimeterQ().value() << " hPa" << endl;

    if (metar->WindSpeed().has_value())
    {
      cout << "\nWind:        ";
      if (!metar->isVariableWindDirection())
      {
        cout << metar->WindDirection().value() << DEG_SIM;
      }
      else
      {
        cout << "VRB";
      }
      cout << " / " << metar->WindSpeed().value();
      if (metar->WindGust().has_value())
      {
        cout << " (" << metar->WindGust().value() << ")";
      }
      cout << " "
           << speed_units[
              static_cast<int>(metar->WindSpeedUnits().value())] << endl;
    }

    cout << setprecision(2) << fixed;
    if (metar->Visibility().has_value())
    {
      cout << "\nVisibility:  " << metar->Visibility().value() << " ";
      if (metar->VisibilityUnits() == Metar::distance_units::M)
      {
        cout << "meters" << endl;
      }
      else
      {
        cout << "miles" << endl;
      }
    }

    cout << endl;
    for (unsigned int i = 0 ; i < metar->NumCloudLayers() ; i++)
    {
      auto layer = metar->Layer(i);
      if (!layer->Temporary())
      {
        cout << sky_conditions[static_cast<int>(layer->Cover())];
        if (layer->Altitude().has_value())
        {
          cout << ": " << layer->Altitude().value() * 100  << " feet";
          if (layer->CloudType().has_value())
          {
            cout << " ("
                 << cloud_types[static_cast<int>(layer->CloudType().value())]
                 << ")";
          }
        }
        cout << endl;
      }
    }

    for (unsigned int i = 0 ; i < metar->NumPhenomena() ; i++)
    {
      const auto& p = metar->Phenomenon(i);
      cout << Phenom2String(p) << endl;
    }

    return 0;
  }

  usage(command);

  return 1;
}


/* MIT License

Copyright (c) 2020 FORNO

Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:

The above copyright notice and this permission notice shall be included in all
copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
SOFTWARE.
*/
#include <cmath>
#include <cstddef>
#include <filesystem>
#include <iostream>
#include <limits>
#include <utility>

#include <boost/optional.hpp>
#include <boost/program_options.hpp>
#include <boost/property_tree/ini_parser.hpp>
#include <boost/property_tree/ptree.hpp>
#include <boost/tokenizer.hpp>

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

constexpr std::size_t marker_count {29};

struct index_holder
{
  std::size_t left_asis;
  std::size_t right_asis;
  std::size_t v_sacral;
};

index_holder read_indexies(std::filesystem::path path);

int main(int argc, char** argv)
{
  std::ios::sync_with_stdio(false);
  std::cin.tie(nullptr);
  std::cout << std::fixed;

  namespace po = boost::program_options;

  po::options_description description("Options");
  description.add_options()
    ("header,h", po::value<bool>()->default_value(true), "Dose the CSV has header?")
    ("config,c", po::value<std::string>(), "Config file path")
    ("l2w,l", po::value<bool>()->default_value(false), "Local to World flag")
    ("help,H", "Help")
    ;

  po::variables_map vm;
  po::store(po::parse_command_line(argc, argv, description), vm);
  po::notify(vm);

  if (argc < 2 || vm.count("help")) {
    std::cerr << "Usage: " << *argv << " --config config/Cooking < cooking.csv\n";
    return 1;
  }

  try {
    const auto config_path = vm["config"].as<std::string>();
    const auto indexies {read_indexies(config_path)};
    if (vm["header"].as<bool>()) {
      std::cin.ignore(std::numeric_limits<std::streamsize>::max(), '\n');
    }
    if (!vm["l2w"].as<bool>()) {
      for (std::string line; std::getline(std::cin, line);) {
        boost::tokenizer<boost::escaped_list_separator<char>> tokens(line);
        Eigen::Matrix<float, marker_count, 3, Eigen::RowMajor> values;
        {
          auto it = tokens.begin();
          for (std::size_t i {0}; i < marker_count * 3; ++i) {
            try {
              values(i) = std::stof(*it);
            } catch (std::invalid_argument&) {
              values(i) = std::numeric_limits<float>::quiet_NaN();
            }
            ++it;
          }
        }

        const Eigen::Vector3f center_asis = values.row(indexies.left_asis) / 2 + values.row(indexies.right_asis) / 2;
        for (std::size_t i {0}; i < marker_count; ++i) {
          values.row(i) -= center_asis;
        }

        const Eigen::Vector3f forward = -values.row(indexies.v_sacral);
        const auto w2l_rotation = Eigen::Quaternionf::FromTwoVectors(forward, Eigen::Vector3f{0, 0, 1.f});

        for (std::size_t i {0}; i < marker_count; ++i) {
          values.row(i) = w2l_rotation * values.row(i);
        }

        auto is_first {true};
        for (std::size_t i {0}; i < static_cast<std::size_t>(values.size()); ++i) {
          if (!std::exchange(is_first, false)) {
            std::cout.put(',');
          }
          if (!isnan(values(i))) {
            std::cout << values(i);
          }
        }
        const auto l2w_translation {-center_asis};
        for (std::size_t i {0}; i < static_cast<std::size_t>(l2w_translation.size()); ++i) {
          std::cout << ',' << l2w_translation(i);
        }
        const auto l2w_rotation {w2l_rotation.inverse()};
        const auto& l2w_coeffs {l2w_rotation.coeffs()};
        for (std::size_t i {0}; i < static_cast<std::size_t>(l2w_coeffs.size()); ++i) {
          std::cout << ',' << l2w_coeffs(i);
        }
        std::cout.put('\n');
      }
    } else {
      for (std::string line; std::getline(std::cin, line);) {
        Eigen::Matrix<float, marker_count, 3, Eigen::RowMajor> values;
        Eigen::Vector3f translation;
        Eigen::Quaternionf rotation;
        {
          boost::tokenizer<boost::escaped_list_separator<char>> tokens(line);
          auto it = tokens.begin();
          for (std::size_t i {0}; i < static_cast<std::size_t>(values.size()); ++i) {
            try {
              values(i) = std::stof(*it);
            } catch (std::invalid_argument&) {
              values(i) = std::numeric_limits<float>::quiet_NaN();
            }
            ++it;
          }
          for (std::size_t i {0}; i < static_cast<std::size_t>(translation.size()); ++i) {
            try {
              translation(i) = std::stof(*it);
            } catch (std::invalid_argument&) {
              translation(i) = std::numeric_limits<float>::quiet_NaN();
            }
            ++it;
          }
          Eigen::Vector4f rotation_coeffs ;
          for (std::size_t i {0}; i < static_cast<std::size_t>(rotation_coeffs.size()); ++i) {
            try {
              rotation_coeffs(i) = std::stof(*it);
            } catch (std::invalid_argument&) {
              rotation_coeffs(i) = std::numeric_limits<float>::quiet_NaN();
            }
            ++it;
          }
          rotation = Eigen::Quaternionf(rotation_coeffs);
        }
        for (std::size_t i {0}; i < marker_count; ++i) {
          values.row(i) = rotation * values.row(i) + translation;
        }
        auto is_first {true};
        for (std::size_t i {0}; i < static_cast<std::size_t>(values.size()); ++i) {
          if (!std::exchange(is_first, false)) {
            std::cout.put(',');
          }
          if (!isnan(values(i))) {
            std::cout << values(i);
          }
        }
        std::cout.put('\n');
      }
    }
  } catch (boost::wrapexcept<boost::property_tree::ptree_bad_path>& e) {
    std::cerr << "Invalid indexies format: " << e.what() << '\n';
    return 1;
  } catch (boost::wrapexcept<boost::property_tree::ini_parser::ini_parser_error>& e) {
    std::cerr << "Invalid indexies file: " << e.what() << '\n';
    return 1;
  }

  return 0;
}

index_holder read_indexies(std::filesystem::path path)
{
  using namespace boost::property_tree;
  ptree pt;
  read_ini(path.native(), pt);
  return index_holder {pt.get<std::size_t>("Indexies.LeftAsis"), pt.get<std::size_t>("Indexies.RightAsis"), pt.get<std::size_t>("Indexies.VSacral")};
}