//   Copyright 2015 Patrick Putnam
//
//   Licensed under the Apache License, Version 2.0 (the "License");
//   you may not use this file except in compliance with the License.
//   You may obtain a copy of the License at
//
//       http://www.apache.org/licenses/LICENSE-2.0
//
//   Unless required by applicable law or agreed to in writing, software
//   distributed under the License is distributed on an "AS IS" BASIS,
//   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
//   See the License for the specific language governing permissions and
//   limitations under the License.
#ifndef CLOTHO_SMALL_CROSSOVER_EVENT_GENERATOR_HPP_
#define CLOTHO_SMALL_CROSSOVER_EVENT_GENERATOR_HPP_

#include <boost/property_tree/ptree.hpp>
#include <boost/random/bernoulli_distribution.hpp>
#include <cstring>
#include <iostream>

#include "clotho/recombination/recombination_rate_parameter.hpp"
#include "clotho/recombination/sequence_bias_parameter.hpp"
#include "clotho/data_spaces/generators/position_distribution_helper.hpp"
#include "clotho/data_spaces/generators/crossover_event_distribution_helper.hpp"

#ifdef DEBUGGING
#define ASSERT_VALID_RANGE( x, min, max ) assert( min <= x && x < max );
#else
#define ASSERT_VALID_RANGE( x, min, max )
#endif

namespace clotho {
namespace genetics {

template < class RNG, class PositionType >
class small_crossover_event_generator {
public:
    typedef RNG             random_engine_type;
    typedef PositionType    position_type;
    typedef PositionType    event_type;
    typedef unsigned int    bin_type;

    typedef typename position_distribution_helper< PositionType >::type position_distribution_type;

    typedef crossover_event_distribution_helper< double > event_distribution_helper_type;

    typedef boost::random::bernoulli_distribution< double > sequence_bias_distribution_type;

    typedef typename event_distribution_helper_type::IntType    IntType;
    typedef typename event_distribution_helper_type::type       event_distribution_type;

    typedef position_type *                         position_vector;
    typedef event_type *                            event_vector;
    typedef bin_type *                              bin_vector;

    typedef small_crossover_event_generator< RNG, PositionType > self_type;

    static const unsigned int BUFFER_MAX = 64;

    small_crossover_event_generator( random_engine_type * rng, double rho, double bias, position_vector pos, unsigned int N ) :
        m_rand( rng )
        , m_pos( pos )
        , m_pos_size( N )
        , m_events_size(0)
        , m_event_dist( rho )
        , m_seq_bias( bias )
    {}

    small_crossover_event_generator( random_engine_type * rng, boost::property_tree::ptree & config ) :
        m_rand( rng )
        , m_pos( NULL )
        , m_pos_size(0)
        , m_events_size(0)
        , m_base_seq(0)
    {
        recombination_rate_parameter< double > rho( config);

        m_event_dist.param( typename event_distribution_type::param_type( rho.m_rho ) );

        sequence_bias_parameter< double > bias( config );
        m_seq_bias.param( typename sequence_bias_distribution_type::param_type( bias.m_bias ) );
    }

    small_crossover_event_generator( const self_type & other ) :
        m_rand( other.m_rand )
        , m_pos( other.m_pos )
        , m_pos_size( other.m_pos_size )
        , m_events_size( other.m_events_size )
        , m_base_seq( other.m_base_seq )
    {   }

    void update( position_vector pos, size_t N ) {
        m_pos = pos;
        m_pos_size = N;
    }

    // generate a new event distribution
    size_t generate() {
        // clear the counts
        IntType N = m_event_dist( *m_rand );    // generate the maximum number of events
        resize( N );

        setBaseSequence( m_seq_bias( *m_rand ) );

        position_type accum = 0.0;

        size_t i = 0;
        position_type e = (position_type)(N);
        while( e > (position_type) 0 ) {
            position_type p = m_pos_dist( *m_rand );

            while( p == 0.0 ) { p = m_pos_dist( *m_rand ); }

            accum += log( p ) / e;

            p = (1.0 - exp( accum ));

            assert( i == 0 || m_events[ i - 1 ] < p);

            m_events[ i++ ] = p;
            e -= 1.0;
        }

        return N;
    }

    // test whether the genetic position at the given index
    //
    inline bool operator()( const size_t index ) const {
#ifdef DEBUGGING
        if( index >= m_pos_size ) {
            std::cerr << "Crossover: " << index << " < " << m_pos_size << std::endl;
            assert(false);
        }
#endif

        position_type p = m_pos[ index ];
        return eval( p );
    }

    inline bool eval( position_type p ) const {

        bool res = false;
        if( m_events_size == 1 ) {
            res = ( m_events[0] < p );
        } else {
            size_t lo = 0;
            while( lo < m_events_size && m_events[lo] < p ){ ++lo; }
            res = ((lo & 1) ? true : false); // % 2
        }

        return res;
    }

    void setBaseSequence( bool is_base ) {
        m_base_seq = ((is_base) ? 0 : 1);
    }

    size_t getBaseSequence() const {
        return m_base_seq;
    }

    virtual ~small_crossover_event_generator() { }

protected:

    void resize( size_t e ) {
        assert( e < BUFFER_MAX );
        m_events_size = e;
    }

    random_engine_type          * m_rand;
    event_distribution_type     m_event_dist;
    position_distribution_type  m_pos_dist;

    position_vector     m_pos;
    size_t              m_pos_size;

    event_type          m_events[ BUFFER_MAX ];
    size_t              m_events_size;

    sequence_bias_distribution_type m_seq_bias;
    size_t              m_base_seq;
};

#undef ASSERT_VALID_RANGE

}   // namespace genetics
}   // namespace clotho

#endif  // CLOTHO_SMALL_CROSSOVER_EVENT_GENERATOR_HPP_


//==================================================================================================
/*!
    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_SSE2_SIMD_FUNCTION_NEARBYINT_HPP_INCLUDED
#define BOOST_SIMD_ARCH_X86_SSE2_SIMD_FUNCTION_NEARBYINT_HPP_INCLUDED

#include <boost/simd/detail/overload.hpp>
#include <boost/simd/function/tofloat.hpp>
#include <boost/simd/function/inearbyint.hpp>

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

  BOOST_DISPATCH_OVERLOAD ( nearbyint_
                          , (typename A0)
                          , bs::sse2_
                          , bs::raw_tag
                          , bs::pack_<bd::floating_<A0>, bs::sse_>
                         )
  {
    BOOST_FORCEINLINE A0 operator()(raw_tag const&, const A0 & a0) const BOOST_NOEXCEPT
    {
      return tofloat(inearbyint(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_TOOLBOX_OPERATOR_FUNCTIONS_SIMD_SSE_SSE2_IS_NOT_EQUAL_HPP_INCLUDED
#define BOOST_SIMD_TOOLBOX_OPERATOR_FUNCTIONS_SIMD_SSE_SSE2_IS_NOT_EQUAL_HPP_INCLUDED
#ifdef BOOST_SIMD_HAS_SSE2_SUPPORT
#include <boost/simd/toolbox/operator/functions/is_not_equal.hpp>
#include <boost/simd/include/functions/simd/bitwise_cast.hpp>
#include <boost/simd/include/functions/simd/complement.hpp>
#include <boost/simd/include/functions/simd/is_equal.hpp>
#include <boost/simd/include/functions/simd/bitwise_or.hpp>
#include <boost/simd/include/functions/simd/minus.hpp>
#include <boost/simd/include/constants/zero.hpp>
#include <boost/simd/sdk/simd/logical.hpp>
#include <boost/dispatch/meta/downgrade.hpp>
#include <boost/simd/toolbox/swar/functions/details/shuffle.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::is_not_equal_, boost::simd::tag::sse2_
                            , (A0)
                            , ((simd_<double_<A0>,boost::simd::tag::sse_>))
                              ((simd_<double_<A0>,boost::simd::tag::sse_>))
                            )
  {
    typedef typename meta::as_logical<A0>::type result_type;
    BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    {
      return _mm_cmpneq_pd(a0,a1);
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::is_not_equal_, boost::simd::tag::sse2_
                            , (A0)
                            , ((simd_<single_<A0>,boost::simd::tag::sse_>))
                              ((simd_<single_<A0>,boost::simd::tag::sse_>))
                            )
  {
    typedef typename meta::as_logical<A0>::type result_type;
    BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    {
      return _mm_cmpneq_ps(a0,a1);
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::is_not_equal_, boost::simd::tag::sse2_
                            , (A0)
                            , ((simd_<integer_<A0>,boost::simd::tag::sse_>))
                              ((simd_<integer_<A0>,boost::simd::tag::sse_>))
                            )
  {
    typedef typename meta::as_logical<A0>::type result_type;
    BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    {
      return bitwise_cast<result_type>(boost::simd::complement(bitwise_cast<A0>(boost::simd::eq(a0,a1))));
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::is_not_equal_, boost::simd::tag::sse2_
                            , (A0)
                            , ((simd_<ints64_<A0>,boost::simd::tag::sse_>))
                              ((simd_<ints64_<A0>,boost::simd::tag::sse_>))
                            )
  {
    typedef typename meta::as_logical<A0>::type result_type;
    BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    {
      typedef typename dispatch::meta::downgrade<A0, unsigned>::type  type;
      type tmp      = bitwise_cast<type>(a0 - a1);
      tmp           = bitwise_cast<type>(boost::simd::neq(tmp,Zero<type>()));
      type tmp2     = details::shuffle<1,0,3,2>(tmp);
      return bitwise_cast<result_type>(tmp | tmp2);
    }
  };
} } }

#endif
#endif


///////////////////////////////////////////////////////////////////////////////
//  Copyright 2018 John Maddock. 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_MP_EIGEN_HPP
#define BOOST_MP_EIGEN_HPP

#include <nil/crypto3/multiprecision/number.hpp>

#include <Eigen/Core>

//
// Generic Eigen support code:
//
namespace Eigen {
    template<class Backend, nil::crypto3::multiprecision::expression_template_option ExpressionTemplates>
    struct NumTraits<nil::crypto3::multiprecision::number<Backend, ExpressionTemplates>> {
        using self_type = nil::crypto3::multiprecision::number<Backend, ExpressionTemplates>;
        using Real = typename nil::crypto3::multiprecision::scalar_result_from_possible_complex<self_type>::type;
        using NonInteger = self_type;    // Not correct but we can't do much better??
        using Literal = double;
        using Nested = self_type;
        enum {
            IsComplex = nil::crypto3::multiprecision::number_category<self_type>::value ==
                        nil::crypto3::multiprecision::number_kind_complex,
            IsInteger = nil::crypto3::multiprecision::number_category<self_type>::value ==
                        nil::crypto3::multiprecision::number_kind_integer,
            ReadCost = 1,
            AddCost = 4,
            MulCost = 8,
            IsSigned =
                std::numeric_limits<self_type>::is_specialized ? std::numeric_limits<self_type>::is_signed : true,
            RequireInitialization = 1,
        };
        static Real epsilon() {
            return std::numeric_limits<Real>::epsilon();
        }
        static Real dummy_precision() {
            return 1000 * epsilon();
        }
        static Real highest() {
            return (std::numeric_limits<Real>::max)();
        }
        static Real lowest() {
            return (std::numeric_limits<Real>::min)();
        }
        static int digits10_imp(const std::integral_constant<bool, true>&) {
            return std::numeric_limits<Real>::digits10;
        }
        template<bool B>
        static int digits10_imp(const std::integral_constant<bool, B>&) {
            return Real::default_precision();
        }
        static int digits10() {
            return digits10_imp(
                std::integral_constant < bool,
                std::numeric_limits<Real>::digits10 && (std::numeric_limits<Real>::digits10 != INT_MAX) ? true :
                                                                                false > ());
        }
    };
    template<class tag, class Arg1, class Arg2, class Arg3, class Arg4>
    struct NumTraits<nil::crypto3::multiprecision::detail::expression<tag, Arg1, Arg2, Arg3, Arg4>>
        : public NumTraits<
              typename nil::crypto3::multiprecision::detail::expression<tag, Arg1, Arg2, Arg3, Arg4>::result_type> { };

#define BOOST_MP_EIGEN_SCALAR_TRAITS_DECL(A)                                                                        \
    template<class Backend, nil::crypto3::multiprecision::expression_template_option ExpressionTemplates,           \
             typename BinaryOp>                                                                                \
    struct ScalarBinaryOpTraits<nil::crypto3::multiprecision::number<Backend, ExpressionTemplates>, A, BinaryOp> {  \
        /*static_assert(nil::crypto3::multiprecision::is_compatible_arithmetic_type<A,                              \
         * nil::crypto3::multiprecision::number<Backend, ExpressionTemplates> >::value, "Interoperability with this \
         * arithmetic type is not supported.");*/                                                                   \
        using ReturnType = nil::crypto3::multiprecision::number<Backend, ExpressionTemplates>;                      \
    };                                                                                \
    template<class Backend, nil::crypto3::multiprecision::expression_template_option ExpressionTemplates,           \
             typename BinaryOp>                                                                                \
    struct ScalarBinaryOpTraits<A, nil::crypto3::multiprecision::number<Backend, ExpressionTemplates>, BinaryOp> {  \
        /*static_assert(nil::crypto3::multiprecision::is_compatible_arithmetic_type<A,                              \
         * nil::crypto3::multiprecision::number<Backend, ExpressionTemplates> >::value, "Interoperability with this \
         * arithmetic type is not supported.");*/                                                                   \
        using ReturnType = nil::crypto3::multiprecision::number<Backend, ExpressionTemplates>;                      \
    };

    BOOST_MP_EIGEN_SCALAR_TRAITS_DECL(float)
    BOOST_MP_EIGEN_SCALAR_TRAITS_DECL(double)
    BOOST_MP_EIGEN_SCALAR_TRAITS_DECL(long double)
    BOOST_MP_EIGEN_SCALAR_TRAITS_DECL(char)
    BOOST_MP_EIGEN_SCALAR_TRAITS_DECL(unsigned char)
    BOOST_MP_EIGEN_SCALAR_TRAITS_DECL(signed char)
    BOOST_MP_EIGEN_SCALAR_TRAITS_DECL(short)
    BOOST_MP_EIGEN_SCALAR_TRAITS_DECL(unsigned short)
    BOOST_MP_EIGEN_SCALAR_TRAITS_DECL(int)
    BOOST_MP_EIGEN_SCALAR_TRAITS_DECL(unsigned int)
    BOOST_MP_EIGEN_SCALAR_TRAITS_DECL(long)
    BOOST_MP_EIGEN_SCALAR_TRAITS_DECL(unsigned long)

#if 0
      template<class Backend, nil::crypto3::multiprecision::expression_template_option ExpressionTemplates, class Backend2, nil::crypto3::multiprecision::expression_template_option ExpressionTemplates2, typename BinaryOp>
   struct ScalarBinaryOpTraits<nil::crypto3::multiprecision::number<Backend, ExpressionTemplates>, nil::crypto3::multiprecision::number<Backend2, ExpressionTemplates2>, BinaryOp>
   {
      static_assert(
         nil::crypto3::multiprecision::is_compatible_arithmetic_type<nil::crypto3::multiprecision::number<Backend2, ExpressionTemplates2>, nil::crypto3::multiprecision::number<Backend, ExpressionTemplates> >::value
         || nil::crypto3::multiprecision::is_compatible_arithmetic_type<nil::crypto3::multiprecision::number<Backend, ExpressionTemplates>, nil::crypto3::multiprecision::number<Backend2, ExpressionTemplates2> >::value, "Interoperability with this arithmetic type is not supported.");
      using ReturnType = typename std::conditional<std::is_convertible<nil::crypto3::multiprecision::number<Backend2, ExpressionTemplates2>, nil::crypto3::multiprecision::number<Backend, ExpressionTemplates> >::value,
         nil::crypto3::multiprecision::number<Backend, ExpressionTemplates>, nil::crypto3::multiprecision::number<Backend2, ExpressionTemplates2> >::type;
   };

   template<unsigned D, typename BinaryOp>
   struct ScalarBinaryOpTraits<nil::crypto3::multiprecision::number<nil::crypto3::multiprecision::backends::mpc_complex_backend<D>, nil::crypto3::multiprecision::et_on>, nil::crypto3::multiprecision::mpfr_float, BinaryOp>
   {
      using ReturnType = nil::crypto3::multiprecision::number<nil::crypto3::multiprecision::backends::mpc_complex_backend<D>, nil::crypto3::multiprecision::et_on>;
   };

   template<typename BinaryOp>
   struct ScalarBinaryOpTraits<nil::crypto3::multiprecision::mpfr_float, nil::crypto3::multiprecision::mpc_complex, BinaryOp>
   {
      using ReturnType = nil::crypto3::multiprecision::number<nil::crypto3::multiprecision::backends::mpc_complex_backend<0>, nil::crypto3::multiprecision::et_on>;
   };

   template<class Backend, nil::crypto3::multiprecision::expression_template_option ExpressionTemplates, typename BinaryOp>
   struct ScalarBinaryOpTraits<nil::crypto3::multiprecision::number<Backend, ExpressionTemplates>, nil::crypto3::multiprecision::number<Backend, ExpressionTemplates>, BinaryOp>
   {
      using ReturnType = nil::crypto3::multiprecision::number<Backend, ExpressionTemplates>;
   };
#endif

    template<class Backend, nil::crypto3::multiprecision::expression_template_option ExpressionTemplates, class tag,
             class Arg1, class Arg2, class Arg3, class Arg4, typename BinaryOp>
    struct ScalarBinaryOpTraits<nil::crypto3::multiprecision::number<Backend, ExpressionTemplates>,
                                nil::crypto3::multiprecision::detail::expression<tag, Arg1, Arg2, Arg3, Arg4>,
                                BinaryOp> {
        static_assert(
            std::is_convertible<
                typename nil::crypto3::multiprecision::detail::expression<tag, Arg1, Arg2, Arg3, Arg4>::result_type,
                nil::crypto3::multiprecision::number<Backend, ExpressionTemplates>>::value,
            "Interoperability with this arithmetic type is not supported.");
        using ReturnType = nil::crypto3::multiprecision::number<Backend, ExpressionTemplates>;
    };

    template<class tag, class Arg1, class Arg2, class Arg3, class Arg4, class Backend,
             nil::crypto3::multiprecision::expression_template_option ExpressionTemplates, typename BinaryOp>
    struct ScalarBinaryOpTraits<nil::crypto3::multiprecision::detail::expression<tag, Arg1, Arg2, Arg3, Arg4>,
                                nil::crypto3::multiprecision::number<Backend, ExpressionTemplates>, BinaryOp> {
        static_assert(
            std::is_convertible<
                typename nil::crypto3::multiprecision::detail::expression<tag, Arg1, Arg2, Arg3, Arg4>::result_type,
                nil::crypto3::multiprecision::number<Backend, ExpressionTemplates>>::value,
            "Interoperability with this arithmetic type is not supported.");
        using ReturnType = nil::crypto3::multiprecision::number<Backend, ExpressionTemplates>;
    };

    namespace internal {
        template<typename Scalar>
        struct conj_retval;

        template<typename Scalar, bool IsComplex>
        struct conj_impl;

        template<class tag, class Arg1, class Arg2, class Arg3, class Arg4>
        struct conj_retval<nil::crypto3::multiprecision::detail::expression<tag, Arg1, Arg2, Arg3, Arg4>> {
            using type =
                typename nil::crypto3::multiprecision::detail::expression<tag, Arg1, Arg2, Arg3, Arg4>::result_type;
        };

        template<class tag, class Arg1, class Arg2, class Arg3, class Arg4>
        struct conj_impl<nil::crypto3::multiprecision::detail::expression<tag, Arg1, Arg2, Arg3, Arg4>, true> {
            EIGEN_DEVICE_FUNC
            static inline
                typename nil::crypto3::multiprecision::detail::expression<tag, Arg1, Arg2, Arg3, Arg4>::result_type
                run(const typename nil::crypto3::multiprecision::detail::expression<tag, Arg1, Arg2, Arg3, Arg4>& x) {
                return conj(x);
            }
        };

    }    // namespace internal

}    // namespace Eigen

#endif


#include <armadillo>

#include "settings.h"

#include "landmark/LandmarkIO.h"

#include "mesh/MeshIO.h"
#include "mesh/Mesh.h"

#include "optimization/rigidalignment/Energy.h"
#include "optimization/rigidalignment/EnergyMinimizer.h"
#include "optimization/rigidalignment/MinimizerSettings.h"

int main(int argc, char* argv[]) {

  Settings settings(argc, argv);

  // read input data
  Mesh source = MeshIO::read(settings.source);
  Mesh target = MeshIO::read(settings.target);

  // deal with target meshes that do not provide normals
  if( target.has_normals() == false ) {

    // try to estimate normals if the mesh has faces
    if( target.has_faces() == true ) {

      NormalEstimation estimation(target);
      target.set_vertex_normals(estimation.compute());

    } // end if
    else {

      // use point-to-point distance measure
      settings.energySettings.searchStrategy =
        rigidAlignment::EnergySettings::SearchStrategy::BASIC;

    } // end else

  } // end if

  std::vector<Landmark> landmarks;

  if(settings.landmarksPresent == true) {

    landmarks = LandmarkIO::read(settings.landmarks);

  }

  // first initialize with landmark information alone
  if( settings.noInitializationWithLandmarks == false && settings.landmarksPresent == true) {

    rigidAlignment::EnergyData data(source, target);
    data.landmarks = landmarks;

    settings.energySettings.weights["dataTerm"] = 0;
    settings.energySettings.weights["landmarkTerm"] = 1;

    // do not optimize rotation: might drastically shrink the template
    const bool noScaling = settings.minimizerSettings.noScaling;

    // only use one iteration: landmarks do not change between iterations
    const int iterationAmount = settings.minimizerSettings.iterationAmount;

    settings.minimizerSettings.noScaling = true;
    settings.minimizerSettings.iterationAmount = 1;

    rigidAlignment::Energy energy(data, settings.energySettings);
    rigidAlignment::EnergyMinimizer minimizer(energy, settings.minimizerSettings);

    minimizer.minimize();

    // replace source with aligned one
    source = energy.derived_data().source;

    // restore old settings
    settings.minimizerSettings.noScaling = noScaling;
    settings.minimizerSettings.iterationAmount = iterationAmount;

  }

  // now use both, data and landmarks
  if( settings.noCombinedOptimization == false && settings.landmarksPresent == true) {

    rigidAlignment::EnergyData data(source, target);

    data.landmarks = landmarks;

    settings.energySettings.weights["dataTerm"] = 1;
    settings.energySettings.weights["landmarkTerm"] = 1;

    rigidAlignment::Energy energy(data, settings.energySettings);
    rigidAlignment::EnergyMinimizer minimizer(energy, settings.minimizerSettings);

    minimizer.minimize();

    // replace source with aligned one
    source = energy.derived_data().source;

  }

  // final step: only use data
  if( settings.noDataOnlyOptimization == false) {

    rigidAlignment::EnergyData data(source, target);

    settings.energySettings.weights["dataTerm"] = 1;
    settings.energySettings.weights["landmarkTerm"] = 0;

    rigidAlignment::Energy energy(data, settings.energySettings);
    rigidAlignment::EnergyMinimizer minimizer(energy, settings.minimizerSettings);

    minimizer.minimize();

    // replace source with aligned one
    source = energy.derived_data().source;

  }

  MeshIO::write(source, settings.output);

  return 0;

}


#include "BoundingBox.hpp"
#include "QuadKey.hpp"
#include "utils/GeoUtils.hpp"

#include <boost/test/unit_test.hpp>

using namespace utymap;
using namespace utymap::utils;

namespace {
    const double Precision = 0.1e-7;
    const double TestLatitude = 52.53171;
    const double TestLongitude = 13.38730;
}

BOOST_AUTO_TEST_SUITE(Utils_GeoUtils)

BOOST_AUTO_TEST_CASE( GivenTestLocationAtFirstLod_WhenGetQuadKey_ThenReturnValidQuadKey )
{
    QuadKey quadKey = GeoUtils::latLonToQuadKey(GeoCoordinate(TestLatitude, TestLongitude), 1);

    BOOST_CHECK_EQUAL(quadKey.levelOfDetail, 1);
    BOOST_CHECK_EQUAL(quadKey.tileX, 1);
    BOOST_CHECK_EQUAL(quadKey.tileY, 0);
}

BOOST_AUTO_TEST_CASE(GivenTestLocationAtNineLod_WhenGetQuadKey_ThenReturnValidQuadKey)
{
    QuadKey quadKey = GeoUtils::latLonToQuadKey(GeoCoordinate(TestLatitude, TestLongitude), 9);

    BOOST_CHECK_EQUAL(quadKey.levelOfDetail, 9);
    BOOST_CHECK_EQUAL(quadKey.tileX, 275);
    BOOST_CHECK_EQUAL(quadKey.tileY, 167);
}

BOOST_AUTO_TEST_CASE(GivenTestLocationAtNineteenLod_WhenGetQuadKey_ThenReturnValidQuadKey)
{
    QuadKey quadKey = GeoUtils::latLonToQuadKey(GeoCoordinate(TestLatitude, TestLongitude), 19);

    BOOST_CHECK_EQUAL(quadKey.levelOfDetail, 19);
    BOOST_CHECK_EQUAL(quadKey.tileX, 281640);
    BOOST_CHECK_EQUAL(quadKey.tileY, 171914);
}

BOOST_AUTO_TEST_CASE(GivenQuadKeyAtNineteenLod_WhenGetBoundgingBox_ThenReturnValidBoundingBox)
{
    QuadKey quadKey (19, 281640, 171914);

    utymap::BoundingBox boundingBox = GeoUtils::quadKeyToBoundingBox(quadKey);

    BOOST_CHECK_CLOSE(boundingBox.minPoint.latitude, 52.531678559, Precision);
    BOOST_CHECK_CLOSE(boundingBox.maxPoint.latitude, 52.532096259, Precision);
    BOOST_CHECK_CLOSE(boundingBox.minPoint.longitude, 13.386840820, Precision);
    BOOST_CHECK_CLOSE(boundingBox.maxPoint.longitude, 13.387527465, Precision);
}

BOOST_AUTO_TEST_CASE(GivenQuadKeyAtNineteenLod_WhenToString_ThenReturnValidCode)
{
    QuadKey quadKey(19 ,281640, 171914);

    std::string code = GeoUtils::quadKeyToString(quadKey);

    BOOST_CHECK_EQUAL("1202102332220103020", code);
}

BOOST_AUTO_TEST_CASE(GivenBboxAtLodOne_WhenVisitTileRange_VisitsOneTile)
{
    BoundingBox bbox(GeoCoordinate(1, 1), GeoCoordinate(2, 2));
    int count = 0;
    std::shared_ptr<QuadKey> lastQuadKey;

    GeoUtils::visitTileRange(bbox, 1, [&](const QuadKey& quadKey, const BoundingBox&) {
        count++;
        lastQuadKey = std::make_shared<QuadKey>(quadKey);
    });

    BOOST_CHECK_EQUAL(1, count);
    BOOST_CHECK_EQUAL(1, lastQuadKey->tileX);
    BOOST_CHECK_EQUAL(0, lastQuadKey->tileY);
}

BOOST_AUTO_TEST_CASE(GivenBboxAtLodOne_WhenVisitTileRange_VisitsTwoTiles)
{
    BoundingBox bbox(GeoCoordinate(-1, 1), GeoCoordinate(2, 2));
    int count = 0;

    GeoUtils::visitTileRange(bbox, 1, [&](const QuadKey& quadKey, const BoundingBox&) {
        count++;
    });

    BOOST_CHECK_EQUAL(2, count);
}

BOOST_AUTO_TEST_SUITE_END()


#define BOOST_TEST_MODULE pcrblock remove
#include <boost/test/unit_test.hpp>
#include "dal_MathUtils.h"
#include "discr_block.h"
#include "discr_blockdata.h"
#include "discr_raster.h"
#include "discr_rasterdata.h"
#include "block_compactors.h"
#include "block_dummycompactor.h"
#include "block_functions.h"

struct Fixture
{

    Fixture()
    {
      // Set up a block with some voxels and a blockdata filled with sediment.
      size_t nrRows = 3;
      size_t nrCols = 4;
      double cellSize = 1.5;
      double west = 1.0;
      double north = 0.0;

      // Discretisations.
      d_raster = new discr::Raster(nrRows, nrCols, cellSize, west, north);
      discr::RasterData<REAL4> baseElevation(d_raster, REAL4(5.0));
      d_block = new discr::Block(baseElevation);

      // Data.
      d_originalThickness = new discr::BlockData<REAL4>(d_block);
      d_sediment = new discr::BlockData<INT4>(d_block, 3);

      // Add function arguments.
      discr::RasterData<REAL4> thickness(d_raster, REAL4(15.5));
      d_maxVoxelThickness = REAL4(1.0);

      // No compaction.
      block::Compactors<block::MackeyBridgeCompactor> compactors;
      compactors.setCompactor(3, block::DummyCompactor());

      // TODO work around this add, see ResampleTest, remove functions.h inclusion.
      mackeyBridgeAdd(*d_block, *d_originalThickness, *d_sediment, thickness,
             d_maxVoxelThickness, compactors);
    }

    ~Fixture()
    {
      delete d_sediment;
      delete d_originalThickness;
      delete d_block;
      delete d_raster;
    }

    discr::Raster* d_raster;

    discr::Block* d_block;

    discr::BlockData<REAL4>* d_originalThickness;

    discr::BlockData<INT4>* d_sediment;

    REAL4 d_maxVoxelThickness;

};

BOOST_FIXTURE_TEST_SUITE(remove_, Fixture)

BOOST_AUTO_TEST_CASE(remove__)
{
  using namespace block;

  // Remove nothing.
  {
    discr::RasterData<REAL4> thickness(d_raster, REAL4(0.0));
    remove(*d_block, thickness);

    for(size_t i = 0; i < d_block->nrCells(); ++i) {
      BOOST_CHECK(d_block->cell(i).size() == 16);
      BOOST_CHECK(d_block->cell(i).thickness() == REAL4(15.5));
      BOOST_CHECK(d_block->cell(i).back() == REAL4(0.5));
    }

    for(size_t i = 0; i < d_block->nrCells(); ++i) {
      for(size_t j = 0; j < d_block->cell(i).size() - 1; ++j) {
        BOOST_CHECK(d_originalThickness->cell(i)[j] == d_maxVoxelThickness);
        BOOST_CHECK(d_sediment->cell(i)[j] == 3);
      }

      BOOST_CHECK(d_originalThickness->cell(i).back() == REAL4(0.5));
      BOOST_CHECK(d_sediment->cell(i).back() == 3);
    }
  }

  // Remove a little.
  {
    discr::RasterData<REAL4> thickness(d_raster, REAL4(0.4));
    remove(*d_block, thickness);

    for(size_t i = 0; i < d_block->nrCells(); ++i) {
      BOOST_CHECK(d_block->cell(i).size() == 16);
      BOOST_CHECK(dal::comparable(d_block->cell(i).thickness(), REAL4(15.1)));
      BOOST_CHECK(dal::comparable(d_block->cell(i).back(), REAL4(0.1)));
    }

    for(size_t i = 0; i < d_block->nrCells(); ++i) {
      for(size_t j = 0; j < d_block->cell(i).size() - 1; ++j) {
        BOOST_CHECK(dal::comparable(d_originalThickness->cell(i)[j],
              d_maxVoxelThickness));
        BOOST_CHECK(d_sediment->cell(i)[j] == 3);
      }

      BOOST_CHECK(dal::comparable(
              d_originalThickness->cell(i).back(), REAL4(0.5)));
      BOOST_CHECK(d_sediment->cell(i).back() == 3);
    }
  }

  // Remove some more.
  {
    discr::RasterData<REAL4> thickness(d_raster, REAL4(0.4));
    remove(*d_block, thickness);

    for(size_t i = 0; i < d_block->nrCells(); ++i) {
      BOOST_CHECK(d_block->cell(i).size() == 15);
      BOOST_CHECK(dal::comparable(d_block->cell(i).thickness(), REAL4(14.7)));
      BOOST_CHECK(dal::comparable(d_block->cell(i).back(), REAL4(0.7)));
    }

    for(size_t i = 0; i < d_block->nrCells(); ++i) {
      for(size_t j = 0; j < d_block->cell(i).size() - 1; ++j) {
        BOOST_CHECK(dal::comparable(d_originalThickness->cell(i)[j],
              d_maxVoxelThickness));
        BOOST_CHECK(d_sediment->cell(i)[j] == 3);
      }

      BOOST_CHECK(dal::comparable(
              d_originalThickness->cell(i).back(), REAL4(1.0)));
      BOOST_CHECK(d_sediment->cell(i).back() == 3);
    }
  }

  // Remove still some more.
  {
    discr::RasterData<REAL4> thickness(d_raster, REAL4(4.4));
    remove(*d_block, thickness);

    for(size_t i = 0; i < d_block->nrCells(); ++i) {
      BOOST_CHECK(d_block->cell(i).size() == 11);
      BOOST_CHECK(dal::comparable(d_block->cell(i).thickness(), REAL4(10.3)));
      BOOST_CHECK(dal::comparable(d_block->cell(i).back(), REAL4(0.3)));
    }

    for(size_t i = 0; i < d_block->nrCells(); ++i) {
      for(size_t j = 0; j < d_block->cell(i).size() - 1; ++j) {
        BOOST_CHECK(dal::comparable(d_originalThickness->cell(i)[j],
              d_maxVoxelThickness));
        BOOST_CHECK(d_sediment->cell(i)[j] == 3);
      }

      BOOST_CHECK(dal::comparable(
              d_originalThickness->cell(i).back(), REAL4(1.0)));
      BOOST_CHECK(d_sediment->cell(i).back() == 3);
    }
  }

  BOOST_WARN(false);
}

BOOST_AUTO_TEST_CASE(remove_more_than_available)
{
  using namespace block;

  // Remove more than available (dig in).
  {
    discr::RasterData<REAL4> thickness(d_raster, REAL4(16.0));
    remove(*d_block, thickness);

    for(size_t i = 0; i < d_block->nrCells(); ++i) {
      BOOST_CHECK(dal::comparable(d_block->cell(i).baseElevation(), REAL4(4.5)));
      BOOST_CHECK(d_block->cell(i).size() == 0);
      BOOST_CHECK(dal::comparable(d_block->cell(i).thickness(), REAL4(0)));
    }

    for(size_t i = 0; i < d_block->nrCells(); ++i) {
      BOOST_CHECK(d_originalThickness->cell(i).empty());
      BOOST_CHECK(d_sediment->cell(i).empty());
    }
  }
}

BOOST_AUTO_TEST_SUITE_END()


//==============================================================================
//         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_FAST_SQRT_HPP_INCLUDED
#define BOOST_SIMD_ARITHMETIC_FUNCTIONS_SIMD_COMMON_FAST_SQRT_HPP_INCLUDED

#include <boost/simd/arithmetic/functions/fast_sqrt.hpp>
#include <boost/simd/include/functions/simd/sqrt.hpp>
#include <boost/dispatch/attributes.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_DISPATCH_IMPLEMENT          ( fast_sqrt_
                                    , boost::simd::tag::simd_
                                    , (A0)(X)
                                    , ((simd_< double_<A0>, X>))
                                    )
  {
    typedef A0 result_type;

    BOOST_FORCEINLINE result_type operator()(A0 const& a0) const
    {
      return sqrt(a0);
    }
  };
} } }

#endif


/*
 * Copyright 2020 Tier IV, Inc. All rights reserved.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#pragma once

#include <memory>
#include <string>

#include <diagnostic_msgs/DiagnosticStatus.h>
#include <diagnostic_msgs/KeyValue.h>
#include <geometry_msgs/TwistStamped.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl_ros/point_cloud.h>
#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <tf2/utils.h>
#include <tf2_ros/transform_listener.h>
#include <visualization_msgs/MarkerArray.h>
#include <boost/assert.hpp>
#include <boost/assign/list_of.hpp>
#include <boost/format.hpp>
#include <boost/geometry.hpp>
#include <boost/geometry/geometries/linestring.hpp>
#include <boost/geometry/geometries/point_xy.hpp>

#include <autoware_perception_msgs/DynamicObjectArray.h>
#include <autoware_planning_msgs/Trajectory.h>

#include "surround_obstacle_checker/debug_marker.hpp"

using Point2d = boost::geometry::model::d2::point_xy<double>;
using Polygon2d =
  boost::geometry::model::polygon<Point2d, false, false>;  // counter-clockwise, open

enum class State { PASS, STOP };

class SurroundObstacleCheckerNode
{
public:
  SurroundObstacleCheckerNode();

private:
  void pathCallback(const autoware_planning_msgs::Trajectory::ConstPtr & input_msg);
  void pointCloudCallback(const sensor_msgs::PointCloud2::ConstPtr & input_msg);
  void dynamicObjectCallback(
    const autoware_perception_msgs::DynamicObjectArray::ConstPtr & input_msg);
  void currentVelocityCallback(const geometry_msgs::TwistStamped::ConstPtr & input_msg);
  void insertStopVelocity(const size_t closest_idx, autoware_planning_msgs::Trajectory * traj);
  bool convertPose(
    const geometry_msgs::Pose & pose, const std::string & source, const std::string & target,
    const ros::Time & time, geometry_msgs::Pose & conv_pose);
  bool getPose(
    const std::string & source, const std::string & target, const ros::Time & time,
    geometry_msgs::Pose & pose);
  void getNearestObstacle(double * min_dist_to_obj, geometry_msgs::Point * nearest_obj_point);
  void getNearestObstacleByPointCloud(
    double * min_dist_to_obj, geometry_msgs::Point * nearest_obj_point);
  void getNearestObstacleByDynamicObject(
    double * min_dist_to_obj, geometry_msgs::Point * nearest_obj_point);
  bool isObstacleFound(const double min_dist_to_obj);
  bool isStopRequired(const bool is_obstacle_found, const bool is_stopped);
  size_t getClosestIdx(
    const autoware_planning_msgs::Trajectory & traj, const geometry_msgs::Pose current_pose);
  bool checkStop(const autoware_planning_msgs::TrajectoryPoint & closest_point);
  Polygon2d createSelfPolygon();
  Polygon2d createObjPolygon(const geometry_msgs::Pose & pose, const geometry_msgs::Vector3 & size);
  Polygon2d createObjPolygon(
    const geometry_msgs::Pose & pose, const geometry_msgs::Polygon & footprint);
  diagnostic_msgs::DiagnosticStatus makeStopReasonDiag(
    const std::string no_start_reason, const geometry_msgs::Pose & stop_pose);
  std::string jsonDumpsPose(const geometry_msgs::Pose & pose);

  /*
   * ROS
   */
  // publisher and subscriber
  ros::NodeHandle nh_;
  ros::NodeHandle pnh_;
  ros::Subscriber path_sub_;
  ros::Subscriber current_velocity_sub_;
  ros::Subscriber dynamic_object_sub_;
  ros::Subscriber pointcloud_sub_;
  ros::Publisher path_pub_;
  ros::Publisher stop_reason_diag_pub_;
  std::shared_ptr<SurroundObstacleCheckerDebugNode> debug_ptr_;
  tf2_ros::Buffer tf_buffer_;
  tf2_ros::TransformListener tf_listener_;

  // parameter
  geometry_msgs::TwistStamped::ConstPtr current_velocity_ptr_;
  sensor_msgs::PointCloud2::ConstPtr pointcloud_ptr_;
  autoware_perception_msgs::DynamicObjectArray::ConstPtr object_ptr_;
  double wheel_base_, front_overhang_, rear_overhang_, left_overhang_, right_overhang_,
    wheel_tread_, vehicle_width_, vehicle_length_;
  Polygon2d self_poly_;
  bool use_pointcloud_;
  bool use_dynamic_object_;
  double surround_check_distance_;
  // For preventing chattering,
  // surround_check_recover_distance_ must be  bigger than surround_check_distance_
  double surround_check_recover_distance_;
  double state_clear_time_;
  double stop_state_ego_speed_;
  bool is_surround_obstacle_;

  // State Machine
  State state_ = State::PASS;
  std::shared_ptr<const ros::Time> last_obstacle_found_time_;
};


/*
 * svn_split.hpp
 *
 *  Created on: Feb 9, 2012
 *      Author: mdantone
 */

#ifndef SVN_SPLIT_HPP_
#define SVN_SPLIT_HPP_

#include <boost/serialization/shared_ptr.hpp>
#include "boost/smart_ptr.hpp"
#include <boost/random.hpp>
#include "cpp/utils/libsvm/liblinear.hpp"
#include "cpp/learning/forest/simple_split.hpp"

namespace learning
{
namespace forest
{

template<typename Feature>
class SVNSplit
{
public:
  SVNSplit(){
    info = boost::numeric::bounds<double>::lowest();
    gain = boost::numeric::bounds<double>::lowest();
    oob = boost::numeric::bounds<double>::highest();
    used_subsampling = false;
  };

  template<typename Sample>
  double get_feature_value(const Sample* sample, const Feature* feature) const {
    std::vector<float> desc;
    sample->extract(feature, desc);
    double r = predict(desc);
    return r;
  }

  template<typename Sample>
  bool eval( const Sample* sample ) const {
    return get_feature_value(sample, &feature) <= 0;
  }

  template<typename Sample>
  void split(const std::vector<Sample*>& data,
    std::vector<Sample*>& set_a, std::vector<Sample*>& set_b) const {
    for (unsigned int l = 0; l < data.size(); ++l) {
      if(eval(data[l])) {
        set_a.push_back(data[l]);
      }else{
        set_b.push_back(data[l]);
      }
    }
  }

  template<typename Sample>
  void evaluate(const std::vector<Sample*>& data,  const std::vector<float> class_weights,
       int split_mode, int depth, int min_samples = 10) {
    double pre_info = Sample::eval(data, split_mode, class_weights);

    std::vector<Sample*> set_a, set_b;
    split(data, set_a, set_b);
    if(set_a.size() < min_samples || set_b.size() < min_samples){
      info = boost::numeric::bounds<double>::lowest();
      gain = boost::numeric::bounds<double>::lowest();
    }else{
      info = Sample::evalSplit(set_a, set_b, class_weights, split_mode, depth);
      gain = info - pre_info;
    }
  }

  void get_binary_labels( std::vector<int>& binary_labels,
                          std::vector<int>& hist,
                          boost::mt19937* rng) {

    if( hist.size() == 2){
      binary_labels.push_back(0);
      binary_labels.push_back(1);
      return;
    }

    binary_labels.resize(hist.size(),0);
    std::vector<int> new_hist(2,0);

    //assigne random binary label
    boost::uniform_int<> dist_bin(0,1);
    boost::variate_generator<boost::mt19937&, boost::uniform_int<> > rand_bin( *rng, dist_bin);
    while(true){
      new_hist[0] = 0;
      new_hist[1] = 0;
      for(int i=0; i < hist.size(); i++){
        binary_labels[i] = rand_bin();
        if(binary_labels[i] == 0) {
          new_hist[0] += hist[i];
        }else{
          new_hist[1] += hist[i];
        }
      }
      if(new_hist[0] > 0 && new_hist[1] > 0 )
        break;
    }

    hist.resize(2,0);
    hist[0] = new_hist[0];
    hist[1] = new_hist[1];
  }

  template<typename Sample, typename F>
  void get_statistics(const std::vector<Sample*>& set,
      F label_function,
      std::vector<int>& hist,
      int* num_samples,
      int* num_labels,
      int max_samples = 1000000) {

    int size = MIN(set.size(), max_samples);
    for (unsigned int i = 0; i < size; ++i) {
      int l = label_function(set[i]);
      if( l >= 0){
        if(l >= hist.size()) {
          hist.resize(l+1,0);
        }
        hist[l]++;
      }
    }
    int n_samples = 0;
    int n_labels = 0;
    for (unsigned int i = 0; i < hist.size(); ++i) {
      n_samples += hist[i];
      if(hist[i] > 0) {
        n_labels += 1;
      }
    }
    *num_samples = n_samples;
    *num_labels = n_labels;

  }

  template<typename Sample, typename F>
  void train(const std::vector<Sample*>& set,
      F label_function,
      boost::mt19937* rng,
      ::utils::liblinear::solver_type::T solver,
      double C = 1.0, double eps = 0.10,
      bool use_weights = true,
      int max_samples = 1000000) {

    //allocate problem
    std::vector<float> desc;
    set[0]->extract(&feature, desc);
    int non_zero = desc.size();
    CHECK(non_zero > 0);

    int num_samples = 0;
    int num_labels = 0;
    std::vector<int> label_hist(2,0);
    get_statistics(set, label_function, label_hist,
        &num_samples, &num_labels, max_samples);

    if(num_labels > 1) {
      std::vector<int> binary_labels;
      get_binary_labels(binary_labels, label_hist, rng);

      ::utils::liblinear::ProblemHolder p;
      p.allocate(num_samples,non_zero*num_samples);

      int size = MIN(set.size(), max_samples);
      for (unsigned int i = 0; i < size; ++i) {
        int l = label_function(set[i]);
        if (l >= 0 ) {
          set[i]->extract(&feature, desc);
          cv::Mat mat = cv::Mat_<float>(1, non_zero, &desc[0]);
          p.push_problem<float>(mat, binary_labels[l]);
        }
      }

      boost::shared_ptr< ::utils::liblinear::LinearHolder> model;
      model =  boost::shared_ptr< ::utils::liblinear::LinearHolder>(
          new ::utils::liblinear::LinearHolder() );

      std::vector<double> class_weights;
      std::vector<int> class_weights_label;
      if(use_weights){
        class_weights.resize(2);
        class_weights[0] = 1 - label_hist[0] / static_cast<float>(num_samples);
        class_weights[1] = 1 - label_hist[1] / static_cast<float>(num_samples);
        class_weights_label.resize(2);
        class_weights_label[0] = 0;
        class_weights_label[1] = 1;
      }

      model->train( p, solver, C, eps, class_weights, class_weights_label );
      model->get_weights(weights);

    }
    if(weights.size() <= 0) {
      weights.resize(non_zero, 0.0);
    }
  }

  double predict(const std::vector<float>& desc) const {
    CHECK(desc.size() > 0);
    CHECK(weights.size() > 0);

    int n = MIN(desc.size(), weights.size());
    double r = 0.0;
    for(int i=0; i < n; i++) {
      r += desc[i]*weights[i];
    }
    return r;
  }

  void set_feature(Feature f) {
    feature = f;
  }

  std::vector<double> weights;
  Feature feature;
  double info;
  double gain;
  double oob;
  bool used_subsampling;

private:
  friend class boost::serialization::access;
  template<class Archive>
  void serialize(Archive & ar, const unsigned int version) {
     ar & weights;
     ar & feature;
     ar & info;
     ar & gain;
     ar & oob;
  }
};

} //namespace forest

} //namespace learning

#endif /* LIBLINEARSPLIT_HPP_ */


/**
 * @brief defines power netork class
 *
 * @file powerNet.hpp
 * @author Luiz Victor Linhares Rocha
 * @date 2018-09-22
 * @copyright 2018
 */

#ifndef REDE_POWERNET_HPP_
#define REDE_POWERNET_HPP_

#include <vector>
#include <boost/shared_ptr.hpp>
#include "../libraries/Eigen/Eigen"
#include "neuralFluxDeclare.hpp"
#include "barra.hpp"
#include "../util/identifiable.hpp"

namespace neuralFlux {

class PowerNet : public Identifiable {
 public:
    explicit PowerNet(uint64_t id = 0);
    virtual ~PowerNet();
    PowerNet(const PowerNet& net);
    PowerNet& operator=(const PowerNet&);
    // network setup methods
    void addSlackBar(
        const size_t id,
        const double V,
        const double teta,
        const double p = 0,
        const double q = 0,
        const double bsh = 0);
    void addPQBar(
        const size_t id,
        const double p,
        const double q,
        const double v = 1,
        const double teta = 0,
        const double bsh = 0);
    void addPVBar(
        const size_t id,
        const double p,
        const double v,
        const double teta = 0,
        const double q = 0,
        const double bsh = 0);
    void addBar(
        const Bar::barType type,
        const size_t id,
        const double v = 1,
        const double teta = 0,
        const double p = 0,
        const double q = 0,
        const double bsh = 0);
    /** connect bars using the bars ids, hence the bars insertion order*/
    void connect(
        size_t k,
        size_t m,
        const double rkm,
        const double xkm,
        const double bshkm = 0,
        const double a = 1,
        const double phi = 0);
    /** connect bars using the sorted vector*/
    void connectByIndex(
        size_t k,
        size_t m,
        const double rkm,
        const double xkm,
        const double bshkm = 0,
        const double a = 1,
        const double phi = 0);
    // network tear down methods
    void deleteBranch(uint64_t id);
    void deleteBranch(uint64_t kId, uint64_t mId);
    void deleteBranchByIndex(uint64_t i);
    void deleteBranchByBarIndex(uint64_t kI, uint64_t mI);
    void deleteBar(size_t i);
    void deleteBarByIndex(size_t i);
    void cleanBranches();
    void clean();
    // network reset to standard
    void setFlatStart();
    // caracteristics getters
    size_t getNBars() const;
    size_t getNBranches() const;
    size_t getNSlack() const;
    size_t getNPQ() const;
    size_t getNPV() const;
    size_t getBarIndex(const BarPtr bar) const;
    size_t getBarIndex(const size_t id) const;
    size_t getBranchIndex(const BranchPtr branch) const;
    // gets branch index by branch id
    size_t getBranchIndex(uint64_t id) const;
    // gets branch index by bars id
    size_t getBranchIndex(uint64_t kId, uint64_t mId) const;
    // gets branch index by index of bars
    size_t getBranchIndexByIndex(size_t kId, size_t mId) const;
    // gets bar by id, using addition sequence
    const BarPtr getBar(uint64_t i) const;
    // gets bar by index
    const BarPtr getBarByIndex(size_t i) const;
    // gets branch by id
    const BranchPtr getBranch(uint64_t i) const;
    // gets branch by index
    const BranchPtr getBranchByIndex(size_t i) const;
    // gets branch by id of two bars
    const BranchPtr getBranch(uint64_t kId, uint64_t mId) const;
    // gets branch by index of two bars
    const BranchPtr getBranchByIndex(size_t kId, size_t mId) const;
    void updateY();
    const complexo getY(size_t i, size_t j);
    void completeNetPowers();
    // calcula a potência demandada de uma barra, calculada a partir das admitancias
    complexo getSk(size_t i);
    double getTetaKm(uint64_t kId, uint64_t mId) const;
    double getTetaKmByIndex(size_t kI, size_t mI) const;
    // checks if 2 bars are connected
    bool areConnected(uint64_t i, uint64_t j);
    bool areConnectedByIndex(size_t i, size_t j);

 private:
    std::vector<BarPtr> m_busBars;
    std::vector<BranchPtr> m_branches;
    Eigen::MatrixXcd m_Y;

    size_t m_nPQ;
    size_t m_nPV;
    size_t m_nSlack;

    /**
    *   @brief called when a bar is deleted, decrement the count for the type the bar was
    */
    void discontBarCount(Bar::barType type);
    bool checkBarIdExists(uint64_t id);
    void sortBackBranch();
    void sortBackBar();
    bool checkChanged();
};
}  // namespace neuralFlux
#endif  // REDE_POWERNET_HPP_


#include <frovedis.hpp>
#include <frovedis/matrix/sliced_vector.hpp>
#include <frovedis/matrix/pblas_wrapper.hpp>

#define BOOST_TEST_MODULE FrovedisTest
#include <boost/test/unit_test.hpp>

using namespace frovedis;
using namespace std;

BOOST_AUTO_TEST_CASE( frovedis_test )
{
    int argc = 1;
    char** argv = NULL;
    use_frovedis use(argc, argv);

    auto bm = make_blockcyclic_matrix_load<float> ("./sample_4x1");

    // creating slice blockcyclic vector from entire "blockcyclic matrix with single column"
    sliced_blockcyclic_vector<float> sv (bm);

    // scaling entire slice with 2
    scal<float> (sv, 2);
    bm.save("./out_4x1");

    // if slice was correctly created, this check should pass
    BOOST_CHECK (system("diff ./out_4x1 ref_4x1") == 0);
    system("rm -f ./out_4x1");
}


/*
 * Author: Johannes M Dieterich
 */
#ifndef INTKERNELODE_HPP
#define INTKERNELODE_HPP

#include <armadillo>
#include <list>
#include <memory>
#include <rksuite.h>

class IntKernelODE {

public:
    IntKernelODE(bool stats = false);
    ~IntKernelODE();
    std::unique_ptr<arma::mat> makeFirstOrderKernel(ODEKernel* ode, const double wInf);
    std::unique_ptr<arma::mat> makeSecondOrderKernel(ODEKernel* ode, const double wInf);
    const std::shared_ptr<arma::vec> getLastEta() const;
    std::unique_ptr<arma::mat> makeSecondOrderKernel(ODEKernel* ode, const double wInf, const double tstart, const double tend, const double tstep);

private:
    RKSUITE* _rksuite;
    bool _stats;
    std::shared_ptr<arma::vec> _eta;

    const double MAGICINIT = -4242.0;
    const double MAXT = 100.0;
    const double TSTART = 1e-3;
    const double TEND = 3.0;
    const double TSTEP = 1e-3;
    const double THRESH = 1e-10;
};

#endif /* INTKERNELODE_HPP */


#include <corsika/Constants.h>
#include <boost/python.hpp>

using namespace boost::python;
using namespace corsika;

void register_PhysicalConstants()
{
  namespace bp = boost::python;
  // map the Unit namespace to a sub-module
  // make "from mypackage.Unit import <whatever>" work
  bp::object constantsModule(bp::handle<>(bp::borrowed(PyImport_AddModule("corsika.constants"))));
  bp::scope().attr("constants") = constantsModule;
  bp::scope unit_scope = constantsModule;

  bp::scope().attr("kgEarth") = kgEarth;
  bp::scope().attr("kEarthRadius") = kEarthRadius;
  bp::scope().attr("kRadiationLength") = kRadiationLength;

  // grammage at sea level
  bp::scope().attr("kOverburdenSeaLevel") = kOverburdenSeaLevel;
  // refraction index sea level
  bp::scope().attr("kRefractiveIndexSeaLevel") = kRefractiveIndexSeaLevel;

  // A few gas and liquid properties

  bp::scope().attr("kMolarGasConstant") = kMolarGasConstant;
  bp::scope().attr("kAvogadro") = kAvogadro;
  bp::scope().attr("kBoltzmann") = kBoltzmann;

  bp::scope().attr("kDryAirMolarMass") = kDryAirMolarMass;
  bp::scope().attr("kN2MolarMass") = kN2MolarMass;
  bp::scope().attr("kO2MolarMass") = kO2MolarMass;
  bp::scope().attr("kArMolarMass") = kArMolarMass;
  bp::scope().attr("kCO2MolarMass") = kCO2MolarMass;
  bp::scope().attr("kH2OMolarMass") = kH2OMolarMass;

  bp::scope().attr("kN2AirFraction") = kN2AirFraction;
  bp::scope().attr("kO2AirFraction") = kO2AirFraction;
  bp::scope().attr("kArAirFraction") = kArAirFraction;
  bp::scope().attr("kCO2AirFraction") = kCO2AirFraction;

  bp::scope().attr("kH2OFreezingPoint") = kH2OFreezingPoint;

  // Gaisser-Hillas parameter for the electron mean free path in air
  bp::scope().attr("kLambdaGH") = kLambdaGH;

  // Constants

  bp::scope().attr("kSpeedOfLightSI") = kSpeedOfLightSI;
  bp::scope().attr("kSpeedOfLight") = kSpeedOfLight;
  bp::scope().attr("kSpeedOfLight2") = kSpeedOfLight2;
  bp::scope().attr("kPlanckSI") = kPlanckSI;
  bp::scope().attr("kPlanckReducedSI") = kPlanckReducedSI;
  bp::scope().attr("kPlanck") = kPlanck;
  bp::scope().attr("kPlanckReduced") = kPlanckReduced;
  bp::scope().attr("kMuZeroSI") = kMuZeroSI;
  bp::scope().attr("kMuZero") = kMuZero;

  // Derived constants

  bp::scope().attr("kEpsilonZeroSI") = kEpsilonZeroSI;
  bp::scope().attr("kAlpha") = kAlpha;
  bp::scope().attr("kElectronRadiusSI") = kElectronRadiusSI;
  bp::scope().attr("kThomsonCrossSectionSI") = kThomsonCrossSectionSI;
}


#include <boost/hana/monadic_fold_right.hpp>


#include <boost/assert.hpp>
#include "cblas.h"
#include "blas-wrapper.h"

namespace argos {
    namespace blas {
        template <>
        void gemm (float const *A, size_t A_rows, size_t A_cols, bool transA,
                   float const *B, size_t B_rows, size_t B_cols, bool transB,
                   float *C, size_t C_rows, size_t C_cols, float alpha, float beta) {
            size_t M, N, K;
            enum CBLAS_TRANSPOSE TA, TB;
            if (transA) {
                M = A_cols;
                K = A_rows;
                TA = CblasTrans;
            } else {
                M = A_rows;
                K = A_cols;
                TA = CblasNoTrans;
            }

            if (transB) {
                N = B_rows;
                BOOST_VERIFY(K == B_cols);
                TB = CblasTrans;
            } else {
                N = B_cols;
                BOOST_VERIFY(K == B_rows);
                TB = CblasNoTrans;
            }
            BOOST_VERIFY(M == C_rows);
            BOOST_VERIFY(N == C_cols);
            cblas_sgemm(CblasRowMajor, TA, TB, M, N, K, alpha, A, A_cols, B, B_cols, beta, C, C_cols);
        }

        template <>
        void gemm (double const *A, size_t A_rows, size_t A_cols, bool transA,
                   double const *B, size_t B_rows, size_t B_cols, bool transB,
                   double *C, size_t C_rows, size_t C_cols, double alpha, double beta) {
            size_t M, N, K;
            enum CBLAS_TRANSPOSE TA, TB;
            if (transA) {
                M = A_cols;
                K = A_rows;
                TA = CblasTrans;
            } else {
                M = A_rows;
                K = A_cols;
                TA = CblasNoTrans;
            }

            if (transB) {
                N = B_rows;
                BOOST_VERIFY(K == B_cols);
                TB = CblasTrans;
            } else {
                N = B_cols;
                BOOST_VERIFY(K == B_rows);
                TB = CblasNoTrans;
            }
            BOOST_VERIFY(M == C_rows);
            BOOST_VERIFY(N == C_cols);
            cblas_dgemm(CblasRowMajor, TA, TB, M, N, K, alpha, A, A_cols, B, B_cols, beta, C, C_cols);
        }
    }
}


/*
@copyright Louis Dionne 2014
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/hana/list/mcd.hpp>

#include <boost/hana/detail/assert.hpp>
#include <boost/hana/detail/constexpr.hpp>
#include <boost/hana/detail/minimal/comparable.hpp>
#include <boost/hana/detail/minimal/list.hpp>

#include <tuple>
using namespace boost::hana;

template <int i>
constexpr auto x = detail::minimal::comparable<>(i);

BOOST_HANA_CONSTEXPR_LAMBDA auto f = [](auto ...xs) {
    return std::make_tuple(x<888>, xs...);
};

BOOST_HANA_CONSTEXPR_LAMBDA auto g = [](auto ...xs) {
    return std::make_tuple(x<999>, xs...);
};

template <typename mcd>
void test() {
    BOOST_HANA_CONSTEXPR_LAMBDA auto list = detail::minimal::list<mcd>;
    using L = detail::minimal::List<mcd>;

    BOOST_HANA_CONSTANT_ASSERT(ap(list(), list()) == list());
    BOOST_HANA_CONSTANT_ASSERT(ap(list(), list(x<0>)) == list());
    BOOST_HANA_CONSTANT_ASSERT(ap(list(), list(x<0>, x<1>)) == list());
    BOOST_HANA_CONSTANT_ASSERT(ap(list(), list(x<0>, x<1>, x<2>)) == list());

    BOOST_HANA_CONSTANT_ASSERT(ap(list(f), list()) == list());
    BOOST_HANA_CONSTEXPR_ASSERT(ap(list(f), list(x<0>)) == list(f(x<0>)));
    BOOST_HANA_CONSTEXPR_ASSERT(ap(list(f), list(x<0>, x<1>)) == list(f(x<0>), f(x<1>)));
    BOOST_HANA_CONSTEXPR_ASSERT(ap(list(f), list(x<0>, x<1>, x<2>)) == list(f(x<0>), f(x<1>), f(x<2>)));

    BOOST_HANA_CONSTANT_ASSERT(ap(list(f, g), list()) == list());
    BOOST_HANA_CONSTEXPR_ASSERT(ap(list(f, g), list(x<0>)) == list(f(x<0>), g(x<0>)));
    BOOST_HANA_CONSTEXPR_ASSERT(ap(list(f, g), list(x<0>, x<1>)) == list(f(x<0>), f(x<1>), g(x<0>), g(x<1>)));
    BOOST_HANA_CONSTEXPR_ASSERT(ap(list(f, g), list(x<0>, x<1>, x<2>)) == list(f(x<0>), f(x<1>), f(x<2>), g(x<0>), g(x<1>), g(x<2>)));
}

int main() {
    test<List::mcd<void>>();
}


//---------------------------------------------------------------------------//
//!
//! \file   Utility_RandomNumberGenerator.hpp
//! \author Alex Robinson
//! \brief  Random number generation wrapper struct declaration
//!
//---------------------------------------------------------------------------//

#ifndef UTILITY_RANDOM_NUMBER_GENERATOR_HPP
#define UTILITY_RANDOM_NUMBER_GENERATOR_HPP

// Std Lib Includes
#include <vector>

// Boost Includes
#include <boost/ptr_container/ptr_vector.hpp>
#include <boost/scoped_ptr.hpp>

// FRENSIE includes
#include "Utility_LinearCongruentialGenerator.hpp"
#include "Utility_GlobalOpenMPSession.hpp"
#include "Utility_ContractException.hpp"

namespace Utility{

//! Struct that is used to obtain random numbers
class RandomNumberGenerator
{

public:

  //! Create the number of random number streams required
  static void createStreams();

  //! Create the number of random number streams requested
  static void createStreams( const unsigned number_of_streams );

  //! Initialize the generator for the desired history
  static void initialize( const unsigned long long history_number = 0ULL );

  //! Initialize the generator for the next history
  static void initializeNextHistory();

  //! Set a fake stream for the generator
  static void setFakeStream( std::vector<double>& fake_stream,
			     const unsigned thread_id = 0u );

  //! Unset the fake stream
  static void unsetFakeStream( const unsigned thread_id = 0u );

  //! Return a random number in interval [0,1)
  template<typename ScalarType>
  static ScalarType getRandomNumber();

  //! Destructor
  ~RandomNumberGenerator()
  { /* ... */ }

private:

  // Constructor
  RandomNumberGenerator();

  // Pointer to generator
  static boost::ptr_vector<LinearCongruentialGenerator> generator;
};

// Return a random number in interval [0,1)
template<typename ScalarType>
inline ScalarType RandomNumberGenerator::getRandomNumber()
{
  // Make sure the generator has been set up correctly
  testPrecondition( GlobalOpenMPSession::getThreadId() < generator.size() );
  // Make sure that the generator has been initialized
  testPrecondition( !generator.is_null( GlobalOpenMPSession::getThreadId() ) );

  return static_cast<ScalarType>(
	     generator[GlobalOpenMPSession::getThreadId()].getRandomNumber() );
}

// Return a random double in interval [0,1)
template<>
inline double RandomNumberGenerator::getRandomNumber<double>()
{
  // Make sure the generator has been set up correctly
  testPrecondition( GlobalOpenMPSession::getThreadId() < generator.size() );
  // Make sure that the generator has been initialized
  testPrecondition( !generator.is_null( GlobalOpenMPSession::getThreadId() ) );

  return generator[GlobalOpenMPSession::getThreadId()].getRandomNumber();
}

// Return a random long long unsigned integer in [0,2^64)
template<>
inline unsigned long long
RandomNumberGenerator::getRandomNumber<unsigned long long>()
{
  // Make sure the generator has been set up correctly
  testPrecondition( GlobalOpenMPSession::getThreadId() < generator.size() );
  // Make sure that the generator has been initialized
  testPrecondition( !generator.is_null( GlobalOpenMPSession::getThreadId() ) );

  generator[GlobalOpenMPSession::getThreadId()].getRandomNumber();

  return generator[GlobalOpenMPSession::getThreadId()].getGeneratorState();
}

} // end Utility namespace

#endif // end UTILITY_RANDOM_NUMBER_GENERATOR_HPP

//---------------------------------------------------------------------------//
// end Utility_RandomNumberGenerator.hpp
//---------------------------------------------------------------------------//