//=============================================================================
//
//  CLASS NConstraintInterfaceADOLC
//
//=============================================================================

#ifndef COMISO_NCONSTRAINTINTERFACEADOLC_HH
#define COMISO_NCONSTRAINTINTERFACEADOLC_HH

//== COMPILE-TIME PACKAGE REQUIREMENTS ========================================
#include <CoMISo/Config/config.hh>
#if COMISO_ADOLC_AVAILABLE
#if COMISO_EIGEN3_AVAILABLE

//== INCLUDES =================================================================

#include <CoMISo/Config/CoMISoDefines.hh>
#include "SuperSparseMatrixT.hh"

#include <adolc/adolc.h>
#include <adolc/adouble.h>
#include <adolc/drivers/drivers.h>
#include <adolc/sparse/sparsedrivers.h>
#include <adolc/taping.h>

#include "NConstraintInterface.hh"

#include "TapeIDSingleton.hh"

#define EIGEN_YES_I_KNOW_SPARSE_MODULE_IS_NOT_STABLE_YET
#include <Eigen/Sparse>

//== FORWARDDECLARATIONS ======================================================

//== NAMESPACES ===============================================================

namespace COMISO {

//== CLASS DEFINITION =========================================================

/** \class NProblemInterfaceADOLC NProblemInterfaceADOLC.hpp

    The problem interface using automatic differentiation.
 */
class COMISODLLEXPORT NConstraintInterfaceADOLC : public NConstraintInterface {
public:

    // Define Sparse Datatypes
    typedef NConstraintInterface::SVectorNC SVectorNC;
    typedef NConstraintInterface::SMatrixNC SMatrixNC;

    typedef NConstraintInterface::ConstraintType ConstraintType;

    /// Default constructor
    NConstraintInterfaceADOLC(int _n_unknowns,
    		const ConstraintType _type = NC_EQUAL,
    		double _eps = 1e-6) :
        NConstraintInterface(_type, _eps),
        n_unknowns_(_n_unknowns),
        tape_available_(false),
        use_tape_(true),
        tape_(static_cast<short int>(TapeIDSingleton::Instance()->requestId())),
        dense_hessian_(NULL),
        sparse_nz_(0),
		sparse_r_ind_p_(NULL),
		sparse_c_ind_p_(NULL),
		sparse_val_p_(NULL) {

    }

    /// Destructor
    virtual ~NConstraintInterfaceADOLC() {

    	cleanup_sparse_hessian();

        TapeIDSingleton::Instance()->releaseId(static_cast<size_t>(tape_));
    }

    /**
     * \brief Only override this function
     */
    virtual adouble eval_c_adouble(const adouble* _x) = 0;

public:

    virtual int n_unknowns() {
        return n_unknowns_;
    }

    virtual double eval_constraint(const double* _x) {

        double y = 0.0;

        if(!tape_available_ || !use_tape_) {

        	adouble y_d = 0.0;

//        	if(active_vars_.get() == NULL)
//        		active_vars_.reset(new adouble[n_unknowns_]);

        	adouble* xa = new adouble[n_unknowns_];

			trace_on(tape_); // Start taping

			// Fill data vector
			for(int i = 0; i < n_unknowns_; ++i)
				xa[i] <<= _x[i];

			// Call virtual function to compute
			// functional value
			y_d = eval_c_adouble(xa);

			y_d >>= y;

			trace_off();

#ifdef ADOLC_STATS
			print_stats();
#endif

			delete[] xa;

			tape_available_ = true;

        } else {

        	int ec = function(tape_, 1, n_unknowns_, const_cast<double*>(_x), &y);

#ifdef ADOLC_RET_CODES
			std::cout << "Info: function() returned code " << ec << std::endl;
#endif

			// tape not valid anymore? retape and evaluate again
			if(ec < 0)
			{
			  tape_available_ = false;
			  return eval_constraint(_x);
			}
        }

        return y;
    }

    virtual void eval_gradient(const double* _x, SVectorNC& _g) {

        if(!tape_available_ || !use_tape_) {

        	// Evaluate original functional
            eval_constraint(_x);
        }

//        if(gradient_.get() == NULL)
//        	gradient_.reset(new double[n_unknowns_]);

        double* grad_p = new double[n_unknowns_];

        _g.resize(n_unknowns_);
        _g.setZero();

        // Evaluate gradient
		int ec = gradient(tape_, n_unknowns_, _x, grad_p);

		// Check if retaping is required
		if(ec < 0)
		{
#ifdef ADOLC_RET_CODES
			std::cout << __FUNCTION__ << " invokes retaping of function due to discontinuity! Return code: " << ec << std::endl;
#endif

			tape_available_ = false;
			eval_gradient(_x,_g);
		}

#ifdef ADOLC_RET_CODES
        std::cout << "Info: gradient() returned code " << ec << std::endl;
#endif

        for(int i = 0; i < n_unknowns_; ++i) {
            _g.coeffRef(i) += grad_p[i];
        }

        delete[] grad_p;
    }

    virtual void eval_hessian(const double* _x, SMatrixNC& _H) {

        _H.resize(n_unknowns_, n_unknowns_);

        // tape update required?
		if (!tape_available_ || !use_tape_)
			eval_constraint(_x);

		/*
		 * Hessian matrix is sparse
		 */
		if(sparse_hessian()) {

			int opt[2] = {0, 0};

			bool sparsity_pattern_available = bool(sparse_r_ind_p_);

			int ec = sparse_hess(tape_, n_unknowns_,
					sparsity_pattern_available, _x, &sparse_nz_,
					&sparse_r_ind_p_, &sparse_c_ind_p_, &sparse_val_p_, opt);

			if (ec < 0) {
#ifdef ADOLC_RET_CODES
				std::cout << __FUNCTION__ << " invokes retaping of function due to discontinuity! Return code: " << ec << std::endl;
#endif
				// Retape function if return code indicates discontinuity
				tape_available_ = false;
				eval_constraint(_x);
				ec = sparse_hess(tape_, n_unknowns_,
						sparsity_pattern_available, _x, &sparse_nz_,
						&sparse_r_ind_p_, &sparse_c_ind_p_, &sparse_val_p_,	opt);
			}

			// data should be available now
			assert(sparse_r_ind_p_ != NULL);
			assert(sparse_c_ind_p_ != NULL);
			assert(sparse_val_p_ != NULL);

#ifdef ADOLC_RET_CODES
			std::cout << "Info: sparse_hessian() returned code " << ec << std::endl;
#endif

			// Store data
			for (int i = 0; i < sparse_nz_; ++i) {

				_H(sparse_r_ind_p_[i], sparse_c_ind_p_[i]) = sparse_val_p_[i];

				// Provide also upper diagonal part?
				if (sparse_r_ind_p_[i] != sparse_c_ind_p_[i])
					_H(sparse_c_ind_p_[i], sparse_r_ind_p_[i]) = sparse_val_p_[i];
			}

		} else {
			/*
			 * Hessian matrix is dense
			 */
			allocate_dense_hessian();

			int ec = hessian(tape_, n_unknowns_, const_cast<double*>(_x),
					dense_hessian_);

			if (ec < 0) {
#ifdef ADOLC_RET_CODES
				std::cout << __FUNCTION__ << " invokes retaping of function due to discontinuity! Return code: " << ec << std::endl;
#endif
				// Retape function if return code indicates discontinuity
				tape_available_ = false;
				eval_constraint(_x);
				ec = hessian(tape_, n_unknowns_, const_cast<double*>(_x),
						dense_hessian_);
			}

#ifdef ADOLC_RET_CODES
			std::cout << "Info: hessian() returned code " << ec << std::endl;
#endif

			for (int i = 0; i < n_unknowns_; ++i) {
				// Diagonal
				_H(i, i) = dense_hessian_[i][i];
				for (int j = 0; j < i; ++j) {
					_H(i, j) = dense_hessian_[i][j];
					// Also store upper diagonal part
					_H(j, i) = dense_hessian_[i][j];
				}
			}
		}
    }

    /** \brief Use tape
     * Set this to false if the energy functional
     * is discontinuous (so that the operator tree
     * has to be re-established at each evaluation)
     */
    bool use_tape() const {
        return use_tape_;
    }

    void use_tape(bool _b) {
        use_tape_ = _b;
    }

    /**
     * \brief Provide information to make computations
     * as efficient as possible.
     */
    virtual bool is_linear()         const { return false; }
    virtual bool constant_gradient() const { return false; }

    /**
     * \brief Indicate whether the hessian is sparse.
     * If so, the computations (as well as the memory
     * consumption) can be performed more efficiently.
     */
    virtual bool constant_hessian () const { return false;}

private:

	void allocate_dense_hessian() {
		if (!dense_hessian_) {
			dense_hessian_ = new double*[n_unknowns_];
			for (int i = 0; i < n_unknowns_; ++i)
				dense_hessian_[i] = new double[i + 1];
		}

		// store #unknowns to access in destructor
		sparse_nz_ = n_unknowns_;
	}

	void cleanup_dense_hessian() {
		if (dense_hessian_) {
			for (int i = 0; i < sparse_nz_; ++i)
				delete[] dense_hessian_[i];

			delete[] dense_hessian_;
		}
	}

	void cleanup_sparse_hessian() {

		if (sparse_r_ind_p_ != NULL)
			delete[] sparse_r_ind_p_;
		if (sparse_c_ind_p_ != NULL)
			delete[] sparse_c_ind_p_;
		if (sparse_val_p_ != NULL)
			delete[] sparse_val_p_;
	}

    void print_stats() {

    	size_t tape_stats[11];
    	tapestats(tape_, tape_stats);
		std::cout << "Status values for tape " << tape_ << std::endl;
		std::cout << "===============================================" << std::endl;
		std::cout << "Number of independent variables:\t" << tape_stats[0] << std::endl;
		std::cout << "Number of dependent variables:\t\t" << tape_stats[1] << std::endl;
		std::cout << "Max. number of live active variables:\t" << tape_stats[2] << std::endl;
		std::cout << "Size of value stack:\t\t\t" << tape_stats[3] << std::endl;
		std::cout << "Buffer size:\t\t\t\t" << tape_stats[4] << std::endl;
		std::cout << "Total number of operations recorded:\t" << tape_stats[5] << std::endl;
		std::cout << "Other stats [6]:\t\t\t" << tape_stats[6] << std::endl;
		std::cout << "Other stats [7]:\t\t\t" << tape_stats[7] << std::endl;
		std::cout << "Other stats [8]:\t\t\t" << tape_stats[8] << std::endl;
		std::cout << "Other stats [9]:\t\t\t" << tape_stats[9] << std::endl;
		std::cout << "Other stats [10]:\t\t\t" << tape_stats[10] << std::endl;
		std::cout << "===============================================" << std::endl;
    }

    // Number of unknowns
    int n_unknowns_;

    bool tape_available_;
    bool use_tape_;

    SMatrixNC constant_hessian_;

    const short int tape_;

//    std::auto_ptr<adouble> active_vars_;
//    std::auto_ptr<double> gradient_;

    // dense hessian (if required)
    double** dense_hessian_;

    // Sparse hessian data
	int sparse_nz_;
	unsigned int* sparse_r_ind_p_;
	unsigned int* sparse_c_ind_p_;
	double* sparse_val_p_ ;
};

//=============================================================================
}// namespace COMISO
//=============================================================================
#endif // COMISO_ADOLC_AVAILABLE
//=============================================================================
#endif // COMISO_EIGEN3_AVAILABLE
//=============================================================================
#endif // ACG_NCONSTRAINTINTERFACEAD_HH defined
//=============================================================================


/**
@file

Copyright John Reid 2006

*/

#include "biopsy/defs.h"
#include "biopsy/binding_hits_max_chain.h"
#include "biopsy/analyse.h"
#include "biopsy/pssm.h"

#include <boost/iterator/filter_iterator.hpp>
#include <boost/range/adaptor/transformed.hpp>
#include <boost/range/adaptor/indirected.hpp>
#include <boost/range/algorithm/copy.hpp>

namespace biopsy {

binding_hit_traits::data
binding_hit_traits::get_data( const binding_hit & h )
{
    return boost::addressof( h );
}

const binding_hit_traits::character &
binding_hit_traits::get_char( const binding_hit & h )
{
    return h._binder_name;
}

binding_hit_traits::weight
binding_hit_traits::get_weight( const binding_hit & h )
{
    const weight odds_ratio = get_odds_ratio_from_p_binding( h._p_binding );
    const weight pre_prior_odds_ratio = odds_ratio / pssm_parameters::singleton().binding_background_odds_prior;
    return ( pre_prior_odds_ratio < 1.0 ) ? 0.0 : log( pre_prior_odds_ratio );
}

binding_hit_traits::coord
binding_hit_traits::get_start( const binding_hit & h )
{
    return h._location._position;
}

binding_hit_traits::coord
binding_hit_traits::get_end( const binding_hit & h )
{
    return h._location._position + h._location._length;
}

///**
//Estimate the number of boxes the maximal chain algorithm will use at the given threshold.
//*/
//unsigned
//num_boxes_for_threshold(
//    binding_hits_vec_ptr hit_array,
//    double threshold );
//
///**
//Calculate the best threshold to filter hits at before calculating maximal chain.
//*/
//double
//calculate_best_threshold(
//    binding_hits_vec_ptr hit_array,
//    unsigned num_boxes_limit );
//

unsigned
get_max_chain_max_num_sequences()
{
    return BIO_MAX_CHAIN_MAX_SEQUENCES;
}

unsigned
num_hits_at_threshold( const binding_hit::vec & hits, double threshold ) {
    return std::count_if(
        hits.begin(),
        hits.end(),
        binding_hit::p_binding_greater( threshold )
    );
}

unsigned
num_boxes_for_threshold(
    binding_hits_vec_ptr hit_array,
    double threshold )
{
    unsigned num_boxes = 1;
    BOOST_FOREACH( const binding_hit::vec_ptr & hits, *hit_array )
    {
        num_boxes *= num_hits_at_threshold( *hits, threshold );
    }

    return num_boxes;
}

typedef std::pair< double, unsigned > size_for_threshold;
typedef std::pair< size_for_threshold, size_for_threshold > threshold_pair;

threshold_pair
calculate_best_threshold(
    binding_hits_vec_ptr hit_array,
    unsigned num_boxes_limit )
{
    //std::cout << "Calculating best threshold..." << std::endl;

    size_for_threshold upper( 1.0, num_boxes_for_threshold( hit_array, 1.0 ) );
    size_for_threshold lower( 0.0, num_boxes_for_threshold( hit_array, 0.0 ) );

    // we only have to do something if the lower limit is too large...
    if( lower.second > num_boxes_limit ) {

        while( upper.first - lower.first > 1e-5 ) //whilst our search has not converged sufficiently
        {
            BOOST_ASSERT( upper.second <= num_boxes_limit );
            BOOST_ASSERT( lower.second > num_boxes_limit );

            // the mid-point of the current thresholds
            const double next_threshold = ( upper.first + lower.first ) / 2.0;
            // how many boxes for the new threshold?
            const size_for_threshold next( next_threshold, num_boxes_for_threshold( hit_array, next_threshold ) );

            // which threshold to update, the lower or the upper?
            if( next.second > num_boxes_limit ) //do we have too many boxes still?
            {
                lower = next; //yes - so update lower bound
            }
            else
            {
                upper = next; //no - so update upper bound
            }
        }

        BOOST_ASSERT( lower.second > num_boxes_limit );
    }
    BOOST_ASSERT( upper.second <= num_boxes_limit );

    return threshold_pair( lower, upper );
}

/** Calculate how many to remove given a count and a log-scale reduction. */
unsigned
how_many_to_remove( unsigned count, double log_to_remove_per_count ) {
    const double log_count = std::log( count );
    const unsigned to_remove = std::ceil( count - std::exp( log_count - log_to_remove_per_count ) );
    const unsigned result = std::min( count - 1, std::max( 0u, to_remove ) );
    return result;
}

/**
 * We know how many hits we want to remove between which thresholds.
 */
binding_hit::vec_ptr
remove_thresholded_hits(
    const binding_hit::vec & hits,
    double lower,
    double upper,
    unsigned to_remove
) {
    BOOST_ASSERT( to_remove ); // we should want to remove at least 1 to call this function.

    // how many hits in total do we have to remove from?
    const unsigned to_remove_from_total = num_hits_at_threshold( hits, lower ) - num_hits_at_threshold( hits, upper );
    unsigned could_remove_from = 0;
    unsigned removed = 0;

    binding_hit::p_binding_greater lower_pred( lower );
    binding_hit::p_binding_greater upper_pred( upper );

    binding_hit::vec_ptr results( new binding_hit::vec );
    BOOST_FOREACH( const binding_hit & hit, hits ) {
        if( upper_pred( hit ) ) { // if we pass the upper threshold we always keep the hit
            results->push_back( hit );
        } else if( lower_pred( hit ) ) { // if between the lower and upper thresholds we may/may not
            if( removed * to_remove_from_total > could_remove_from * to_remove ) {
                results->push_back( hit ); // we keep this hit
            } else {
                ++removed; // we removed this hit
            }
            ++could_remove_from;
        }
    }
    BOOST_ASSERT( could_remove_from == to_remove_from_total );
#ifndef NDEBUG
    const unsigned should_be_left =
#endif
    to_remove_from_total - to_remove;
#ifndef NDEBUG
    const unsigned are_left =
#endif
    num_hits_at_threshold( *results, lower ) - num_hits_at_threshold( *results, upper );
    BOOST_ASSERT( should_be_left == are_left );

    return results;
}

namespace { //anonymous

struct max_chain_builder
{
    binding_hit::vec_ptr _mc;
    max_chain_builder( binding_hit::vec_ptr mc ) : _mc( mc ) { }
    template< typename BoxPtr >
    void operator()( BoxPtr b )
    {
        _mc->push_back( *( b->_data ) );
    }
};

} //anonymous namespace

binding_hit::vec_ptr
analyse_max_chain(
    binding_hits_vec_ptr hit_array,
    unsigned num_boxes_limit
) {
    using namespace boost;

    //calculate the maximal chain if we don't have too many sequences
    binding_hit::vec_ptr mc;
    if( BIO_MAX_CHAIN_MAX_SEQUENCES >= hit_array->size() && hit_array->size() != 1 )
    {
        //calculate the maximal chain if possible
        mc.reset( new binding_hit::vec );
        if(
            ! BIO_NS::max_chain< binding_hit_traits >(
                *hit_array | adaptors::indirected,
                make_function_output_iterator( max_chain_builder( mc ) ),
                num_boxes_limit
            )
        ) {
            mc.reset();
        }
    }

    return mc;
}

} //namespace biopsy


//==================================================================================================
/**
  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_SLIDE_HPP_INCLUDED
#define BOOST_SIMD_ARCH_X86_AVX_SIMD_FUNCTION_SLIDE_HPP_INCLUDED

#include <boost/simd/detail/overload.hpp>
#include <boost/simd/detail/dispatch/hierarchy/exactly.hpp>
#include <boost/simd/detail/dispatch/meta/as_floating.hpp>
#include <boost/simd/function/genmask.hpp>
#include <boost/simd/detail/nsm.hpp>

#define BOOST_SIMD_UNARY_SLIDE(SZ,IDX)                                                              \
BOOST_DISPATCH_OVERLOAD ( slide_, (typename T), bs::avx_                                            \
                        , bs::pack_< bd::type##SZ##_<T>, bs::avx_ >                                 \
                          , bd::exactly_<nsm::type_traits::integral_constant<int,IDX>> \
                        )                                                                           \
/**/

#define BOOST_SIMD_BINARY_SLIDE(SZ,IDX)                                                             \
BOOST_DISPATCH_OVERLOAD ( slide_, (typename T), bs::avx_                                            \
                        , bs::pack_< bd::type##SZ##_<T>, bs::avx_ >                                 \
                        , bs::pack_< bd::type##SZ##_<T>, bs::avx_ >                                 \
                          , bd::exactly_<nsm::type_traits::integral_constant<int,IDX>> \
                        )                                                                           \
/**/

namespace boost { namespace simd { namespace ext
{
  namespace bd = boost::dispatch;
  namespace bs = boost::simd;
  namespace tt = nsm::type_traits;

  // -----------------------------------------------------------------------------------------------
  // [64 bits] Unary exact matches for all cardinal to minimize latency and # of registers used
  // Thanks to Sylvain Jubertie for those tips
  BOOST_SIMD_UNARY_SLIDE(64,1)
  {
    BOOST_FORCEINLINE T operator()( T const& a0
                                  , tt::integral_constant<int,1> const&
                                  ) const BOOST_NOEXCEPT
    {
      using f_t = bd::as_floating_t<T>;
      auto const b0 = bitwise_cast<f_t>(a0);
      return bitwise_cast<T>(f_t( _mm256_shuffle_pd(b0, _mm256_permute2f128_pd(b0, b0, 0x81), 0x5)));
    }
  };

  BOOST_SIMD_UNARY_SLIDE(64,2)
  {
    BOOST_FORCEINLINE T operator()( T const& a0
                                  , tt::integral_constant<int,2> const&
                                  ) const BOOST_NOEXCEPT
    {
      using f_t = bd::as_floating_t<T>;
      auto const b0 = bitwise_cast<f_t>(a0);
      return bitwise_cast<T>(f_t( _mm256_permute2f128_pd(b0, b0, 0x81)));
    }
  };

  BOOST_SIMD_UNARY_SLIDE(64,3)
  {
    BOOST_FORCEINLINE T operator()( T const& a0
                                  , tt::integral_constant<int,3> const&
                                  ) const BOOST_NOEXCEPT
    {
      using f_t = bd::as_floating_t<T>;
      auto const b0 = bitwise_cast<f_t>(a0);
      return bitwise_cast<T>(f_t( _mm256_shuffle_pd ( _mm256_permute2f128_pd(b0, b0, 0x81)
                                                    , bd::as_floating_t<T>(0)
                                                    , 0x1
                                                    )
                                )
                            );
    }
  };

  // -----------------------------------------------------------------------------------------------
  // [64 bits] Binary exact matches for all cardinal to minimize latency and # of registers used
  // Thanks to Sylvain Jubertie for those tips
  BOOST_SIMD_BINARY_SLIDE(64,1)
  {
    BOOST_FORCEINLINE T operator()( T const& a0, T const& a1
                                  , tt::integral_constant<int,1> const&
                                  ) const BOOST_NOEXCEPT
    {
      using f_t = bd::as_floating_t<T>;
      return bitwise_cast<T>(f_t( _mm256_shuffle_pd ( bitwise_cast<f_t>(a0)
                                                      , _mm256_permute2f128_pd( bitwise_cast<f_t>(a0)
                                                                              , bitwise_cast<f_t>(a1)
                                                                              , 0x21
                                                                              )
                                                      , 0x5
                                                      )
                                  )
                            );
    }
  };

  BOOST_SIMD_BINARY_SLIDE(64,2)
  {
    BOOST_FORCEINLINE T operator()( T const& a0, T const& a1
                                  , tt::integral_constant<int,2> const&
                                  ) const BOOST_NOEXCEPT
    {
      using f_t = bd::as_floating_t<T>;
      return bitwise_cast<T>(f_t( _mm256_permute2f128_pd( bitwise_cast<f_t>(a0)
                                                    , bitwise_cast<f_t>(a1)
                                                    , 0x21
                                                    )
                                  )
                            );
    }
  };

  BOOST_SIMD_BINARY_SLIDE(64,3)
  {
    BOOST_FORCEINLINE T operator()( T const& a0, T const& a1
                                  , tt::integral_constant<int,3> const&
                                  ) const BOOST_NOEXCEPT
    {
      using f_t = bd::as_floating_t<T>;
      return bitwise_cast<T>(f_t( _mm256_shuffle_pd ( _mm256_permute2f128_pd( bitwise_cast<f_t>(a0)
                                                                              , bitwise_cast<f_t>(a1)
                                                                              , 0x21
                                                                              )
                                                      , bitwise_cast<f_t>(a1)
                                                      , 0x5
                                                      )
                                  )
                            );
    }
  };

  // -----------------------------------------------------------------------------------------------
  // [32 bits] Unary exact matches for all cardinal to minimize latency and # of registers used
  BOOST_SIMD_UNARY_SLIDE(32,1)
  {
    BOOST_FORCEINLINE T operator()( T const& a0
                                  , tt::integral_constant<int,1> const&
                                  ) const BOOST_NOEXCEPT
    {
      using f_t = bd::as_floating_t<T>;
      auto const b0 = bitwise_cast<f_t>(a0);
      return bitwise_cast<T>(f_t( _mm256_permute_ps( _mm256_blend_ps( b0
                                                              , _mm256_permute2f128_ps(b0,b0,0x81)
                                                              , 0x11
                                                              )
                                              , 0x39
                                              )
                                 )
                            );
    }
  };

  BOOST_SIMD_UNARY_SLIDE(32,2)
  {
    BOOST_FORCEINLINE T operator()( T const& a0
                                  , tt::integral_constant<int,2> const&
                                  ) const BOOST_NOEXCEPT
    {
      using f_t = bd::as_floating_t<T>;
      auto const b0 = bitwise_cast<f_t>(a0);
      return bitwise_cast<T>(f_t( _mm256_shuffle_ps(b0, _mm256_permute2f128_ps(b0, b0, 0x81), 0x4e) ));
    }
  };

  BOOST_SIMD_UNARY_SLIDE(32,3)
  {
    BOOST_FORCEINLINE T operator()( T const& a0
                                  , tt::integral_constant<int,3> const&
                                  ) const BOOST_NOEXCEPT
    {
      using f_t = bd::as_floating_t<T>;
      auto const b0 = bitwise_cast<f_t>(a0);
      return bitwise_cast<T>(f_t( _mm256_permute_ps( _mm256_blend_ps( b0
                                                              , _mm256_permute2f128_ps(b0,b0,0x81)
                                                              , 0x77
                                                              )
                                              , 0x93
                                              )
                                 )
                            );
    }
  };

  BOOST_SIMD_UNARY_SLIDE(32,4)
  {
    BOOST_FORCEINLINE T operator()( T const& a0
                                  , tt::integral_constant<int,4> const&
                                  ) const BOOST_NOEXCEPT
    {
      using f_t = bd::as_floating_t<T>;
      auto const b0 = bitwise_cast<f_t>(a0);
      return bitwise_cast<T>(f_t( _mm256_permute2f128_ps(b0,b0,0x81)));
    }
  };

  BOOST_SIMD_UNARY_SLIDE(32,5)
  {
    BOOST_FORCEINLINE T operator()( T const& a0
                                  , tt::integral_constant<int,5> const&
                                  ) const BOOST_NOEXCEPT
    {
      using f_t = bd::as_floating_t<T>;
      auto const b0 = bitwise_cast<f_t>(a0);
      return bitwise_cast<T>(f_t( _mm256_permute_ps( _mm256_blend_ps (f_t(0)
                                                                , _mm256_permute2f128_ps(b0,b0,0x81)
                                                                , 0xe
                                                                )
                                              , 0x39
                                              )
                                 )
                            );
    }
  };

  BOOST_SIMD_UNARY_SLIDE(32,6)
  {
    BOOST_FORCEINLINE T operator()( T const& a0
                                  , tt::integral_constant<int,6> const&
                                  ) const BOOST_NOEXCEPT
    {
      using f_t = bd::as_floating_t<T>;
      auto const b0 = bitwise_cast<f_t>(a0);
      return bitwise_cast<T>(f_t( _mm256_shuffle_ps( _mm256_permute2f128_ps(b0,b0,0x81 ), f_t(0), 0xe )));
    }
  };

  BOOST_SIMD_UNARY_SLIDE(32,7)
  {
    BOOST_FORCEINLINE T operator()( T const& a0
                                  , tt::integral_constant<int,7> const&
                                  ) const BOOST_NOEXCEPT
    {
      using f_t = bd::as_floating_t<T>;
      auto const b0 = bitwise_cast<f_t>(a0);
      return bitwise_cast<T>(f_t( _mm256_permute_ps( _mm256_blend_ps (f_t(0)
                                                                , _mm256_permute2f128_ps(b0,b0,0x81)
                                                                , 0x8
                                                                )
                                              , 0x93
                                              )
                                 )
                            );
    }
  };

  // -----------------------------------------------------------------------------------------------
  // [32 bits] Binary exact matches for all cardinal to minimize latency and # of registers used
  BOOST_SIMD_BINARY_SLIDE(32,1)
  {
    BOOST_FORCEINLINE T operator()( T const& a0, T const& a1
                                  , tt::integral_constant<int,1> const&
                                  ) const BOOST_NOEXCEPT
    {
      using f_t = bd::as_floating_t<T>;
      auto const b0 = bitwise_cast<f_t>(a0);
      auto const b1 = bitwise_cast<f_t>(a1);

      return bitwise_cast<T>(f_t( _mm256_permute_ps
                                   ( _mm256_blend_ps ( b0
                                                     , _mm256_permute2f128_ps(b0,b1,0x21)
                                                     , 0x11
                                                     )
                                   , 0x39
                                   )
                                 )
                            );
    }
  };

  BOOST_SIMD_BINARY_SLIDE(32,2)
  {
    BOOST_FORCEINLINE T operator()( T const& a0, T const& a1
                                  , tt::integral_constant<int,2> const&
                                  ) const BOOST_NOEXCEPT
    {
      using f_t = bd::as_floating_t<T>;
      auto const b0 = bitwise_cast<f_t>(a0);
      auto const b1 = bitwise_cast<f_t>(a1);

      return bitwise_cast<T>(f_t( _mm256_shuffle_ps(b0, _mm256_permute2f128_ps(b0,b1,0x21), 0x4e) ));
    }
  };

  BOOST_SIMD_BINARY_SLIDE(32,3)
  {
    BOOST_FORCEINLINE T operator()( T const& a0, T const& a1
                                  , tt::integral_constant<int,3> const&
                                  ) const BOOST_NOEXCEPT
    {
      using f_t = bd::as_floating_t<T>;
      auto const b0 = bitwise_cast<f_t>(a0);
      auto const b1 = bitwise_cast<f_t>(a1);
      return bitwise_cast<T>(f_t( _mm256_permute_ps( _mm256_blend_ps ( b0
                                                                , _mm256_permute2f128_ps(b0,b1,0x21)
                                                                , 0x77
                                                                )
                                              , 0x93
                                                   )
                                 )
                            );
    }
  };

  BOOST_SIMD_BINARY_SLIDE(32,4)
  {
    BOOST_FORCEINLINE T operator()( T const& a0, T const& a1
                                  , tt::integral_constant<int,4> const&
                                  ) const BOOST_NOEXCEPT
    {
      using f_t = bd::as_floating_t<T>;
      return bitwise_cast<T>(f_t( _mm256_permute2f128_ps(bitwise_cast<f_t>(a0),bitwise_cast<f_t>(a1),0x21)));
    }
  };

  BOOST_SIMD_BINARY_SLIDE(32,5)
  {
    BOOST_FORCEINLINE T operator()( T const& a0, T const& a1
                                  , tt::integral_constant<int,5> const&
                                  ) const BOOST_NOEXCEPT
    {
      using f_t = bd::as_floating_t<T>;
      auto const b0 = bitwise_cast<f_t>(a0);
      auto const b1 = bitwise_cast<f_t>(a1);
      return bitwise_cast<T>(f_t( _mm256_permute_ps
                                   ( _mm256_blend_ps ( b1
                                                     , _mm256_permute2f128_ps(b0,b1,0x21)
                                                     , 0xee
                                                     )
                                   , 0x39
                                   )
                                  )
                            );
    }
  };

  BOOST_SIMD_BINARY_SLIDE(32,6)
  {
    BOOST_FORCEINLINE T operator()( T const& a0, T const& a1
                                  , tt::integral_constant<int,6> const&
                                  ) const BOOST_NOEXCEPT
    {
      using f_t = bd::as_floating_t<T>;
      auto const b0 = bitwise_cast<f_t>(a0);
      auto const b1 = bitwise_cast<f_t>(a1);
      return bitwise_cast<T>(f_t( _mm256_shuffle_ps( _mm256_permute2f128_ps(b0,b1,0x21),b1,0x4e)));
    }
  };

  BOOST_SIMD_BINARY_SLIDE(32,7)
  {
    BOOST_FORCEINLINE T operator()( T const& a0, T const& a1
                                  , tt::integral_constant<int,7> const&
                                  ) const BOOST_NOEXCEPT
    {
      using f_t = bd::as_floating_t<T>;
      auto const b0 = bitwise_cast<f_t>(a0);
      auto const b1 = bitwise_cast<f_t>(a1);
      return bitwise_cast<T>(f_t( _mm256_permute_ps
                                   ( _mm256_blend_ps ( b1
                                                     , _mm256_permute2f128_ps(b0,b1,0x21)
                                                     , 0x88
                                                     )
                                   , 0x93
                                   )
                                 )
                            );
    }
  };

  // -----------------------------------------------------------------------------------------------
  // Reroute for small integers
  BOOST_DISPATCH_OVERLOAD ( slide_
                          , (typename T, typename Offset)
                          , bs::avx_
                          , bs::pack_< bd::integer_<T>, bs::avx_ >
                          , bs::pack_< bd::integer_<T>, bs::avx_ >
                          , bd::constant_<bd::integer_<Offset>>
                          )
  {
    using hcard = tt::integral_constant<int,(T::static_size/2)>;

    static BOOST_FORCEINLINE T unroll( T const& a0, T const& a1, tt::true_type const& )
    {
      auto s0 = slice(a0);
      auto l1 = slice_low(a1);
      return combine( slide<Offset::value%hcard::value>(s0[0], s0[1])
                    , slide<Offset::value%hcard::value>(s0[1], l1)
                    );
    }

    static BOOST_FORCEINLINE T unroll( T const& a0, T const& a1, tt::false_type const& )
    {
      auto s1 = slice(a1);
      auto h0 = slice_high(a0);
      return combine( slide<Offset::value%hcard::value>(h0   , s1[0])
                    , slide<Offset::value%hcard::value>(s1[0], s1[1])
                    );
    }

    BOOST_FORCEINLINE T operator()(T const& a0, T const& a1, Offset const&) const
    {
      return unroll ( a0, a1
                    , nsm::bool_<(Offset::value < hcard::value)>{}
                    );
    }
  };

  // -----------------------------------------------------------------------------------------------
  // Reroute for logical
  BOOST_DISPATCH_OVERLOAD ( slide_
                          , (typename T, typename Offset)
                          , bs::avx_
                          , bs::pack_< bs::logical_<T>, bs::avx_ >
                          , bd::constant_<bd::integer_<Offset>>
                          )
  {
    BOOST_FORCEINLINE T operator()(T const& a0, Offset const&) const BOOST_NOEXCEPT
    {
      return bitwise_cast<T>( slide<Offset::value>( genmask(a0) ) );
    }
  };

  BOOST_DISPATCH_OVERLOAD ( slide_
                          , (typename T, typename Offset)
                          , bs::avx_
                          , bs::pack_< bs::logical_<T>, bs::avx_ >
                          , bs::pack_< bs::logical_<T>, bs::avx_ >
                          , bd::constant_<bd::integer_<Offset>>
                          )
  {
    BOOST_FORCEINLINE T operator()(T const& a0, T const& a1, Offset const&) const BOOST_NOEXCEPT
    {
      return bitwise_cast<T>( slide<Offset::value>( genmask(a0), genmask(a1) ) );
    }
  };
} } }

#endif


#include <algorithm>
#include <boost/optional.hpp>
#include <climits>
#include <cmath>
#include <cstdio>
#include <cstring>
#include <functional>
#include <iomanip>
#include <iostream>
#include <limits>
#include <list>
#include <map>
#include <numeric>
#include <queue>
#include <set>
#include <sstream>
#include <string>
#include <tuple>
#include <type_traits>
#include <unordered_map>
#include <unordered_set>
#include <utility>
#include <vector>

#define REP(i, n) for(int i = 0, i##_MACRO = (n); i < i##_MACRO; i++)
#define RANGE(i, a, b) for(int i = (a), i##_MACRO = (b); i < i##_MACRO; i++)
#define EACH(e, a) for(auto&& e : a)
#define ALL(a) std::begin(a), std::end(a)
#define RALL(a) std::rbegin(a), std::rend(a)
#define FILL(a, n) memset((a), n, sizeof(a))
#define FILLZ(a) FILL(a, 0)
#define INT(x) (static_cast<int>(x))
#define PRECISION(x) std::fixed << std::setprecision(x)

using namespace std;

using ll = long long;
using VI = std::vector<int>;
using VI2D = std::vector<vector<int>>;
using VLL = std::vector<long long>;
using VLL2D = std::vector<vector<long long>>;

constexpr int INF = 2e9;
constexpr double EPS = 1e-10;
constexpr double PI = acos(-1.0);

constexpr int dx[] = {-1, 0, 1, 0};
constexpr int dy[] = {0, -1, 0, 1};

template <typename T, std::size_t N>
struct make_vector_type {
	using type =
		typename std::vector<typename make_vector_type<T, (N - 1)>::type>;
};

template <typename T>
struct make_vector_type<T, 0> {
	using type = typename std::vector<T>;
};

template <typename T, size_t N>
auto make_vector_impl(const std::vector<std::size_t>& ls, T init_value) {
	if constexpr(N == 0) {
		return std::vector<T>(ls[N], init_value);
	} else {
		return typename make_vector_type<T, N>::type(
			ls[N], make_vector_impl<T, (N - 1)>(ls, init_value));
	}
}

template <typename T, std::size_t N>
auto make_vector(const std::size_t (&ls)[N], T init_value) {
	std::vector<std::size_t> dimensions(N);
	for(int i = 0; i < N; i++) {
		dimensions[N - i - 1] = ls[i];
	}
	return make_vector_impl<T, N - 1>(dimensions, init_value);
}

template <typename T>
std::vector<T> make_vector(std::size_t size, T init_value) {
	return std::vector<T>(size, init_value);
}

template <typename T>
constexpr int sign(T x) {
	return x < 0 ? -1 : x > 0 ? 1 : 0;
}

template <>
constexpr int sign(double x) {
	return x < -EPS ? -1 : x > EPS ? 1 : 0;
}

template <typename T, typename U>
constexpr void chmax(T& m, U x) {
	m = max<T>(m, x);
}

template <typename T, typename U>
constexpr void chmin(T& m, U x) {
	m = min<T>(m, x);
}

template <typename T>
constexpr T square(T x) {
	return x * x;
}

using PII = pair<int, int>;

const int N_MAX = 100000;

int use_turns[N_MAX];

int count_turn(VI2D& children, int node) {
	int& ret = use_turns[node];
	ret = 3;
	EACH(child, children[node]) { ret += count_turn(children, child); }
	return ret;
}

PII memo[N_MAX][2][2];

PII rec(VI2D& children, int node, int coin, int turn) {
	PII& ret = memo[node][coin][turn];
	if(ret != PII()) {
		return ret;
	}
	if(coin) {
		ret = rec(children, node, 0, !turn);
		(turn == 0 ? ret.first : ret.second) += 1;
		return ret;
	}
	int num_child = children[node].size();
	if(num_child == 0) {
		return make_pair(0, 0);
	} else if(num_child == 1) {
		return ret = rec(children, children[node][0], 1, !turn);
	}

	vector<PII> c(num_child);
	REP(i, num_child) {
		int child = children[node][i];
		c[i] = make_pair(
			child, use_turns[child] * (use_turns[child] % 2 == 1 ? 1 : -1));
	}
	sort(ALL(c), [](auto a, auto b) { return a.second < b.second; });
	int nt = turn;
	EACH(ct, c) {
		auto [child, tcount] = ct;
		if(use_turns[child] % 2 == 1) {
			nt = !nt;
			auto tmp = rec(children, child, 1, nt);
			ret.first += tmp.first;
			ret.second += tmp.second;
		} else {
			auto tmp = rec(children, child, 1, !nt);
			ret.first += tmp.first;
			ret.second += tmp.second;
		}
	}
	return ret;
}

int main() {
	int n;
	cin >> n;
	VI p(n - 1);
	EACH(e, p) {
		cin >> e;
		e--;
	}

	VI parents(n, -1);
	VI2D children(n);
	REP(i, n - 1) {
		parents[i + 1] = p[i];
		children[p[i]].push_back(i + 1);
	}

	count_turn(children, 0);
	auto [takahashi, aoki] = rec(children, 0, 1, 0);

	cout << takahashi << endl;
	return 0;
}


//==============================================================================
//         Copyright 2003 - 2012 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//         Copyright 2012 - 2014 MetaScale SAS
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_REDUCTION_FUNCTIONS_SIMD_COMMON_IS_INCLUDED_C_HPP_INCLUDED
#define BOOST_SIMD_REDUCTION_FUNCTIONS_SIMD_COMMON_IS_INCLUDED_C_HPP_INCLUDED

#include <boost/simd/reduction/functions/is_included_c.hpp>
#include <boost/simd/include/functions/simd/bitwise_and.hpp>
#include <boost/simd/include/functions/simd/none.hpp>
#include <boost/simd/sdk/meta/as_logical.hpp>
#include <boost/simd/sdk/meta/scalar_of.hpp>
#include <boost/dispatch/attributes.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_DISPATCH_IMPLEMENT          ( is_included_c_, tag::cpu_
                                    , (A0)(X)
                                    , ((simd_<arithmetic_<A0>,X>))
                                      ((simd_<arithmetic_<A0>,X>))
                                    )
  {
    typedef typename meta::scalar_of<A0>::type sA0;
    typedef typename meta::as_logical<sA0>::type result_type;

    BOOST_FORCEINLINE result_type operator()(A0 const& a0,A0 const& a1) const
    {
      return none(bitwise_and(a1, a0));
    }
  };

} } }
#endif


#include <gazebo_state_plugins/GazeboMapPublisher.h>
#include <gazebo_version_helpers/GazeboVersionHelpers.h>

#include <gazebo/physics/physics.hh>

#include <Eigen/Core>

#include <shape_msgs/SolidPrimitive.h>
#include <geometry_msgs/Pose.h>

#include <nav_msgs/MapMetaData.h>
#include <std_msgs/Float64.h>

#define DEFAULT_REQUEST_MAP_SERVICE "dynamic_map"

//publishing rate
#define DEFAULT_MAP_PUB_FREQ "1"

#define MAP_OFFSET_X -10
#define MAP_OFFSET_Y -10
#define MAP_LEN_X 20
#define MAP_LEN_Y 20
//map resolution (m/cell)
#define MAP_RESOLUTION 0.1

//height of obstacles to consider for generating the map
#define MAP_HEIGHT 1.0

#define MAP_QUEUE_SIZE 50

#define MSG_PRINT(o)\
    std::cout<<o<<std::endl;

#define MAX_MAP_VAL 100

using gazebo::GazeboMapPublisher;
using gazebo::GzVector3;

GZ_REGISTER_WORLD_PLUGIN(GazeboMapPublisher)

class GazeboMapPublisher::CollisionMapRequest {
    public:
    CollisionMapRequest(): threshold(MAX_MAP_VAL) {
    }
    CollisionMapRequest(const CollisionMapRequest& o):
        upperLeft(o.upperLeft),
        upperRight(o.upperRight),
        lowerRight(o.lowerRight),
        lowerLeft(o.lowerLeft),
        height(o.height),
        resolution(o.resolution),
        threshold(o.threshold){}

    Eigen::Vector2i  upperLeft;
    Eigen::Vector2i  upperRight;
    Eigen::Vector2i  lowerRight;
    Eigen::Vector2i  lowerLeft;
    double height;
    double resolution;
    unsigned int  threshold;
};

GazeboMapPublisher::GazeboMapPublisher() : WorldPlugin() {

    ros::NodeHandle _node("/gazebo_state_plugins");

    _node.param<std::string>("publish_map_topic", MAP_TOPIC, "");
    MSG_PRINT("Got gazebo map topic name :"<<MAP_TOPIC.c_str());

    _node.param<std::string>("map_frame_id", MAP_FRAME, "/map");
    MSG_PRINT("Got gazebo map frame id: "<<MAP_FRAME.c_str());

    _node.param<std::string>("map_metadata_topic", METADATA_TOPIC, "map_metadata");
    MSG_PRINT("Got gazebo map metadata topic name:"<<METADATA_TOPIC.c_str());

    _node.param<std::string>("request_map_service", REQUEST_MAP_SERVICE, DEFAULT_REQUEST_MAP_SERVICE);
    MSG_PRINT("Got gazebo map service name: " <<REQUEST_MAP_SERVICE.c_str());

    _node.param<std::string>("robot_name", ROBOT_NAME, "robot");
    MSG_PRINT("Got gazebo map publisher robot name: " <<ROBOT_NAME.c_str());

    std::string MAP_PUB_FREQUENCY;
    _node.param<std::string>("map_pub_frequency", MAP_PUB_FREQUENCY, DEFAULT_MAP_PUB_FREQ);
    MAP_PUB_FREQ=atoi(MAP_PUB_FREQUENCY.c_str());
    MSG_PRINT("Got gazebo map publish frequency: <"<<MAP_PUB_FREQ<<"> from string "<<MAP_PUB_FREQUENCY.c_str());

    map_offset_x=MAP_OFFSET_X;
    map_offset_y=MAP_OFFSET_Y;
    map_len_x=MAP_LEN_X;
    map_len_y=MAP_LEN_Y;
    map_resolution=MAP_RESOLUTION;
    map_height=MAP_HEIGHT;

    worldChangedSinceLastAdvert=true;

}

void GazeboMapPublisher::Load(physics::WorldPtr _world, sdf::ElementPtr _sdf){
    world=_world;

    request_map_srv = node.advertiseService(REQUEST_MAP_SERVICE, &GazeboMapPublisher::requestMap,this);

    if (MAP_TOPIC.length()==0) {
        ROS_WARN("No map being published from gazebo, as the parameter 'gazebo_publish_map' was specified empty. This may be intended.");
        return;
    }

    update_connection = event::Events::ConnectWorldUpdateBegin(boost::bind(&GazeboMapPublisher::onWorldUpdate, this));

    ros::Rate rate(MAP_PUB_FREQ);
    publishTimer=node.createTimer(rate,&GazeboMapPublisher::advertEvent, this);

    map_pub = node.advertise<nav_msgs::OccupancyGrid>(MAP_TOPIC, MAP_QUEUE_SIZE);
    meta_pub = node.advertise<nav_msgs::MapMetaData>(METADATA_TOPIC,MAP_QUEUE_SIZE);
}

bool GazeboMapPublisher::requestMap(nav_msgs::GetMap::Request &req, nav_msgs::GetMap::Response &res) {
    nav_msgs::OccupancyGrid map=getMap();
    res.map=map;
    MSG_PRINT("Received service request for map!");
    return true;
}

void GazeboMapPublisher::advertEvent(const ros::TimerEvent& e) {

    if (!worldChangedSinceLastAdvert) return;

    if (map_pub.getNumSubscribers()!=0) {
        nav_msgs::OccupancyGrid map=getMap();
        map_pub.publish(map);
    }

    if (meta_pub.getNumSubscribers()!=0) {
        nav_msgs::MapMetaData mdata=getMetaData();
        meta_pub.publish(mdata);
    }

    worldChangedSinceLastAdvert=false;
}

void GazeboMapPublisher::onWorldUpdate() {
    worldChangedSinceLastAdvert=true;
}

//see http://gazebosim.org/wiki/Tutorials/1.4/custom_messages#Collision_Map_Creator_Plugin
bool GazeboMapPublisher::createMap(const CollisionMapRequest &msg, const std::string& map_frame, nav_msgs::OccupancyGrid& map) {
    /*MSG_PRINT("Creating collision map with corners at (" <<
                 msg.upperLeft.x() << ", " << msg.upperLeft.y() << "), (" <<
                 msg.upperRight.x() << ", " << msg.upperRight.y() << "), (" <<
                 msg.lowerRight.x() << ", " << msg.lowerRight.y() << "), (" <<
                 msg.lowerLeft.x() << ", " << msg.lowerLeft.y() << ") with collision projected from z = " <<
                 msg.height << "\nResolution = " << msg.resolution << " m\n" <<
                 "Occupied spaces will be filled with: " << msg.threshold);
    */
    double z = msg.height;
    double dX_vertical = msg.upperLeft.x() - msg.lowerLeft.x();
    double dY_vertical = msg.upperLeft.y() - msg.lowerLeft.y();
    double mag_vertical = sqrt(dX_vertical * dX_vertical + dY_vertical * dY_vertical);
    dX_vertical = msg.resolution * dX_vertical / mag_vertical;
    dY_vertical = msg.resolution * dY_vertical / mag_vertical;

    double step_s = msg.resolution;

    double dX_horizontal = msg.upperRight.x() - msg.upperLeft.x();
    double dY_horizontal = msg.upperRight.y() - msg.upperLeft.y();
    double mag_horizontal = sqrt(dX_horizontal * dX_horizontal + dY_horizontal * dY_horizontal);
    dX_horizontal = msg.resolution * dX_horizontal / mag_horizontal;
    dY_horizontal = msg.resolution * dY_horizontal / mag_horizontal;

    int count_vertical = mag_vertical / msg.resolution;
    int count_horizontal = mag_horizontal / msg.resolution;

    if (count_vertical == 0 || count_horizontal == 0)
    {
        MSG_PRINT("Image has a zero dimensions, check coordinates");
        return false;
    }
    double x,y;

    unsigned int threshold=msg.threshold;
    if (threshold > MAX_MAP_VAL) threshold=MAX_MAP_VAL;
    unsigned int blank=MAX_MAP_VAL-threshold;
    unsigned int fill=MAX_MAP_VAL;
    std::vector<std::vector<int> > grid;

    double dist;
    std::string entityName;
    GzVector3 start = gazebo::GetVector(0, 0, msg.height);
    GzVector3 end = gazebo::GetVector(0, 0, 0.001);

    gazebo::physics::PhysicsEnginePtr engine = gazebo::GetPhysics(world);
    engine->InitForThread();
    //gazebo::physics::RayShapePtr ray = boost::shared_dynamic_cast<gazebo::physics::RayShape>(
    gazebo::physics::RayShapePtr ray = boost::dynamic_pointer_cast<gazebo::physics::RayShape>(
          engine->CreateShape("ray", gazebo::physics::CollisionPtr()));

    //MSG_PRINT("Rasterizing model and checking collisions");
    grid.resize(count_horizontal,std::vector<int>(count_vertical,blank));

    for (int i = 0; i < count_vertical; ++i) {
        //MSG_PRINT("Percent complete: " << i * 100.0 / count_vertical);
        x = i * dX_vertical + msg.lowerLeft.x();
        y = i * dY_vertical + msg.lowerLeft.y();
        for (int j = 0; j < count_horizontal; ++j) {
            x += dX_horizontal;
            y += dY_horizontal;

            gazebo::SetX(start, x);
            gazebo::SetX(end, x);
            gazebo::SetY(start, y);
            gazebo::SetY(end, y);
            ray->SetPoints(start, end);
            ray->GetIntersection(dist, entityName);
            if (!entityName.empty()) {
                std::string model=entityName.substr(0,entityName.find("::"));
                //MSG_PRINT("Collides: "<<entityName<<" robot name="<<ROBOT_NAME<<" model: "<<model);
                if (model!=ROBOT_NAME)
                    grid[i][j]=fill;
            }
        }
    }

    map.header.frame_id=map_frame;
    map.header.stamp=ros::Time::now();

    nav_msgs::MapMetaData meta=getMetaData();
    map.info=meta;

    /*map.info.map_load_time=ros::Time::now();
    map.info.resolution=msg.resolution;
    map.info.width=count_horizontal;
    map.info.height=count_vertical;
    map.info.origin.position.x=msg.lowerLeft.x();
    map.info.origin.position.y=msg.lowerLeft.y();
    map.info.origin.position.z=0;
    map.info.origin.orientation.x=0;
    map.info.origin.orientation.y=0;
    map.info.origin.orientation.z=0;
    map.info.origin.orientation.w=1;
    MSG_PRINT("Meta compare: "<<std::endl<<meta<<"===="<<std::endl<<map.info);
    */

    //MSG_PRINT("Completed calculations, created map");

    int _x=0;
    int _y=0;
    for (std::vector<std::vector<int> >::iterator yIt=grid.begin(); yIt!=grid.end(); ++yIt) {
        _x=0;
        for (std::vector<int>::iterator xIt=yIt->begin(); xIt!=yIt->end(); ++xIt) {
            map.data.push_back(*xIt);
            ++_x;
        }
        ++_y;
    }
    return true;
}

GazeboMapPublisher::CollisionMapRequest GazeboMapPublisher::getCollisionRequest(){
    CollisionMapRequest r;
    r.lowerLeft= Eigen::Vector2i(map_offset_x,map_offset_y);
    r.lowerRight=Eigen::Vector2i(map_offset_x+map_len_x,map_offset_y);
    r.upperLeft= Eigen::Vector2i(map_offset_x,map_offset_y+map_len_y);
    r.upperRight=Eigen::Vector2i(map_offset_x+map_len_x,map_offset_y+map_len_y);
    r.resolution=map_resolution;
    r.height=map_height;
    r.threshold=MAX_MAP_VAL;
    return r;
}

nav_msgs::MapMetaData GazeboMapPublisher::getMetaData() {
    nav_msgs::MapMetaData mdata;
    mdata.map_load_time=ros::Time::now();
    mdata.resolution=map_resolution;
    mdata.width=map_len_x/map_resolution;
    mdata.height=map_len_y/map_resolution;
    mdata.origin.position.x=map_offset_x;
    mdata.origin.position.y=map_offset_y;
    mdata.origin.position.z=0;
    mdata.origin.orientation.w=0;
    return mdata;
}

nav_msgs::OccupancyGrid GazeboMapPublisher::getMap() {
    nav_msgs::OccupancyGrid map;

#if 1
    CollisionMapRequest r=getCollisionRequest();
    if (!createMap(r,MAP_FRAME,map)) {
        ROS_ERROR("Could not request map");
    }
#else
    map.header.frame_id=MAP_FRAME;
    map.header.stamp=ros::Time::now();

    map.info=getMetaData();

    float map_cells_x=floor(map_len_x/map_resolution);
    float map_cells_y=floor(map_len_y/map_resolution);
    //create an empty occupancy grid
    map.data.resize(map_cells_x*map_cells_y,0);

    //... fill it with data!
    //for each object found in gazebo, locate it on the map and check if it intersects a cell,
    //and put a value of 100 in that cell.

    //skip the robot from the occupancy grid!
#endif
    return map;
}


// See
// http://herbsutter.com/2009/05/15/effective-concurrency-eliminate-false-sharing/

#include <Common/Timer.h>
#include <algorithm>
#include <boost/lexical_cast.hpp>
#include <iostream>
#include <random>
#include <tbb/task_group.h>
#include <tbb/task_scheduler_init.h>
#include <vector>

class SquareMatrix
{
public:
  SquareMatrix(size_t dim) : m_data(dim * dim, 0), m_dim(dim)
  {
  }
  double* operator[](size_t i)
  {
    return &m_data[i * m_dim];
  }

  std::vector<double>::iterator begin()
  {
    return m_data.begin();
  }
  std::vector<double>::iterator end()
  {
    return m_data.end();
  }

private:
  std::vector<double> m_data;
  size_t m_dim;
};

int main(int argc, char* argv[])
{
  size_t THREADS = 4;
  if (argc > 1)
  {
    THREADS = boost::lexical_cast<size_t>(argv[1]);
  }

  size_t dim = 1024;
  if (argc > 2)
  {
    dim = boost::lexical_cast<size_t>(argv[2]);
  }

  std::uniform_real_distribution<double> distribution(-1, 1);
  std::mt19937 engine;
  auto generator = [&]
  {
    return distribution(engine);
  };

  SquareMatrix m(dim);
  std::generate(m.begin(), m.end(), generator);

  std::vector<size_t> perThreadCount(THREADS, 0);
  tbb::task_scheduler_init tsInit(THREADS);
  tbb::task_group tg;
  {
    Timer t("Time count");

    for (size_t iTask = 0; iTask < THREADS; ++iTask)
    {
      tg.run([=, &perThreadCount, &m]
             {
               size_t taskSize = dim / THREADS;
               size_t taskStart = iTask * taskSize;
               size_t taskEnd = std::min(taskStart + taskSize, dim);
               size_t count = 0;
               for (size_t i = taskStart; i < taskEnd; ++i)
               {
                 for (size_t j = 0; j < dim; ++j)
                 {
                   if (m[i][j] >= 0.0)
                   {
                     ++perThreadCount[iTask]; // Causes false sharing
                     //++count; //This prevents false sharing
                   }
                 }
               }
               // perThreadCount[iTask] = count; // This prevents false sharing
             });
    }
    tg.wait();
  }
  auto positiveCount =
      std::accumulate(perThreadCount.begin(), perThreadCount.end(), 0);
  std::cout << dim << " by " << dim << " matrix has " << positiveCount
            << " positive entries = " << 100.0 * positiveCount / (dim * dim)
            << "%" << std::endl;
}


#pragma once

#include <Core/RaCore.hpp>
#include <Eigen/Core>
#include <Eigen/Geometry>

namespace Ra {
namespace Core {
namespace Geometry {
/// An oriented bounding box.
class Obb
{
  public:
    using Transform = Eigen::Transform<Scalar, 3, Eigen::Affine>;
    using Aabb      = Eigen::AlignedBox<Scalar, 3>;

    /// Constructors and destructor.

    /// Initializes an empty bounding box.
    inline Obb() : m_aabb(), m_transform( Transform::Identity() ) {}

    /// Initialize an OBB from an AABB and a transform.
    inline Obb( const Aabb& aabb, const Transform& tr ) : m_aabb( aabb ), m_transform( tr ) {}

    /// Default copy constructor and assignment operator.
    Obb( const Obb& other ) = default;
    Obb& operator=( const Obb& other ) = default;

    virtual inline ~Obb() {}

    /// Return the AABB enclosing this
    inline Aabb toAabb() const {
        if ( m_aabb.isEmpty() ) { return m_aabb; }
        Aabb tmp;
        for ( int i = 0; i < 8; ++i )
        {
            tmp.extend( worldCorner( i ) );
        }
        return tmp;
    }

    /// Extends the OBB with an new point.
    inline void addPoint( const Eigen::Matrix<Scalar, 3, 1>& p ) { m_aabb.extend( p ); }

    /// Returns the position of the i^th corner of AABB (model space)
    inline Eigen::Matrix<Scalar, 3, 1> corner( int i ) const {
        return m_aabb.corner( static_cast<Aabb::CornerType>( i ) );
    }

    /// Returns the position of the ith corner of the OBB ( world space )
    inline Eigen::Matrix<Scalar, 3, 1> worldCorner( int i ) const {
        return m_transform * m_aabb.corner( static_cast<Aabb::CornerType>( i ) );
    }

    /// Non-const access to the obb transformation
    Transform& transform() { return m_transform; }

    /// Const access to the obb transformation
    const Transform& transform() const { return m_transform; }

  private:
    /// The untransformed AABB
    Aabb m_aabb;
    /// Orientation of the box.
    Transform m_transform;
};
} // namespace Geometry
} // namespace Core
} // namespace Ra


//
// Created by david on 6/7/2018.
//

#include <random>
#include "ImageGraph.h"
#include <boost/graph/boykov_kolmogorov_max_flow.hpp>
#include "graph_cut.h"

namespace {
    using namespace Gadgetron;
    static std::mt19937 rng_state(4242);

    template<unsigned int D>
    void update_regularization_edge(ImageGraph<D> &graph, const hoNDArray<uint16_t> &field_map,
                                    const hoNDArray<uint16_t> &proposed_field_map,
                                    const hoNDArray<float> &second_deriv, const size_t idx, const size_t idx2,
                                    const size_t edge_idx, float scaling) {

        int f_value1 = field_map[idx];
        int pf_value1 = proposed_field_map[idx];
        int f_value2 = field_map[idx2];
        int pf_value2 = proposed_field_map[idx2];
        int a = std::norm(f_value1 - f_value2);
        int b = std::norm(f_value1 - pf_value2);
        int c = std::norm(pf_value1 - f_value2);
        int d = std::norm(pf_value1 - pf_value2);

        float weight = b + c - a - d;

        assert(weight >= 0);
        float lambda = std::max(std::min(second_deriv[idx], second_deriv[idx2]), 0.0f) * scaling;
        weight *= lambda;

        assert(lambda >= 0);

        auto &capacity_map = graph.edge_capacity_map;

        capacity_map[edge_idx] += weight;
        {
            float aq = lambda * (c - a);

            if (aq > 0) {
                capacity_map[graph.edge_from_source(idx)] += aq;

            } else {
                capacity_map[graph.edge_to_sink(idx)] -= aq;
            }
        }

        {
            float aj = lambda * (d - c);
            if (aj > 0) {
                capacity_map[graph.edge_from_source(idx2)] += aj;

            } else {
                capacity_map[graph.edge_to_sink(idx2)] -= aj;
            }
        }

    }

    template<unsigned int D>
    ImageGraph<D> make_graph(const hoNDArray<uint16_t> &field_map, const hoNDArray<uint16_t> &proposed_field_map,
                             const hoNDArray<float> &residual_diff_map, const hoNDArray<float> &second_deriv) {

        const auto dims = vector_td<int,3>(field_map.get_size(0),field_map.get_size(1),field_map.get_size(2));

        vector_td<int,D> graph_dims;
        for (int i = 0; i < D; i++) graph_dims[i] = dims[i];

        ImageGraph<D> graph = ImageGraph<D>(graph_dims);

        auto &capacity_map = graph.edge_capacity_map;
        //Add regularization edges

        for (size_t kz = 0; kz < dims[2]; kz++) {
            for (size_t ky = 0; ky < dims[1]; ky++) {
                for (size_t kx = 0; kx < dims[0]; kx++) {
                    size_t idx = kz*dims[1]*dims[0]+ky * dims[0] + kx;

                    if (kx < (dims[0] - 1)) {
                        size_t idx2 = idx + 1;

                        update_regularization_edge(graph, field_map, proposed_field_map, second_deriv, idx, idx2,
                                                   graph.edge(idx, idx2).first, 1);
                    }

                    if (ky < (dims[1] - 1)) {
                        size_t idx2 = idx + dims[0];
                        update_regularization_edge(graph, field_map, proposed_field_map, second_deriv, idx, idx2,
                                                   graph.edge(idx, idx2).first, 1);
                    }

                    if (kz < (dims[2] - 1)) {
                        size_t idx2 = idx + dims[0]*dims[1];
                        update_regularization_edge(graph, field_map, proposed_field_map, second_deriv, idx, idx2,
                                                   graph.edge(idx, idx2).first, 1);
                    }

                    float residual_diff = residual_diff_map[idx];

                    if (residual_diff > 0) {
                        capacity_map[graph.edge_to_sink(idx)] += int(residual_diff);

                    } else {
                        capacity_map[graph.edge_from_source(idx)] -= int(residual_diff);
                    }

                }
            }
        }

        return graph;
    }

    template<unsigned int DIMS>
    std::vector<boost::default_color_type>
    graph_cut(const hoNDArray<uint16_t> &field_map_index, const hoNDArray<uint16_t> &proposed_field_map_index,
              const hoNDArray<float> &lambda_map, const hoNDArray<float> &residual_diff_map) {

        ImageGraph<DIMS> graph = make_graph<DIMS>(field_map_index, proposed_field_map_index, residual_diff_map,
                                                  lambda_map);

        float flow = boost::boykov_kolmogorov_max_flow(graph, graph.source_vertex, graph.sink_vertex);

        return std::move(graph.color_map);
    }

}
namespace Gadgetron {

    hoNDArray<uint16_t>
    update_field_map(const hoNDArray<uint16_t> &field_map_index, const hoNDArray<uint16_t> &proposed_field_map_index,
                     const hoNDArray<float> &residuals_map, const hoNDArray<float> &lambda_map) {

        hoNDArray<float> residual_diff_map(field_map_index.dimensions());
        const auto X = field_map_index.get_size(0);
        const auto Y = field_map_index.get_size(1);
        const auto Z = field_map_index.get_size(2);

        for (size_t kz = 0; kz < Z; kz++) {
            for (size_t ky = 0; ky < Y; ky++) {
                for (size_t kx = 0; kx < X; kx++) {
                    residual_diff_map(kx, ky,kz) = residuals_map(field_map_index(kx, ky,kz), kx, ky,kz) -
                                                residuals_map(proposed_field_map_index(kx, ky,kz), kx, ky,kz);

                }
            }
        }

        std::vector<boost::default_color_type> color_map;
        if (Z == 1) {
            color_map = graph_cut<2>(field_map_index, proposed_field_map_index, lambda_map,
                                     residual_diff_map);
        } else {
            color_map = graph_cut<3>(field_map_index, proposed_field_map_index, lambda_map, residual_diff_map);
        }

        auto result = field_map_index;
        size_t updated_voxels = 0;
        for (size_t i = 0; i < field_map_index.get_number_of_elements(); i++) {
            if (color_map[i] != boost::default_color_type::black_color) {
                updated_voxels++;
                result[i] = proposed_field_map_index[i];
            }
        }

        return result;

    }

}


#include <boost/graph/make_maximal_planar.hpp>


#include <boost/graph/make_biconnected_planar.hpp>


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//         Copyright 2012 - 2014 MetaScale SAS
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_ARITHMETIC_FUNCTIONS_SIMD_COMMON_TOUINT_HPP_INCLUDED
#define BOOST_SIMD_ARITHMETIC_FUNCTIONS_SIMD_COMMON_TOUINT_HPP_INCLUDED

#include <boost/simd/arithmetic/functions/touint.hpp>
#include <boost/simd/include/functions/simd/bitwise_cast.hpp>
#include <boost/simd/include/functions/simd/if_else.hpp>
#include <boost/simd/include/functions/simd/splat.hpp>
#include <boost/simd/include/functions/simd/is_less.hpp>
#include <boost/simd/include/functions/simd/toint.hpp>
#include <boost/simd/include/functions/simd/minus.hpp>
#include <boost/simd/include/functions/simd/plus.hpp>
#include <boost/simd/include/functions/simd/inc.hpp>
#include <boost/simd/include/constants/valmax.hpp>
#include <boost/simd/sdk/meta/scalar_of.hpp>
#include <boost/dispatch/meta/as_integer.hpp>
#include <boost/dispatch/attributes.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_DISPATCH_IMPLEMENT          ( touint_, tag::cpu_
                                    , (A0)(X)
                                    , ((simd_<integer_<A0>,X>))
                                    )
  {
    typedef typename dispatch::meta::as_integer<A0, unsigned> ::type result_type;

    BOOST_FORCEINLINE result_type
    operator()(A0 const& a0) const
    {
      return bitwise_cast<result_type>(a0);
    }
  };

  BOOST_DISPATCH_IMPLEMENT          ( touint_, tag::cpu_
                                    , (A0)(X)
                                    , ((simd_<floating_<A0>,X>))
                                    )
  {
    typedef typename dispatch::meta::as_integer<A0, unsigned> ::type result_type;

    BOOST_FORCEINLINE result_type
    operator()(A0 const& a0) const
    {
      typedef typename dispatch::meta::as_integer<typename meta::scalar_of<A0>::type, signed>::type signed_integer;
      A0 sign_f = inc(splat<A0>(Valmax<signed_integer>()));
      result_type sign_i = inc(splat<result_type>(Valmax<signed_integer>()));
      return if_else(a0 < sign_f, bitwise_cast<result_type>(toint(a0)), bitwise_cast<result_type>(toint(a0 - sign_f)) + sign_i);
    }
  };
} } }

#endif


#include "QuaternionDmp.h"
#include "QuaternionDmpModel.h"
#include "QuaternionDmpConfig.h"
#include <assert.h>
#include <vector>
#include <yaml-cpp/yaml.h>
#include <Eigen/Core>
#include "EigenHelpers.h"
#include <Dmp.h>

namespace dmp
{

using namespace Eigen;

//if the quaternion dmp is used as part of another behavior the create and destroy
//functions should not be generated because the other behavior will generate them
//and there can only be one pair of create/destroy methods per library.
#ifdef BUILD_QUATERNION_STANDALONE
  DESTROY_LIB(QuaternionDmp);
  CREATE_LIB(QuaternionDmp);
#endif

QuaternionDmp::QuaternionDmp(lib_manager::LibManager* manager) :
        LoadableBehavior(manager, "QuaternionDmpBehavior", 1),
        initialized(false)
{
  //all behaviors must use lazy initialization, therefore the constructor
  //does nearly nothing
  LoadableBehavior::init(4, 4);
}

bool QuaternionDmp::initialize(const std::string &initialConfigPath)
{
  QuaternionDmpModel model;
  if(!model.from_yaml_file(initialConfigPath, ""))//FIXME find way to set name
  {
    return false;
  }
  return initialize(model);
}

bool QuaternionDmp::initialize(const QuaternionDmpModel &model)
{
  if(model.is_valid() && model.ft_weights.size() == 3)
  {
    name = model.model_name;

    startT = 0.0;
    goalT = model.cs_execution_time;

    alphaY = model.ts_alpha_z;
    betaY = model.ts_beta_z;
    alphaZ = model.cs_alpha;

    dt = model.cs_dt;
    assert(model.ts_dt == dt);
    assert(model.ts_tau == goalT);

    centers = EigenHelpers::toEigen(model.rbf_centers);
    widths = EigenHelpers::toEigen(model.rbf_widths);

    Dmp::initializeRbf(
        widths.data(), widths.rows(), centers.data(), centers.rows(),
        goalT, startT, 0.8, alphaZ
    );

    weights = EigenHelpers::toEigen(model.ft_weights);

    lastT = 0.0;
    t = 0.0;

    initialized = true;
    return true;
  }
  else
  {
    return false;
  }
}

bool QuaternionDmp::configureYaml(const std::string &yaml)
{
  if(config.fromYamlString(yaml, ""))//FIXME find way to set name
  {
    return configure(config);
  }
  else
  {
    return false;
  }
}

bool QuaternionDmp::configure(const std::string &configPath)
{
  if(config.fromYamlFile(configPath, ""))//FIXME find way to set name
  {
    return configure(config);
  }
  else
  {
    return false;
  }
}

bool QuaternionDmp::configure(const QuaternionDmpConfig &config)
{
  if(initialized && config.isValid())
  {
    startT = 0.0;
    goalT = config.executionTime;

    Eigen::VectorXd q0 = EigenHelpers::toEigen(config.startPosition);
    q0 /= q0.norm();
    Eigen::VectorXd q0d = EigenHelpers::toEigen(config.startVelocity);
    Eigen::VectorXd g = EigenHelpers::toEigen(config.endPosition);
    g /= g.norm();

    startData.resize(10);
    goalData.resize(10);
    lastData.resize(10);
    data.resize(10);

    setNumInputs(10);
    setNumOutputs(10);

    assert(4 == q0.size());
    assert(3 == q0d.size());
    assert(4 == g.size());

    EigenHelpers::assertNonNanInf(q0);
    EigenHelpers::assertNonNanInf(q0d);
    EigenHelpers::assertNonNanInf(g);

    startData.segment(0, 4) = q0;
    startData.segment(4, 3) = q0d;
    startData.segment(7, 3).setZero();

    goalData.segment(0, 4) = g;
    goalData.segment(4, 3).setZero();
    goalData.segment(7, 3).setZero();

    if(t <= 0.0)
    {
      lastData.segment(4, 3) = q0d;
      lastData.segment(7, 3).setZero();
    }

    return true;
  }
  else
  {
    return false;
  }
}

void QuaternionDmp::setInputs(const double *values, int numInputs)
{
  assert(initialized);
  assert(numInputs >= 4); //quaternions always have 4 elements

  lastData.segment<4>(0) = Map<const Array<double, 4, 1> >(values, 4);
  lastData.segment<4>(0) /= lastData.segment<4>(0).matrix().norm();
}

void QuaternionDmp::getOutputs(double *values, int numOutputs) const
{
  assert(initialized);
  assert(numOutputs >= 4);

  Map<Array<double, 4, 1> > map(values);
  map = data.segment<4>(0);

  // TODO That should be easier. Why does segment not work on the left side?
  Map<ArrayXd>((double*) (lastData.data() + 4), 6) = data.segment<6>(4);
}

void QuaternionDmp::step()
{
  assert(initialized);

  Dmp::quaternionDmpStep(
    lastT, t,
    lastData.data(), 4,
    lastData.data() + 4, 3,
    lastData.data() + 7, 3,
    data.data(), 4,
    data.data() + 4, 3,
    data.data() + 7, 3,
    goalData.data(), 4,
    goalData.data() + 4, 3,
    goalData.data() + 7, 3,
    startData.data(), 4,
    startData.data() + 4, 3,
    startData.data() + 7, 3,
    goalT, startT,
    weights.data(), weights.cols(), weights.rows(),
    widths.data(), widths.rows(),
    centers.data(), centers.rows(),
    alphaY, betaY, alphaZ, 0.001
  );

  lastT = t;
  t += dt;
}

bool QuaternionDmp::canStep() const
{
  return t <= goalT;
}

void QuaternionDmp::setWeights(const Eigen::ArrayXXd &weights)
{
  assert(weights.rows() == 3);
  this->weights = weights;
}

const Eigen::MatrixXd QuaternionDmp::getWeights()
{
  return weights.matrix();
}
}//end namespace


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

  @copyright 2016 NumScale SAS
  @copyright 2016 J.T. Lapreste

  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_COMMON_SIMD_FUNCTION_NEGS_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_SIMD_FUNCTION_NEGS_HPP_INCLUDED
#include <boost/simd/detail/overload.hpp>
#include <boost/simd/detail/overload.hpp>

#include <boost/simd/meta/hierarchy/simd.hpp>
#include <boost/simd/constant/valmin.hpp>
#include <boost/simd/function/simd/if_else.hpp>
#include <boost/simd/function/simd/is_equal.hpp>
#include <boost/simd/function/simd/unary_minus.hpp>

namespace boost { namespace simd { namespace ext
{
   namespace bd = boost::dispatch;
   namespace bs = boost::simd;
   BOOST_DISPATCH_OVERLOAD(negs_
                          , (typename A0, typename X)
                          , bd::cpu_
                          , bs::pack_<bd::signed_<A0>, X>
                          )
   {
      BOOST_FORCEINLINE A0 operator()( const A0& a0) const BOOST_NOEXCEPT
      {
        return if_else(is_equal(a0, Valmin<A0>()),Valmax<A0>(),unary_minus(a0));
      }
   };

   BOOST_DISPATCH_OVERLOAD(negs_
                          , (typename A0, typename X)
                          , bd::cpu_
                          , bs::pack_<bd::floating_<A0>, X>
                          )
   {
      BOOST_FORCEINLINE A0 operator()( const A0& a0) const BOOST_NOEXCEPT
      {
        return unary_minus(a0);
      }
   };

} } }

#endif


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_REDUCTION_FUNCTIONS_SIMD_SSE_SSE2_MAXIMUM_HPP_INCLUDED
#define BOOST_SIMD_REDUCTION_FUNCTIONS_SIMD_SSE_SSE2_MAXIMUM_HPP_INCLUDED
#ifdef BOOST_SIMD_HAS_SSE2_SUPPORT
#include <boost/simd/reduction/functions/maximum.hpp>
#include <boost/simd/sdk/meta/make_dependent.hpp>
#include <boost/simd/include/functions/simd/max.hpp>
#include <boost/simd/swar/functions/details/shuffle.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_SIMD_FUNCTOR_IMPLEMENTATION ( boost::simd::tag::maximum_
                                    , boost::simd::tag::sse2_, (A0)
                                    , ((simd_<ints16_<A0>,boost::simd::tag::sse_>))
                                    )
  {
    typedef typename meta::scalar_of<A0>::type result_type;

    BOOST_SIMD_FUNCTOR_CALL(1)
    {
      A0 max1 = _mm_shufflehi_epi16(a0  , _MM_SHUFFLE(1, 0, 3, 2));
         max1 = _mm_shufflelo_epi16(max1, _MM_SHUFFLE(1, 0, 3, 2));
         max1 = max(a0, max1);
      A0 max2 = _mm_shuffle_epi32  (max1, _MM_SHUFFLE(1, 0, 3, 2));
         max2 = _mm_shufflelo_epi16(max2, _MM_SHUFFLE(1, 0, 3, 2));
         max2 = max(max1, max2);
      A0 max3 = _mm_shuffle_epi32(max2, _MM_SHUFFLE(3, 2, 1, 0));
      A0 max4 = _mm_shufflelo_epi16(max3, _MM_SHUFFLE(0, 1, 2, 3));
      A0 that = boost::simd::max(max3, max4);
      return that[0];
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION ( boost::simd::tag::maximum_
                                    , boost::simd::tag::sse2_
                                    , (A0)
                                    , ((simd_<double_<A0>,boost::simd::tag::sse_>))
                                    )
  {
    typedef typename meta::scalar_of<A0>::type result_type;
    BOOST_SIMD_FUNCTOR_CALL(1)
    {
      A0 that = _mm_max_sd(a0, _mm_unpackhi_pd(a0,a0));
      return that[0];
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION ( boost::simd::tag::maximum_
                                    , boost::simd::tag::sse2_, (A0)
                                    , ((simd_<ints64_<A0>,boost::simd::tag::sse_>))
                                    )
  {
    typedef typename meta::scalar_of<A0>::type result_type;
    BOOST_SIMD_FUNCTOR_CALL(1)
    {
      return boost::simd::max(a0[0], a0[1]);
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION ( boost::simd::tag::maximum_
                                    , boost::simd::tag::sse2_
                                    , (A0)
                                    , ((simd_<single_<A0>,boost::simd::tag::sse_>))
                                    )
  {
    typedef typename meta::scalar_of<A0>::type result_type;
    BOOST_SIMD_FUNCTOR_CALL(1)
    {
      A0 max  = _mm_max_ps(a0, _mm_movehl_ps(a0,a0));
      A0 that = _mm_max_ss(max, _mm_shuffle_ps(max,max,0x01));
      return that[0];
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION ( boost::simd::tag::maximum_
                                    , boost::simd::tag::sse2_
                                    , (A0)
                                    , ((simd_<ints8_<A0>,boost::simd::tag::sse_>))
                                    )
  {
    typedef typename meta::scalar_of<A0>::type              result_type;
    typedef typename meta::make_dependent<double,A0>::type  double_t;
    typedef simd::native<double_t,boost::simd::tag::sse_>   rtype;

    BOOST_SIMD_FUNCTOR_CALL(1)
    {
      rtype v0 = simd::bitwise_cast<rtype>(a0);
      A0 pack = simd::bitwise_cast<A0>(_mm_unpackhi_pd(v0,v0));
      A0 max1 = boost::simd::max(a0,pack);
      A0 max2 = max ( max1
                    , simd::bitwise_cast<A0>( _mm_shufflelo_epi16 ( max1
                                                                  , _MM_SHUFFLE(0, 1, 2, 3)
                                                                  )
                                            )
                    );

         max2 = max ( max2
                    , simd::bitwise_cast<A0>( _mm_shuffle_epi32 ( max2
                                                                , _MM_SHUFFLE(2, 3, 0, 1)
                                                                )
                                            )
                    );

      return boost::simd::max(max2[0],max2[1]);
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION ( boost::simd::tag::maximum_
                                    , boost::simd::tag::sse2_
                                    , (A0)
                                    , ((simd_<ints32_<A0>,boost::simd::tag::sse_>))
                                    )
  {
    typedef typename meta::scalar_of<A0>::type result_type;
    BOOST_SIMD_FUNCTOR_CALL(1)
    {
      A0 max1 = max(a0,simd::bitwise_cast<A0>(_mm_shuffle_epi32(a0, _MM_SHUFFLE(1, 0, 3, 2))));
      A0 that = max(max1, simd::bitwise_cast<A0>(_mm_shuffle_epi32(max1, _MM_SHUFFLE(2, 3, 0, 1))));
      return that[0];
    }
  };
} } }

#endif
#endif


// -------------- test the visual odometry -------------
#include <fstream>
#include <boost/timer.hpp>
#include <opencv2/imgcodecs.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/viz.hpp>
#include "opencv2/opencv.hpp"
#include "myslam/config.h"
#include "myslam/visual_odometry.h"
#include "myslam/frame.h"
#define _DEBUG
#define name2str(name) (#name)

int main ( int argc, char** argv )
{
    if ( argc != 2 )
    {
        cout<<"usage: run_vo parameter_file"<<endl;
        return 1;
    }

    myslam::Config::setParameterFile ( argv[1] );
    myslam::VisualOdometry::Ptr vo ( new myslam::VisualOdometry );

//
    string dataset_dir = myslam::Config::get<string> ( "dataset_dir" );
    cout<<"dataset: "<<dataset_dir<<endl;
    ifstream fin_1 ( dataset_dir+"/associate.txt" );
    if ( !fin_1 )
    {
        cout<<"please generate the associate file called associate.txt!"<<endl;
        return 1;
    }

    vector<string> rgb_files, depth_files;
    vector<double> rgb_times, depth_times;
    while ( !fin_1.eof() )
    {
        string rgb_time, rgb_file, depth_time, depth_file;
        fin_1>>rgb_time>>rgb_file>>depth_time>>depth_file;
        rgb_times.push_back ( atof ( rgb_time.c_str() ) );
        depth_times.push_back ( atof ( depth_time.c_str() ) );
        rgb_files.push_back ( dataset_dir+"/"+rgb_file );
        depth_files.push_back ( dataset_dir+"/"+depth_file );

        if ( fin_1.good() == false )
            break;
    }

//    vector<cv::Mat> rgb_images ;
//    string s1;
//
//    s1 = "/home/wzy/slambook/ch9/0.2(version0.1)/rgbsmall";
//   Mat rgb_image;
//
//   // cout<<"rgb_dataset"<<ss<<endl;
//    for (int j = 1; j < 8 ; ++j)
//    {
//
//        string s2;
//        stringstream ss2;
//        ss2 << j;
//        ss2 >> s2;
//        rgb_image = imread(s1 + "/" + s2 + ".png");
//        rgb_images.push_back(rgb_image);
//        cout<<"name"<<" "<<s2<<endl;
//        imshow(s2,rgb_image);
//
//    }

    //myslam::Camera::Ptr camera ( new myslam::Camera );

    // visualization
    cv::viz::Viz3d vis("Visual Odometry");
    cv::viz::WCoordinateSystem world_coor(1.0), camera_coor(0.5);
    cv::Point3d cam_pos( 0, -1.0, -1.0 ), cam_focal_point(0,0,0), cam_y_dir(0,1,0);
    cv::Affine3d cam_pose = cv::viz::makeCameraPose( cam_pos, cam_focal_point, cam_y_dir );
    vis.setViewerPose( cam_pose );

    world_coor.setRenderingProperty(cv::viz::LINE_WIDTH, 2.0);
    camera_coor.setRenderingProperty(cv::viz::LINE_WIDTH, 1.0);
    vis.showWidget( "World", world_coor );
    vis.showWidget( "Camera", camera_coor );

    cout<<"read total "<<rgb_files.size() <<" entries"<<endl;

//I define Tcw to vis show

    for ( int i=0; i<rgb_files.size(); i++ )
    {

        Mat color =  imread(rgb_files[i]);

        //我自己加的检测2幅图像不同
        double avg = 0.0;
        double stddev = 0.0;
        Mat src;
        cvtColor(color,src,CV_BGR2GRAY);
        Mat mean;//平均灰度
        Mat stdDev;//灰度方差
        meanStdDev(src,mean,stdDev);
        avg = mean.ptr<double>(0)[0];
        stdDev = stdDev.ptr<double>(0)[0];
        cout<<"avg = "<< avg<<"stdDev = "<<stdDev<<endl;

        if ( color.data==nullptr )
            break;
        myslam::Frame::Ptr pFrame = myslam::Frame::createFrame();
        //pFrame->camera_ = camera;
        pFrame->color_ = color;

        cout<<"receive a frame"<<endl;
        boost::timer timer;
        vo->addFrame ( pFrame );
        cout<<"VO costs time: "<<timer.elapsed()<<endl;

        if ( vo->state_ == myslam::VisualOdometry::LOST )
            break;
//        //SE3 Tcw = pFrame->T_c_w_.inverse();//Tcw就是变换矩阵T，可以由T得出旋转矩阵R和位移t。
//

//        // show the map and the camera pose
//cv::Affine3d M(
//        cv::Affine3d::Mat3 (
//        pFrame->R.at<float>(0,0),pFrame->R.at<float>(0,1),pFrame->R.at<float>(0,2),
//        pFrame->R.at<float>(1,0),pFrame->R.at<float>(1,1),pFrame->R.at<float>(1,2),
//        pFrame->R.at<float>(2,0),pFrame->R.at<float>(2,1),pFrame->R.at<float>(2,2)
//        ),
//        cv::Affine3d::Vec3(
//        pFrame->t(0),pFrame->t(1),pFrame->t(2)
//        )
//        );

        //convert from Mat to Matx
//        float* dataR = reinterpret_cast<float*>(pFrame->R.data);
//        Matx33f converted(dataR);

//cv::Affine3f T(pFrame->R,pFrame->t);

//        pFrame->RR = RRR;
//<<(
//        pFrame->R.at<double>(0,0), pFrame->R.at<double>(0,1), pFrame->R.at<double>(0,2),
//        pFrame->R.at<double>(1,0), pFrame->R.at<double>(1,1), pFrame->R.at<double>(1,2),
//        pFrame->R.at<double>(2,0), pFrame->R.at<double>(2,1), pFrame->R.at<double>(2,2)
//);
//        cv::Matx33f RRR(
//                pFrame->R.at<float>(0,0), pFrame->R.at<float>(0,1), pFrame->R.at<float>(0,2),
//                pFrame->R.at<float>(1,0), pFrame->R.at<float>(1,1), pFrame->R.at<float>(1,2),
//                pFrame->R.at<float>(2,0), pFrame->R.at<float>(2,1), pFrame->R.at<float>(2,2));

//        cv::Vec3f ttt(pFrame->t.at<float>(0,0),pFrame->t.at<float>(1,0),pFrame->t.at<float>(2,0));

//        cv::Affine3d M(cv::Affine3d::Mat3(
//                pFrame->R.at<float>(0,0), pFrame->R.at<float>(0,1), pFrame->R.at<float>(0,2),
//                pFrame->R.at<float>(1,0), pFrame->R.at<float>(1,1), pFrame->R.at<float>(1,2),
//                pFrame->R.at<float>(2,0), pFrame->R.at<float>(2,1), pFrame->R.at<float>(2,2)
//                ),
//                 cv::Affine3d::Vec3(
//                 pFrame->t.at<float>(0,0),pFrame->t.at<float>(1,0),pFrame->t.at<float>(2,0)
//                         )
//                     );

//
//        Eigen::Matrix3d RE;
//        RE << pFrame->R.at<double>(0,0), pFrame->R.at<double>(0,1), pFrame->R.at<double>(0,2),
//              pFrame->R.at<double>(1,0), pFrame->R.at<double>(1,1), pFrame->R.at<double>(1,2),
//              pFrame->R.at<double>(2,0), pFrame->R.at<double>(2,1), pFrame->R.at<double>(2,2);
//
//        Tcw = SE3(SO3(RE),Vector3d(pFrame->t.at<double>(0,0),pFrame->t.at<double>(1,0),pFrame->t.at<double>(2,0)));

        SE3 Tcw = pFrame->T_c_w_.inverse();
        cv::Affine3d M(
                cv::Affine3d::Mat3(
                        Tcw.rotation_matrix()(0,0), Tcw.rotation_matrix()(0,1), Tcw.rotation_matrix()(0,2),
                        Tcw.rotation_matrix()(1,0), Tcw.rotation_matrix()(1,1), Tcw.rotation_matrix()(1,2),
                        Tcw.rotation_matrix()(2,0), Tcw.rotation_matrix()(2,1), Tcw.rotation_matrix()(2,2)
                ),
                cv::Affine3d::Vec3(
                        Tcw.translation()(0,0), Tcw.translation()(1,0), Tcw.translation()(2,0)
                )
        );

        cv::imshow("image", color );
        cv::waitKey(1);
        vis.setWidgetPose( "Camera",M);
        vis.spinOnce(1, false);
      }

    return 0;
}


//==================================================================================================
/*
  Copyright 2017 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)
*/
//==================================================================================================
//! [min_val]
#include <boost/simd/algorithm/min_val.hpp>
#include <boost/simd/function/is_greater.hpp>
#include <iostream>
#include <vector>

int main()
{
  namespace bs =  boost::simd;
  std::vector<float> d{ 0.0f, 2.0f, 0.0f, 0.0f, -1.0f, 2.0f, 0.0f};

  auto r1 = bs::min_val( d.data(), d.data()+7);
  std::cout << "r1 " << r1<< "\n";
  auto r2 = bs::min_val( d.data(), d.data()+7, bs::is_greater);
  std::cout << "r2 " << r2  << "\n";

  return 0;
}
//! [min_val]


/*
 * NormalColorMapFilter.cpp
 *
 *  Created on: Aug 22, 2017
 *      Author: Peter Fankhauser
 *   Institute: ETH Zurich, ANYbotics
 */

#include "grid_map_filters/NormalColorMapFilter.hpp"

#include <Eigen/Dense>

#include <grid_map_core/grid_map_core.hpp>

using namespace filters;

namespace grid_map {

NormalColorMapFilter::NormalColorMapFilter() = default;

NormalColorMapFilter::~NormalColorMapFilter() = default;

bool NormalColorMapFilter::configure() {
  if (!FilterBase::getParam(std::string("input_layers_prefix"), inputLayersPrefix_)) {
    ROS_ERROR("Normal color map filter did not find parameter `input_layers_prefix`.");
    return false;
  }
  ROS_DEBUG("Normal color map filter input layers prefix is = %s.", inputLayersPrefix_.c_str());

  if (!FilterBase::getParam(std::string("output_layer"), outputLayer_)) {
    ROS_ERROR("Normal color map filter did not find parameter `output_layer`.");
    return false;
  }
  ROS_DEBUG("Normal color map filter output_layer = %s.", outputLayer_.c_str());
  return true;
}

bool NormalColorMapFilter::update(const GridMap& mapIn, GridMap& mapOut) {
  const auto& normalX = mapIn[inputLayersPrefix_ + "x"];
  const auto& normalY = mapIn[inputLayersPrefix_ + "y"];
  const auto& normalZ = mapIn[inputLayersPrefix_ + "z"];

  mapOut = mapIn;
  mapOut.add(outputLayer_);
  auto& color = mapOut[outputLayer_];

  // X: -1 to +1 : Red: 0 to 255
  // Y: -1 to +1 : Green: 0 to 255
  // Z:  0 to  1 : Blue: 128 to 255

  // For each cell in map.
  for (Eigen::Index i = 0; i < color.size(); ++i) {
    const Eigen::Vector3f colorVector((normalX(i) + 1.0) / 2.0, (normalY(i) + 1.0) / 2.0, (normalZ(i) / 2.0) + 0.5);
    colorVectorToValue(colorVector, color(i));
  }

  return true;
}

}  // namespace grid_map


//==================================================================================================
/**
  EVE - Expressive Vector Engine
  Copyright : EVE Contributors & Maintainers
  SPDX-License-Identifier: MIT
**/
//==================================================================================================
#include "test.hpp"
#include <eve/function/cyl_bessel_j0.hpp>
#include <eve/function/prev.hpp>
#include <eve/constant/inf.hpp>
#include <eve/constant/minf.hpp>
#include <eve/constant/nan.hpp>
#include <eve/platform.hpp>
#include <cmath>
#include <boost/math/special_functions/bessel.hpp>

EVE_TEST_TYPES( "Check return types of cyl_bessel_j0"
            , eve::test::simd::ieee_reals
            )
<typename T>(eve::as<T>)
{
  using v_t = eve::element_type_t<T>;
  TTS_EXPR_IS(eve::cyl_bessel_j0(T(0)), T);
  TTS_EXPR_IS(eve::cyl_bessel_j0(v_t(0)), v_t);
};

 EVE_TEST( "Check behavior of cyl_bessel_j0 on wide"
        , eve::test::simd::ieee_reals
        , eve::test::generate(eve::test::randoms(0.0, 20000.0))
         )
   <typename T>(T const& a0)
{
  using v_t = eve::element_type_t<T>;

  auto eve__cyl_bessel_j0 =  [](auto x) { return eve::cyl_bessel_j0(x); };
  auto std__cyl_bessel_j0 =  [](auto x)->v_t { return boost::math::cyl_bessel_j(v_t(0), x); };
  if constexpr( eve::platform::supports_invalids )
  {
    TTS_ULP_EQUAL(eve__cyl_bessel_j0(eve::minf(eve::as<v_t>())), eve::zero(eve::as<v_t>()), 0);
    TTS_ULP_EQUAL(eve__cyl_bessel_j0(eve::inf(eve::as<v_t>())), v_t(0), 0);
    TTS_ULP_EQUAL(eve__cyl_bessel_j0(eve::nan(eve::as<v_t>())), eve::nan(eve::as<v_t>()), 0);
    TTS_ULP_EQUAL(eve__cyl_bessel_j0(eve::minf(eve::as< T>())), eve::zero(eve::as< T>()), 0);
    TTS_ULP_EQUAL(eve__cyl_bessel_j0(eve::inf(eve::as< T>())),  T(0), 0);
    TTS_ULP_EQUAL(eve__cyl_bessel_j0(eve::nan(eve::as< T>())), eve::nan(eve::as< T>()), 0);
  }
  TTS_ULP_EQUAL(eve__cyl_bessel_j0(v_t(1500)), std__cyl_bessel_j0(v_t(1500)), 2.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_j0(v_t(500)), std__cyl_bessel_j0(v_t(500)), 2.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_j0(v_t(10)), std__cyl_bessel_j0(v_t(10))  , 2.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_j0(v_t(5)),  std__cyl_bessel_j0(v_t(5))   , 2.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_j0(v_t(2)),  std__cyl_bessel_j0(v_t(2))   , 2.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_j0(v_t(1.5)),std__cyl_bessel_j0(v_t(1.5)) , 2.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_j0(v_t(0.5)),std__cyl_bessel_j0(v_t(0.5)) , 2.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_j0(v_t(1)),  std__cyl_bessel_j0(v_t(1))   , 2.5);
  TTS_ULP_EQUAL(eve__cyl_bessel_j0(v_t(0)),  eve::one(eve::as<v_t>()), 0.0);

  TTS_ULP_EQUAL(eve__cyl_bessel_j0( T(1500)), T(std__cyl_bessel_j0(v_t(1500))), 2.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_j0( T(500)),  T(std__cyl_bessel_j0(v_t(500)) ), 2.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_j0( T(10)) ,  T(std__cyl_bessel_j0( v_t(10)) ), 2.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_j0( T(5))  ,  T(std__cyl_bessel_j0( v_t(5))  ), 2.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_j0( T(2))  ,  T(std__cyl_bessel_j0( v_t(2))  ), 2.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_j0( T(1.5)),  T(std__cyl_bessel_j0( v_t(1.5))), 2.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_j0( T(0.5)),  T(std__cyl_bessel_j0( v_t(0.5))), 2.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_j0( T(1))  ,  T(std__cyl_bessel_j0( v_t(1))  ), 2.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_j0( T(0))  , eve::one(eve::as< T>()), 0.0);

  TTS_ULP_EQUAL(eve__cyl_bessel_j0(a0), map(std__cyl_bessel_j0, a0), 5.0);
};