#include <array>
#include <cassert>
#include <iostream>
#include <iterator>
#include <numeric>

#include <boost/mpl/vector.hpp>

#define png_infopp_NULL nullptr
#define int_p_NULL      nullptr
#include <boost/gil/gil_all.hpp>
#include <boost/gil/extension/io/png_dynamic_io.hpp>
#include <boost/gil/extension/dynamic_image/dynamic_image_all.hpp>

namespace gil = boost::gil;
namespace mpl = boost::mpl;

// return the average color from all pixels in the view
template<std::size_t NChannels, typename View>
auto average_color(View const& view) {
    using std::begin;
    using std::end;

    using pixel_t = typename View::value_type;
    using channel_t = typename gil::channel_type<View>::type;

    auto accumulator = [](std::array<int, NChannels> sum, pixel_t const& val) {
        for (auto i = NChannels; i > 0; --i) { sum[i - 1] += val[i - 1]; }
        return sum;
    };

    auto sum = std::accumulate(begin(view), end(view),
        std::array<int, NChannels>{}, accumulator);

    pixel_t avg;
    for (auto i = NChannels; i > 0; --i) {
        avg[i - 1] = static_cast<channel_t>(sum[i - 1] / view.size());
    }

    return avg;
}

template<typename SrcView>
auto create_roi_view(SrcView const& src,
                     typename SrcView::x_coord_t x,
                     typename SrcView::y_coord_t y,
                     int size = 3) {
    assert(size > 0);

    // [x_min; x_max)
    auto x_min = x - (size - 1) / 2;
    auto x_max = x_min + size;
    if (x_min < 0) x_min = 0;
    if (x_max > src.width()) x_max = src.width();

    // [y_min; y_max)
    auto y_min = y - (size - 1) / 2;
    auto y_max = y_min + size;
    if (y_min < 0) y_min = 0;
    if (y_max > src.height()) y_max = src.height();

    return gil::subimage_view(src, x_min, y_min, x_max - x_min, y_max - y_min);
}

template<std::size_t NChannels, typename SrcView, typename DstView>
void blur_image(SrcView const& src, DstView const& dst) {
    using std::begin;
    using std::end;

    assert(src.dimensions() == dst.dimensions());

    for (auto y = src.height(); y > 0; --y) {
        for (auto x = src.width(); x > 0; --x) {
            auto roi_view = create_roi_view(src, x - 1, y - 1);
            dst(x - 1, y - 1) = average_color<NChannels>(roi_view);
        }
    }
}

template<typename DstAnyView>
struct blur_visitor {
    using result_type = void;

    blur_visitor(DstAnyView const& dst) :
        m_dst{ dst }
    {}

    // each image type has a different number of channels
#define CALL_OPERATOR(NChannels, SrcView, DstView)              \
    auto operator()(SrcView const& src) const -> result_type {  \
        blur_image<NChannels>(src,                              \
            m_dst.template _dynamic_cast<DstView>());           \
    }

    CALL_OPERATOR(4, gil::rgba8c_view_t, gil::rgba8_view_t)
    CALL_OPERATOR(3, gil::rgb8c_view_t, gil::rgb8_view_t)
    CALL_OPERATOR(1, gil::gray8c_view_t, gil::gray8_view_t)

#undef CALL_OPERATOR

private:
    DstAnyView const& m_dst;
};

// loop through a types sequence until function return true
template<std::size_t Current, std::size_t End>
struct for_until_position_helper {
    template<typename UnaryFunction>
    auto operator()(UnaryFunction f) const -> std::size_t {
        if (f(std::integral_constant<std::size_t, Current>{}))
            return Current;
        return for_until_position_helper<Current + 1, End>{}(f);
    }
};

template<std::size_t End>
struct for_until_position_helper<End, End> {
    template<typename UnaryFunction>
    auto operator()(UnaryFunction) const -> std::size_t {
        return End;
    }
};

template<typename Sequence, typename UnaryFunction>
auto for_until_position(UnaryFunction f) -> std::size_t {
    return for_until_position_helper<0, mpl::size<Sequence>::value>{}(f);
}

// create output image bases on underlying type of input image
template<typename ImagesVector>
void create_output_image(typename gil::any_image<ImagesVector>::const_view_t const& in,
                         gil::any_image<ImagesVector>& out) {
    for_until_position<ImagesVector>(
        [&](auto index_carrier) -> bool {
            if (index_carrier() == in.index()) {
                using real_image_t =
                    typename mpl::at_c<ImagesVector, index_carrier()>::type;
                out = real_image_t{ in.dimensions() };
                return true;
            }
            return false;
        });
}

int main() {
    try {
        using images_vector_t = mpl::vector<gil::rgba8_image_t,
                                            gil::rgb8_image_t,
                                            gil::gray8_image_t>;
        using image_t = gil::any_image<images_vector_t>;

        // read image to view
        image_t in;
        gil::png_read_image("in.png", in);
        auto view = gil::const_view(in);

        // create output image
        image_t out;
        create_output_image(view, out);

        // blurring
        blur_visitor<typename image_t::view_t> visitor{ gil::view(out) };
        gil::apply_operation(view, visitor);
        gil::png_write_view("out.png", gil::const_view(out));
    } catch (std::exception const& e) {
        std::cout << e.what() << '\n';
    }
}


#ifndef STAN__PROB__DISTRIBUTIONS__MULTIVARIATE__CONTINUOUS__MULTI_NORMAL_SUFFICIENT_HPP
#define STAN__PROB__DISTRIBUTIONS__MULTIVARIATE__CONTINUOUS__MULTI_NORMAL_SUFFICIENT_HPP

#include <boost/random/normal_distribution.hpp>
#include <boost/random/variate_generator.hpp>
#include <stan/math/prim/mat/err/check_ldlt_factor.hpp>
#include <stan/math/prim/mat/err/check_symmetric.hpp>
#include <stan/math/prim/scal/err/check_size_match.hpp>
#include <stan/math/prim/scal/err/check_finite.hpp>
#include <stan/math/prim/scal/err/check_not_nan.hpp>
#include <stan/math/prim/scal/err/check_positive.hpp>
#include <stan/math/prim/mat/fun/trace_inv_quad_form_ldlt.hpp>
#include <stan/math/prim/mat/fun/log_determinant_ldlt.hpp>
#include <stan/math/prim/scal/meta/return_type.hpp>
#include <stan/math/prim/scal/meta/VectorViewMvt.hpp>
#include <stan/math/prim/scal/meta/max_size_mvt.hpp>
#include <stan/math/prim/scal/meta/include_summand.hpp>

namespace stan {

  namespace prob {

    template <typename T_sample, typename T_loc, typename T_covar>
    typename boost::math::tools::promote_args<T_sample, typename scalar_type<T_loc>::type, T_covar>::type
    multi_normal_sufficient_log(const int sampleSize,
				const Eigen::Matrix<T_sample,Eigen::Dynamic,1>& sampleMu,
				const Eigen::Matrix<T_sample,Eigen::Dynamic,Eigen::Dynamic>& sampleSigma,
				const T_loc& mu,
				const Eigen::Matrix<T_covar,Eigen::Dynamic,Eigen::Dynamic>& Sigma) {
      static const char *function("stan::prob::multi_normal_sufficient_log");
      typedef typename boost::math::tools::promote_args<T_sample, typename scalar_type<T_loc>::type, T_covar>::type param_t;
      typedef param_t lp_type;
      lp_type lp(0.0);

      using stan::math::check_size_match;
      using stan::math::check_finite;
      using stan::math::check_not_nan;
      using stan::math::check_positive;
      using stan::math::check_symmetric;
      using stan::math::check_ldlt_factor;

      check_size_match(function,
                       "Rows of covariance parameter", sampleSigma.rows(),
                       "columns of covariance parameter", sampleSigma.cols());
      check_positive(function, "Covariance matrix rows", sampleSigma.rows());
      check_symmetric(function, "Covariance matrix", sampleSigma);

      check_size_match(function,
                       "Rows of covariance parameter", Sigma.rows(),
                       "columns of covariance parameter", Sigma.cols());
      check_positive(function, "Covariance matrix rows", Sigma.rows());
      check_symmetric(function, "Covariance matrix", Sigma);

      check_size_match(function,
                       "Size of data location", sampleMu.size(),
                       "size of model location", mu.size());
      check_size_match(function,
                       "Size of data covariance", sampleSigma.rows(),
                       "size of model covariance", Sigma.rows());

      stan::math::LDLT_factor<param_t,Eigen::Dynamic,Eigen::Dynamic> ldlt_Sigma(Sigma);
      check_ldlt_factor(function, "LDLT_Factor of covariance parameter", ldlt_Sigma);

      Eigen::Matrix<param_t, Eigen::Dynamic, Eigen::Dynamic> ss;
      ss = mdivide_left_ldlt(ldlt_Sigma, sampleSigma);

      lp += (ss.diagonal().sum() + log_determinant_ldlt(ldlt_Sigma)) * (sampleSize - 1);

      lp_type lp_location(0.0);
      {
	Eigen::Matrix<param_t, Eigen::Dynamic, 1> y_minus_mu(mu.size());

	for (int j = 0; j < mu.size(); j++)
	  y_minus_mu(j) = mu(j) - sampleMu(j);

	lp_location = trace_inv_quad_form_ldlt(ldlt_Sigma, y_minus_mu) * sampleSize;
	// Could avoid re-solving Sigma
	// lp_location = quad_form(ss, y_minus_mu).diagonal().sum() * sampleSize;
      }
      return (lp + lp_location) * -0.5;
    }
  }
}

#endif


#include <stdint.h>
#include <cstring>
#include <Eigen/Core>
#include <omp.h>
#include "mex.h"

using namespace Eigen;
using namespace std;

// TODO if we aren't outputting stateseqs, don't write it to memory (just need
// t and t+1), so we can save a stack array for each HMM at the cost of a
// branch

void mexFunction( int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[] )
{

    /* SETUP */
    if (nrhs != 6) { mexErrMsgTxt("wrong number of arguments\n"); }

    //// pull out inputs

    // data
    if (!mxGetField(prhs[0],0,"data")) { mexErrMsgTxt("data missing 'data' field\n"); }
    int8_t *alldata = (int8_t *) mxGetData(mxGetField(prhs[0],0,"data"));
    int bigT = mxGetN(mxGetField(prhs[0],0,"data")); // total length of the data
    int num_subparts = mxGetM(mxGetField(prhs[0],0,"data")); // number of sub-parts

    if (!mxGetField(prhs[0],0,"resources")) { mexErrMsgTxt("data missing 'resources' field\n"); }
    int16_t *allresources = (int16_t *) mxGetData(mxGetField(prhs[0],0,"resources"));

    if (!mxGetField(prhs[0],0,"starts")) { mexErrMsgTxt("data missing 'starts' field\n"); }
    int32_t *starts = (int32_t *) mxGetData(mxGetField(prhs[0],0,"starts"));
    int num_sequences = max(mxGetM(mxGetField(prhs[0],0,"starts")),
            mxGetM(mxGetField(prhs[0],0,"starts")));

    if (!mxGetField(prhs[0],0,"lengths")) { mexErrMsgTxt("data missing 'lengths' field\n"); }
    int32_t *lengths = (int32_t *) mxGetData(mxGetField(prhs[0],0,"lengths"));

    // parameters struct
    if (!mxGetField(prhs[1],0,"learns")) { mexErrMsgTxt("model missing 'learns' field\n"); }
    double *learns = mxGetPr(mxGetField(prhs[1],0,"learns"));
    int num_resources = max(mxGetM(mxGetField(prhs[1],0,"learns")),
            mxGetN(mxGetField(prhs[1],0,"learns")));

    if (!mxGetField(prhs[1],0,"forgets")) { mexErrMsgTxt("model missing 'forgets' field\n"); }
    double *forgets = mxGetPr(mxGetField(prhs[1],0,"forgets"));

    if (!mxGetField(prhs[1],0,"guesses")) { mexErrMsgTxt("model missing 'guesses' field\n"); }
    double *guess = mxGetPr(mxGetField(prhs[1],0,"guesses"));

    if (!mxGetField(prhs[1],0,"slips")) { mexErrMsgTxt("model missing 'slips' field\n"); }
    double *slip = mxGetPr(mxGetField(prhs[1],0,"slips"));

    if (!mxGetField(prhs[1],0,"prior")) { mexErrMsgTxt("model missing 'prior' field\n"); }
    double prior = mxGetScalar(mxGetField(prhs[1],0,"prior"));

    Array2d initial_distn;
    initial_distn << 1-prior, prior;

    MatrixXd As(2,2*num_resources);
    for (int n=0; n<num_resources; n++) {
        As.col(2*n) << 1-learns[n], learns[n];
        As.col(2*n+1) << forgets[n], 1-forgets[n];
    }
    MatrixXd ATs(2,2*num_resources);
    for (int n=0; n<num_resources; n++) {
        ATs.block(0,2*n,2,2) = As.block(0,2*n,2,2).transpose();
    }

    Array2Xd Bn(2,2*num_subparts);
    for (int n=0; n<num_subparts; n++) {
        Bn.col(2*n) << 1-guess[n], slip[n]; // incorrect
        Bn.col(2*n+1) << guess[n], 1-slip[n]; // correct
    }

    // temp
    double temp = mxGetScalar(prhs[5]);
    Bn = (Bn.log()/temp).exp();

    //// outputs

    // rhs outputs
    Map<Array2Xd,Aligned> all_trans_counts((double *) mxGetData(prhs[2]),2,2*num_resources);
    all_trans_counts.setZero();
    Map<Array2Xd,Aligned> all_emission_counts((double *) mxGetData(prhs[3]),2,2*num_subparts);
    all_emission_counts.setZero();
    Map<Array2d,Aligned> all_initial_counts((double *) mxGetData(prhs[4]));
    all_initial_counts.setZero();

    // lhs outputs
    Map<Array2Xd,Aligned> beta_out(NULL,2,bigT);
    int8_t *all_stateseqs = NULL;
    double s_total_loglike = 0;
    double *total_loglike = &s_total_loglike;
    switch (nlhs)
    {
        case 3:
            plhs[2] = mxCreateDoubleMatrix(2,bigT,mxREAL);
            new (&beta_out) Map<Array2Xd,Aligned>(mxGetPr(plhs[2]),2,bigT);
        case 2:
            plhs[1] = mxCreateNumericMatrix(1,bigT,mxINT8_CLASS,mxREAL);
            all_stateseqs = (int8_t *) mxGetData(plhs[1]);
        case 1:
            plhs[0] = mxCreateDoubleScalar(0.);
            total_loglike = mxGetPr(plhs[0]);
    }

    /* COMPUTATION */
    Eigen::initParallel();
    /* omp_set_dynamic(0); */
    /* omp_set_num_threads(6); */
    #pragma omp parallel
    {
        double s_trans_counts[2*2*num_resources], s_emission_counts[2*2*num_subparts],
               s_init_counts[2], loglike;
        memset(s_trans_counts,0,sizeof(s_trans_counts));
        memset(s_emission_counts,0,sizeof(s_emission_counts));
        memset(s_init_counts,0,sizeof(s_init_counts));

        Map<Array2Xd,Aligned> trans_counts(s_trans_counts,2,2*num_resources);
        Map<Array2Xd,Aligned> emission_counts(s_emission_counts,2,2*num_subparts);
        Map<Array2d,Aligned> init_counts(s_init_counts,2,1);

        loglike = 0;

        int num_threads = omp_get_num_threads();
        int blocklen = 1 + ((num_sequences - 1) / num_threads);
        int sequence_idx_start = blocklen * omp_get_thread_num();
        int sequence_idx_end = min(sequence_idx_start+blocklen,num_sequences);

        for (int sequence_index=sequence_idx_start; sequence_index < sequence_idx_end; sequence_index++) {
            // NOTE: -1 because Matlab indexing starts at 1
            int32_t sequence_start = starts[sequence_index] - 1;
            int32_t T = lengths[sequence_index];

            int8_t *data = alldata + num_subparts*sequence_start;
            int16_t *resources = allresources + sequence_start;

            //// likelihoods
            double s_likelihoods[2*T];
            Map<Array2Xd,Aligned> likelihoods(s_likelihoods,2,T);
            likelihoods.setOnes();
            for (int t=0; t<T; t++) {
                for (int n=0; n<num_subparts; n++) {
                    if (data[n+num_subparts*t] != 0) {
                        likelihoods.col(t) *= Bn.col(2*n + (data[n+num_subparts*t] == 2));
                    }
                }
            }

            //// backward messages
            // NOTE: these aren't the standard betas; I'm including the
            // evidence potential at time t in beta.col(t) as well so we don't
            // have to touch the likelihoods on the second pass
            double norm;
            double s_beta[2*T];
            Map<ArrayXXd,Aligned> beta(s_beta,2,T);
            beta.col(T-1) = likelihoods.col(T-1);
            norm = beta.col(T-1).sum();
            beta.col(T-1) /= norm;
            loglike += log(norm);
            for (int t=T-2; t>=0; t--) {
                beta.col(t) = (ATs.block(0,2*(resources[t]-1),2,2) *
                    beta.col(t+1).matrix()).array() * likelihoods.col(t);
                norm = beta.col(t).sum();
                beta.col(t) /= norm;
                loglike += log(norm);
            }
            loglike += log((beta.col(0) * initial_distn).sum());

            //// sample forwards and statistic counting
            int8_t stateseq[T];
            int t=0;
            Array2d nextstate_unsmoothed = initial_distn;
            Array2d nextstate_distr = nextstate_unsmoothed * beta.col(t);
            stateseq[t] = nextstate_distr(0) < nextstate_distr.sum()
                * (((double) random() )/((double) RAND_MAX ));
            nextstate_unsmoothed = As.col(2*(resources[t]-1)+stateseq[t]);
            // initial stats
            s_init_counts[stateseq[t]] += 1.0;
            // singleton stats
            for (int n=0; n<num_subparts; n++) {
                if (data[n+num_subparts*t] != 0) {
                    s_emission_counts[2*(2*n+(data[n+num_subparts*t]==2))+stateseq[t]] += 1.0;
                }
            }
            for (t=1; t<T; t++) {
                Array2d nextstate_distr = nextstate_unsmoothed * beta.col(t);
                stateseq[t] = nextstate_distr(0) < nextstate_distr.sum()
                    * (((double) random() )/((double) RAND_MAX ));
                nextstate_unsmoothed = As.col(2*(resources[t]-1)+stateseq[t]);

                // singleton stats
                for (int n=0; n<num_subparts; n++) {
                    if (data[n+num_subparts*t] != 0) {
                        s_emission_counts[4*n+2*(data[n+num_subparts*t]==2)+stateseq[t]] += 1.0;
                    }
                }

                // pairwise stats
                s_trans_counts[4*(resources[t-1]-1)+2*stateseq[t-1]+stateseq[t]] += 1.0;
            }

            switch (nlhs)
            {
                case 3:
                    beta_out.block(0,sequence_start,2,T) = beta;
                case 2:
                    memcpy(all_stateseqs+sequence_start,stateseq,sizeof(stateseq));
            }
        }

        #pragma omp critical
        {
            all_trans_counts += trans_counts;
            all_emission_counts += emission_counts;
            all_initial_counts += init_counts;
            *total_loglike += loglike;
        }
    }
}


#pragma once

#include <Core/CoreMacros.hpp>
#include <Core/Math/Math.hpp>
#include <Eigen/Core>
#include <Eigen/Geometry> //homogeneous
#include <random>

namespace Ra {
namespace Core {
namespace Utils {

/*!
 * Colors are defined as vector4, i.e. 4 Scalars in RGBA order.
 * displayable colors should have all their coordinates between 0 and 1.
 *
 * \warning Vector arithmetics can be used to add, substract, multiply or divide colors with
 * scalars and other colors. In that case, alpha is treated as any other component. It is fine for
 * most cases, however adding two colors (e.g. Blue()+Red()) may lead to unconsistent
 * alpha: Blue()+Red() = {1, 0, 1, 2}.
 * In that case, only the rgb values need to be added:
 * \code
 * Color result = Color::fromRGB( Blue().rgb()+Red().rgb() ); // result = {1, 0, 1, 1}
 * \endcode
 * The validity of alpha can be checked using hasValidAlpha().
 */
template <typename _Scalar>
class ColorBase : public Eigen::Matrix<_Scalar, 4, 1>
{
  public:
    using VectorType = Eigen::Matrix<_Scalar, 4, 1>;

    explicit inline ColorBase() : ColorBase( _Scalar( 1. ), _Scalar( 1. ), _Scalar( 1. ) ) {}

    template <typename S2>
    inline ColorBase( S2 r, S2 g, S2 b, S2 alpha = S2( 1 ) ) :
        VectorType( _Scalar( r ), _Scalar( g ), _Scalar( b ), _Scalar( alpha ) ) {}

    /// Copy constructor
    template <typename S2>
    inline ColorBase( const ColorBase<S2>& other ) : VectorType( other.template cast<_Scalar>() ) {}

    /// Copy constructor from Eigen expressions: `ColorBase<_Scalar> c( (*this) * 255 );`
    template <typename Derived>
    inline ColorBase( const Eigen::MatrixBase<Derived>& v ) :
        VectorType( v.template cast<_Scalar>() ) {}

    /// cast operator, mandatory to use Vector arithmetic
    operator VectorType() { return *this; }

    /// convert the color expressed in sRGB color space to linear RGB
    static inline ColorBase sRGBToLinearRGB( const ColorBase& srgb ) {
        ColorBase<_Scalar> c( srgb );
        for ( auto& u : c.rgb() )
        {
            if ( u < 0.04045_ra ) { u /= 12.92_ra; }
            else
            { u = std::pow( ( u + 0.055_ra ) / 1.055_ra, 2.4_ra ); }
        }
        return c;
    }

    /// convert the color expressed in linear RGB color space to sRGB
    static inline ColorBase linearRGBTosRGB( const ColorBase& lrgb ) {
        ColorBase<_Scalar> c( lrgb );
        for ( auto& u : c.rgb() )
        {
            if ( u < 0.0031308_ra ) { u *= 12.92_ra; }
            else
            { u = 1.055_ra * std::pow( u, 1_ra / 2.4_ra ) - 0.055_ra; }
        }
        return c;
    }

    template <typename Derived>
    static inline ColorBase fromRGB( const Eigen::MatrixBase<Derived>& rgb,
                                     Scalar alpha = Scalar( 1. ) ) {
        ColorBase c( rgb.template cast<_Scalar>().homogeneous() );
        c.alpha() = alpha;
        return c;
    }

    Eigen::Block<VectorType, 3, 1> rgb() { return ( *this ).template head<3>(); }
    const Eigen::Block<VectorType, 3, 1> rgb() const { return ( *this ).template head<3>(); }

    Scalar alpha() const { return ( *this )( 3 ); }
    Scalar& alpha() { return ( *this )( 3 ); }
    bool hasValidAlpha() const { return Math::checkRange( alpha(), 0_ra, 1_ra ); }

    static inline ColorBase<_Scalar> Alpha() {
        return ColorBase<_Scalar>( _Scalar( 0. ), _Scalar( 0. ), _Scalar( 0. ), _Scalar( 0. ) );
    }

    static inline ColorBase<_Scalar> Black() {
        return ColorBase<_Scalar>( _Scalar( 0. ), _Scalar( 0. ), _Scalar( 0. ) );
    }

    static inline ColorBase<_Scalar> Red() {
        return ColorBase<_Scalar>( _Scalar( 1. ), _Scalar( 0. ), _Scalar( 0. ) );
    }

    static inline ColorBase<_Scalar> Green() {
        return ColorBase<_Scalar>( _Scalar( 0. ), _Scalar( 1. ), _Scalar( 0. ) );
    }

    static inline ColorBase<_Scalar> Blue() {
        return ColorBase<_Scalar>( _Scalar( 0. ), _Scalar( 0. ), _Scalar( 1. ) );
    }

    static inline ColorBase<_Scalar> Yellow() {
        return ColorBase<_Scalar>( _Scalar( 1. ), _Scalar( 1. ), _Scalar( 0. ) );
    }

    static inline ColorBase<_Scalar> Magenta() {
        return ColorBase<_Scalar>( _Scalar( 1. ), _Scalar( 0. ), _Scalar( 1. ) );
    }

    static inline ColorBase<_Scalar> Cyan() {
        return ColorBase<_Scalar>( _Scalar( 0. ), _Scalar( 1. ), _Scalar( 1. ) );
    }

    static inline ColorBase<_Scalar> White() {
        return ColorBase<_Scalar>( _Scalar( 1. ), _Scalar( 1. ), _Scalar( 1. ) );
    }

    static inline ColorBase<_Scalar> Grey( _Scalar f = _Scalar( 0.5 ), _Scalar a = _Scalar( 1. ) ) {
        return ColorBase<_Scalar>( f, f, f, a );
    }

    static inline ColorBase<_Scalar> Skin() {
        return ColorBase<_Scalar>( _Scalar( 1.0 ), _Scalar( 0.87 ), _Scalar( 0.74 ) );
    }
    // Convert to/from various int formats

    static inline ColorBase<_Scalar> fromChars( uchar r, uchar g, uchar b, uchar a = 0xff ) {
        return ColorBase<_Scalar>( _Scalar( r ) / 255.0f,
                                   _Scalar( g ) / 255.0f,
                                   _Scalar( b ) / 255.0f,
                                   _Scalar( a ) / 255.0f );
    }

    static inline ColorBase<_Scalar> fromRGBA32( uint32_t rgba ) {
        uchar r = uchar( ( rgba >> 24 ) & 0xff );
        uchar g = uchar( ( rgba >> 16 ) & 0xff );
        uchar b = uchar( ( rgba >> 8 ) & 0xff );
        uchar a = uchar( ( rgba >> 0 ) & 0xff );
        return fromChars( r, g, b, a );
    }

    static inline ColorBase<_Scalar> fromARGB32( uint32_t argb ) {
        uchar a = uchar( ( argb >> 24 ) & 0xff );
        uchar r = uchar( ( argb >> 16 ) & 0xff );
        uchar g = uchar( ( argb >> 8 ) & 0xff );
        uchar b = uchar( ( argb >> 0 ) & 0xff );
        return fromChars( r, g, b, a );
    }

    static inline ColorBase<_Scalar> fromHSV( const _Scalar hue,
                                              const _Scalar saturation = 1.0,
                                              const _Scalar value      = 1.0,
                                              const _Scalar alpha      = 1.0 ) {
        ColorBase<_Scalar> c;

        if ( saturation == 0.0f )
        {
            c[0] = c[1] = c[2] = value;
            c[3]               = alpha;
            return c;
        }
        _Scalar h  = ( ( hue == 1.0f ) ? 0.0f : hue ) * 6.0f;
        int i      = int( std::floor( h ) );
        _Scalar v1 = value * ( 1.0f - saturation );
        _Scalar v2 = value * ( 1.0f - ( saturation * ( h - i ) ) );
        _Scalar v3 = value * ( 1.0f - ( saturation * ( 1.0f - h - i ) ) );
        switch ( i )
        {
        case 0: {
            c[0] = value;
            c[1] = v3;
            c[2] = v1;
        }
        break;
        case 1: {
            c[0] = v2;
            c[1] = value;
            c[2] = v1;
        }
        break;
        case 2: {
            c[0] = v1;
            c[1] = value;
            c[2] = v3;
        }
        break;
        case 3: {
            c[0] = v1;
            c[1] = v2;
            c[2] = value;
        }
        break;
        case 4: {
            c[0] = v3;
            c[1] = v1;
            c[2] = value;
        }
        break;
        default: {
            c[0] = value;
            c[1] = v1;
            c[2] = v2;
        }
        break;
        }
        c[3] = alpha;
        return c;
    }

    inline uint32_t toRGBA32() const {
        ColorBase<_Scalar> c( ( *this ) * 255 );
        Eigen::Matrix<int, 4, 1> scaled( c.x(), c.y(), c.z(), c.w() );
        return ( uint32_t( scaled( 0 ) ) << 24 ) | ( uint32_t( scaled( 1 ) ) << 16 ) |
               ( uint32_t( scaled( 2 ) ) << 8 ) | ( uint32_t( scaled( 3 ) ) << 0 );
    }

    inline uint32_t toARGB32() const {
        ColorBase<_Scalar> c( ( *this ) * 255 );
        Eigen::Matrix<int, 4, 1> scaled( c.x(), c.y(), c.z(), c.w() );
        return ( uint32_t( scaled( 3 ) ) << 24 ) | ( uint32_t( scaled( 0 ) ) << 16 ) |
               ( uint32_t( scaled( 1 ) ) << 8 ) | ( uint32_t( scaled( 2 ) ) << 0 );
    }

    static inline std::vector<ColorBase<_Scalar>> scatter( const uint size, const _Scalar gamma ) {
        std::vector<ColorBase<_Scalar>> color( size );
        if ( size > 1 )
            for ( uint i = 0; i < size; ++i )
            {
                color[i] = fromHSV( ( _Scalar( i ) / _Scalar( size - 1 ) ) * 0.777 );
                color[i] = ( color[i] + ColorBase<_Scalar>::Constant( gamma ) ) * 0.5;
            }
        else
        { color[0] = Red(); }
        std::shuffle( color.begin(), color.end(), std::mt19937( std::random_device()() ) );
        return color;
    }
};

using Color  = ColorBase<Scalar>;
using Colorf = ColorBase<float>;
using Colord = ColorBase<double>;

} // namespace Utils
} // namespace Core
} // namespace Ra


/*=============================================================================

PHAS0100ASSIGNMENT2: PHAS0100 Assignment 2 Gravitational N-body Simulation

Copyright (c) University College London (UCL). All rights reserved.

This software is distributed WITHOUT ANY WARRANTY; without even
the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
PURPOSE.

See LICENSE.txt in the top level directory for details.

=============================================================================*/

#include <CLI11.hpp>
#include <nbsimBasicTypes.h>
#include <nbsimMyFunctions.h>
#include <nbsimExceptionMacro.h>
#include <iostream>
#include <string.h>
#include <stdio.h>
#include <ctime>
#include <chrono>
#include <omp.h>
#include <iomanip>
#include "nbsimSolarSystemData.ipp"

// Example, header-only library, included in project for simplicity's sake.
#include <Eigen/Dense>
#include <unsupported/Eigen/NonLinearOptimization>

#define YEAR2SEC 365*24*60*60
/**
 * \brief Demo file to check that includes and library linkage is correct.
 */
int main(int argc, char** argv)
{

	int returnStatus = EXIT_FAILURE;
	int ibody = 0;
	Eigen::Vector3d pos, vel, r, rcom, p;
	double mass, mtotal;
	std::shared_ptr<nbsim::MassiveParticle> planet[9];
	double ts = 0.0000274 * YEAR2SEC, dur = 1* YEAR2SEC;

	std::string names[9];
	CLI::App app{"Solar system simulator"};
	//app.require_subcommand(1);

	CLI11_PARSE(app, argc, argv);

	/* Init planets */
	for (int ibody=0; ibody <9; ibody++) {
		//std::cout << nbsim::solarSystemData[ibody].name << std::endl;
		names[ibody] = nbsim::solarSystemData[ibody].name;
		mass = nbsim::solarSystemData[ibody].mu / GRAV;
		pos = nbsim::solarSystemData[ibody].position;
		vel = nbsim::solarSystemData[ibody].velocity;
		planet[ibody] = std::make_shared<nbsim::MassiveParticle>(pos, vel, mass);
	}
	/* Add mutual attraction */
	for (int i=0; i <9; i++) {
		for (int j=0; j<9; j++) {
			//printf("Adding %d to %d\n", j, i);
			if (i != j) planet[i]->addAttractor(planet[j]);
		}
	}

	/* start timing */
	std::clock_t c_start = std::clock();
	auto t_start = std::chrono::high_resolution_clock::now();
	/* simulate */
	for (double t=0; t<dur; t+=ts) {
#ifdef DEBUG
		{
			/* calculate rcom and ptotal */
			rcom << 0,0,0;
			p << 0,0,0;
			mtotal = 0;
			for (int i=0; i<9; i++) {
				mass = planet[i]->getMass();
				rcom += mass * planet[i]->getPosition();
				p += mass * planet[i]->getVelocity();
				mtotal += mass;
			}
			rcom = rcom / mtotal;
			std::cout << "rcom = " << std::endl << rcom << std::endl
				<< "ptotal = " << std::endl << p <<std::endl
				<< "mtotal = " << mtotal << std::endl;
		}
#endif
#pragma omp parallel for
		for (int i=0; i<9; i++) {
			planet[i]->calculateAcceleration();
		}
#pragma omp parallel for
		for (int i=0; i<9; i++) {
			planet[i]->integrateTimestep(ts);
		}
	}
	/* stop timing */
    std::clock_t c_end = std::clock();
    auto t_end = std::chrono::high_resolution_clock::now();

	/* output res */
	for (int i=0; i<9; i++) {
		std::cout << names[i] << std::endl;
		std::cout << planet[i]->getPosition() << std::endl;
	}
		{
			/* calculate rcom and ptotal */
			rcom << 0,0,0;
			p << 0,0,0;
			mtotal = 0;
			for (int i=0; i<9; i++) {
				mass = planet[i]->getMass();
				rcom += mass * planet[i]->getPosition();
				p += mass * planet[i]->getVelocity();
				mtotal += mass;
			}
			rcom = rcom / mtotal;
			std::cout << "rcom = " << std::endl << rcom << std::endl
				<< "ptotal = " << std::endl << p <<std::endl
				<< "mtotal = " << mtotal << std::endl;
		}

    std::cout << std::fixed << std::setprecision(2) << "CPU time used: "
              << 1000.0 * (c_end - c_start) / CLOCKS_PER_SEC / 1000 << " s\n"
              << "Wall clock time passed: "
              << std::chrono::duration<double, std::milli>(t_end-t_start).count() / 1000
              << " s\n";
	return 0;
}


#include <boost/numeric/odeint/stepper/bulirsch_stoer.hpp>


#ifndef MLT_MODELS_CLASSIFIERS_CLASSIFIER_HPP
#define MLT_MODELS_CLASSIFIERS_CLASSIFIER_HPP

#include <Eigen/Core>

#include "../base.hpp"
#include "../../utils/eigen.hpp"

namespace mlt {
namespace models {
namespace classifiers {
	using namespace utils::eigen;

	template <class ConcreteType>
	class Classifier : public Predictor<ConcreteType, VectorXi> {
	public:
		inline auto score(Features input, Target target) const {
			return (_self().predict(input).array() == target.array()).cast<double>().sum() / static_cast<double>(input.cols());
		}

	protected:
		Classifier() = default;
		Classifier(const Classifier&) = default;
		Classifier(Classifier&&) = default;
		Classifier& operator=(const Classifier&) = default;
		~Classifier() = default;

		inline auto _to_target_matrix(Target target) {
			return classes_vector_to_classes_matrix(target).cast<double>().eval();
		}
	};
}
}
}
#endif


// Boost.Geometry (aka GGL, Generic Geometry Library)

// Copyright (c) 2007-2015 Barend Gehrels, Amsterdam, the Netherlands.
// Copyright (c) 2008-2015 Bruno Lalande, Paris, France.
// Copyright (c) 2009-2015 Mateusz Loskot, London, UK.

// This file was modified by Oracle on 2015.
// Modifications copyright (c) 2015 Oracle and/or its affiliates.

// Contributed and/or modified by Menelaos Karavelas, on behalf of Oracle

// Parts of Boost.Geometry are redesigned from Geodan's Geographic Library
// (geolib/GGL), copyright (c) 1995-2010 Geodan, Amsterdam, the Netherlands.

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

#ifndef BOOST_GEOMETRY_STRATEGIES_TRANSFORM_MATRIX_TRANSFORMERS_HPP
#define BOOST_GEOMETRY_STRATEGIES_TRANSFORM_MATRIX_TRANSFORMERS_HPP

#include <cstddef>

#include <boost/qvm/mat.hpp>
#include <boost/qvm/vec.hpp>
#include <boost/qvm/mat_access.hpp>
#include <boost/qvm/vec_access.hpp>
#include <boost/qvm/mat_operations.hpp>
#include <boost/qvm/vec_mat_operations.hpp>
#include <boost/qvm/map_mat_mat.hpp>
#include <boost/qvm/map_mat_vec.hpp>

#include <boost/geometry/core/access.hpp>
#include <boost/geometry/core/coordinate_dimension.hpp>
#include <boost/geometry/core/coordinate_promotion.hpp>
#include <boost/geometry/core/cs.hpp>
#include <boost/geometry/util/math.hpp>
#include <boost/geometry/util/select_coordinate_type.hpp>
#include <boost/geometry/util/select_most_precise.hpp>

namespace boost { namespace geometry
{

namespace strategy { namespace transform
{

namespace detail { namespace matrix_transformer
{

template
<
    typename Point,
    std::size_t Dimension = 0,
    std::size_t DimensionCount = geometry::dimension<Point>::value
>
struct set_point_from_vec
{
    template <typename Vector>
    static inline void apply(Point & p, Vector const& v)
    {
        typedef typename geometry::coordinate_type<Point>::type coord_t;
        set<Dimension>(p, boost::numeric_cast<coord_t>(qvm::A<Dimension>(v)));
        set_point_from_vec<Point, Dimension + 1, DimensionCount>::apply(p, v);
    }
};

template
<
    typename Point,
    std::size_t DimensionCount
>
struct set_point_from_vec<Point, DimensionCount, DimensionCount>
{
    template <typename Vector>
    static inline void apply(Point &, Vector const&) {}
};

template
<
    typename Point,
    std::size_t Dimension = 0,
    std::size_t DimensionCount = geometry::dimension<Point>::value
>
struct set_vec_from_point
{
    template <typename Vector>
    static inline void apply(Point const& p, Vector & v)
    {
        qvm::A<Dimension>(v) = get<Dimension>(p);
        set_vec_from_point<Point, Dimension + 1, DimensionCount>::apply(p, v);
    }
};

template
<
    typename Point,
    std::size_t DimensionCount
>
struct set_vec_from_point<Point, DimensionCount, DimensionCount>
{
    template <typename Vector>
    static inline void apply(Point const&, Vector &) {}
};

template
<
    typename CalculationType,
    std::size_t Dimension1,
    std::size_t Dimension2
>
class matrix_transformer
{
protected :
    typedef CalculationType ct;
    typedef boost::qvm::mat<ct, Dimension2 + 1, Dimension1 + 1> matrix_type;
    matrix_type m_matrix;
public :
    matrix_type const& matrix() const { return m_matrix; }
    template <typename P1, typename P2>
    inline bool apply(P1 const& p1, P2& p2) const
    {
        assert_dimension_greater_equal<P1,Dimension1>();
        assert_dimension_greater_equal<P2,Dimension2>();
        qvm::vec<ct,Dimension1 + 1> p1temp;
        qvm::A<Dimension1>(p1temp) = 1;
        qvm::vec<ct,Dimension2 + 1> p2temp;
        set_vec_from_point<P1, 0, Dimension1>::apply(p1, p1temp);
        p2temp = m_matrix * p1temp;
        set_point_from_vec<P2, 0, Dimension2>::apply(p2, p2temp);
        return true;
    }

};

}} // namespace detail::matrix_transform

/*!
\brief Affine transformation strategy in Cartesian system.
\details The strategy serves as a generic definition of an affine transformation
         matrix and procedure for applying it to a given point.
\see http://en.wikipedia.org/wiki/Affine_transformation
     and http://www.devmaster.net/wiki/Transformation_matrices
\ingroup strategies
\tparam Dimension1 number of dimensions to transform from
\tparam Dimension2 number of dimensions to transform to
 */
template
<
    typename CalculationType,
    std::size_t Dimension1,
    std::size_t Dimension2
>
class matrix_transformer : public detail::matrix_transformer::matrix_transformer<CalculationType, Dimension1, Dimension2>
{
public:
    template<typename Matrix>
    inline matrix_transformer(Matrix const& matrix)
    {
        qvm::assign(this->m_matrix, matrix);
    }
    inline matrix_transformer() {}
};

template <typename CalculationType>
class matrix_transformer<CalculationType, 2, 2> : public detail::matrix_transformer::matrix_transformer<CalculationType, 2, 2>
{
    typedef CalculationType ct;
public :
    template<typename Matrix>
    inline matrix_transformer(Matrix const& matrix)
    {
        qvm::assign(this->m_matrix, matrix);
    }

    inline matrix_transformer() {}

    inline matrix_transformer(
                ct const& m_0_0, ct const& m_0_1, ct const& m_0_2,
                ct const& m_1_0, ct const& m_1_1, ct const& m_1_2,
                ct const& m_2_0, ct const& m_2_1, ct const& m_2_2)
    {
        qvm::A<0,0>(this->m_matrix) = m_0_0;   qvm::A<0,1>(this->m_matrix) = m_0_1;   qvm::A<0,2>(this->m_matrix) = m_0_2;
        qvm::A<1,0>(this->m_matrix) = m_1_0;   qvm::A<1,1>(this->m_matrix) = m_1_1;   qvm::A<1,2>(this->m_matrix) = m_1_2;
        qvm::A<2,0>(this->m_matrix) = m_2_0;   qvm::A<2,1>(this->m_matrix) = m_2_1;   qvm::A<2,2>(this->m_matrix) = m_2_2;
    }

    template <typename P1, typename P2>
    inline bool apply(P1 const& p1, P2& p2) const
    {
        assert_dimension_greater_equal<P1, 2>();
        assert_dimension_greater_equal<P2, 2>();

        ct const& c1 = get<0>(p1);
        ct const& c2 = get<1>(p1);

        typedef typename geometry::coordinate_type<P2>::type ct2;
        set<0>(p2, boost::numeric_cast<ct2>(c1 * qvm::A<0,0>(this->m_matrix) + c2 * qvm::A<0,1>(this->m_matrix) + qvm::A<0,2>(this->m_matrix)));
        set<1>(p2, boost::numeric_cast<ct2>(c1 * qvm::A<1,0>(this->m_matrix) + c2 * qvm::A<1,1>(this->m_matrix) + qvm::A<1,2>(this->m_matrix)));

        return true;
    }
};

// It IS possible to go from 3 to 2 coordinates
template <typename CalculationType>
class matrix_transformer<CalculationType, 3, 2> : public detail::matrix_transformer::matrix_transformer<CalculationType, 3, 2>
{
    typedef CalculationType ct;
public :
    template<typename Matrix>
    inline matrix_transformer(Matrix const& matrix)
    {
        qvm::assign(this->m_matrix, matrix);
    }

    inline matrix_transformer() {}

    inline matrix_transformer(
                ct const& m_0_0, ct const& m_0_1, ct const& m_0_2,
                ct const& m_1_0, ct const& m_1_1, ct const& m_1_2,
                ct const& m_2_0, ct const& m_2_1, ct const& m_2_2)
    {
        qvm::A<0,0>(this->m_matrix) = m_0_0;   qvm::A<0,1>(this->m_matrix) = m_0_1;   qvm::A<0,2>(this->m_matrix) = 0;   qvm::A<0,3>(this->m_matrix) = m_0_2;
        qvm::A<1,0>(this->m_matrix) = m_1_0;   qvm::A<1,1>(this->m_matrix) = m_1_1;   qvm::A<1,2>(this->m_matrix) = 0;   qvm::A<1,3>(this->m_matrix) = m_1_2;
        qvm::A<2,0>(this->m_matrix) = m_2_0;   qvm::A<2,1>(this->m_matrix) = m_2_1;   qvm::A<2,2>(this->m_matrix) = 0;   qvm::A<2,3>(this->m_matrix) = m_2_2;
    }

    template <typename P1, typename P2>
    inline bool apply(P1 const& p1, P2& p2) const
    {
        assert_dimension_greater_equal<P1, 3>();
        assert_dimension_greater_equal<P2, 2>();

        ct const& c1 = get<0>(p1);
        ct const& c2 = get<1>(p1);
        ct const& c3 = get<2>(p1);

        typedef typename geometry::coordinate_type<P2>::type ct2;

        set<0>(p2, boost::numeric_cast<ct2>(
            c1 * qvm::A<0,0>(this->m_matrix) + c2 * qvm::A<0,1>(this->m_matrix) + c3 * qvm::A<0,2>(this->m_matrix) + qvm::A<0,3>(this->m_matrix)));
        set<1>(p2, boost::numeric_cast<ct2>(
            c1 * qvm::A<1,0>(this->m_matrix) + c2 * qvm::A<1,1>(this->m_matrix) + c3 * qvm::A<1,2>(this->m_matrix) + qvm::A<1,3>(this->m_matrix)));

        return true;
    }

};

template <typename CalculationType>
class matrix_transformer<CalculationType, 3, 3> : public detail::matrix_transformer::matrix_transformer<CalculationType, 3, 3>
{
    typedef CalculationType ct;
public :
    template<typename Matrix>
    inline matrix_transformer(Matrix const& matrix)
    {
        qvm::assign(this->m_matrix, matrix);
    }

    inline matrix_transformer() {}

    inline matrix_transformer(
                ct const& m_0_0, ct const& m_0_1, ct const& m_0_2, ct const& m_0_3,
                ct const& m_1_0, ct const& m_1_1, ct const& m_1_2, ct const& m_1_3,
                ct const& m_2_0, ct const& m_2_1, ct const& m_2_2, ct const& m_2_3,
                ct const& m_3_0, ct const& m_3_1, ct const& m_3_2, ct const& m_3_3
                )
    {
        qvm::A<0,0>(this->m_matrix) = m_0_0; qvm::A<0,1>(this->m_matrix) = m_0_1; qvm::A<0,2>(this->m_matrix) = m_0_2; qvm::A<0,3>(this->m_matrix) = m_0_3;
        qvm::A<1,0>(this->m_matrix) = m_1_0; qvm::A<1,1>(this->m_matrix) = m_1_1; qvm::A<1,2>(this->m_matrix) = m_1_2; qvm::A<1,3>(this->m_matrix) = m_1_3;
        qvm::A<2,0>(this->m_matrix) = m_2_0; qvm::A<2,1>(this->m_matrix) = m_2_1; qvm::A<2,2>(this->m_matrix) = m_2_2; qvm::A<2,3>(this->m_matrix) = m_2_3;
        qvm::A<3,0>(this->m_matrix) = m_3_0; qvm::A<3,1>(this->m_matrix) = m_3_1; qvm::A<3,2>(this->m_matrix) = m_3_2; qvm::A<3,3>(this->m_matrix) = m_3_3;
    }

    template <typename P1, typename P2>
    inline bool apply(P1 const& p1, P2& p2) const
    {
        assert_dimension_greater_equal<P1, 3>();
        assert_dimension_greater_equal<P2, 3>();

        ct const& c1 = get<0>(p1);
        ct const& c2 = get<1>(p1);
        ct const& c3 = get<2>(p1);

        typedef typename geometry::coordinate_type<P2>::type ct2;

        set<0>(p2, boost::numeric_cast<ct2>(
            c1 * qvm::A<0,0>(this->m_matrix) + c2 * qvm::A<0,1>(this->m_matrix) + c3 * qvm::A<0,2>(this->m_matrix) + qvm::A<0,3>(this->m_matrix)));
        set<1>(p2, boost::numeric_cast<ct2>(
            c1 * qvm::A<1,0>(this->m_matrix) + c2 * qvm::A<1,1>(this->m_matrix) + c3 * qvm::A<1,2>(this->m_matrix) + qvm::A<1,3>(this->m_matrix)));
        set<2>(p2, boost::numeric_cast<ct2>(
            c1 * qvm::A<2,0>(this->m_matrix) + c2 * qvm::A<2,1>(this->m_matrix) + c3 * qvm::A<2,2>(this->m_matrix) + qvm::A<2,3>(this->m_matrix)));

        return true;
    }
};

/*!
\brief Strategy of translate transformation in Cartesian system.
\details Translate moves a geometry a fixed distance in 2 or 3 dimensions.
\see http://en.wikipedia.org/wiki/Translation_%28geometry%29
\ingroup strategies
\tparam Dimension1 number of dimensions to transform from
\tparam Dimension2 number of dimensions to transform to
 */
template
<
    typename CalculationType,
    std::size_t Dimension1,
    std::size_t Dimension2
>
class translate_transformer
{
};

template<typename CalculationType>
class translate_transformer<CalculationType, 2, 2> : public matrix_transformer<CalculationType, 2, 2>
{
public :
    // To have translate transformers compatible for 2/3 dimensions, the
    // constructor takes an optional third argument doing nothing.
    inline translate_transformer(CalculationType const& translate_x,
                CalculationType const& translate_y,
                CalculationType const& = 0)
        : matrix_transformer<CalculationType, 2, 2>(
                1, 0, translate_x,
                0, 1, translate_y,
                0, 0, 1)
    {}
};

template <typename CalculationType>
class translate_transformer<CalculationType, 3, 3> : public matrix_transformer<CalculationType, 3, 3>
{
public :
    inline translate_transformer(CalculationType const& translate_x,
                CalculationType const& translate_y,
                CalculationType const& translate_z)
        : matrix_transformer<CalculationType, 3, 3>(
                1, 0, 0, translate_x,
                0, 1, 0, translate_y,
                0, 0, 1, translate_z,
                0, 0, 0, 1)
    {}

};

/*!
\brief Strategy of scale transformation in Cartesian system.
\details Scale scales a geometry up or down in all its dimensions.
\see http://en.wikipedia.org/wiki/Scaling_%28geometry%29
\ingroup strategies
\tparam Dimension1 number of dimensions to transform from
\tparam Dimension2 number of dimensions to transform to
*/
template
<
    typename CalculationType,
    std::size_t Dimension1,
    std::size_t Dimension2
>
class scale_transformer
{
};

template
<
    typename CalculationType,
    std::size_t Dimension1
>
class scale_transformer<CalculationType, Dimension1, Dimension1> : public matrix_transformer<CalculationType, Dimension1, Dimension1>
{
public:
    inline scale_transformer(CalculationType const& scale)
    {
        boost::qvm::set_identity(this->m_matrix);
        this->m_matrix*=scale;
        qvm::A<Dimension1,Dimension1>(this->m_matrix) = 1;
    }
};

template <typename CalculationType>
class scale_transformer<CalculationType, 2, 2> : public matrix_transformer<CalculationType, 2, 2>
{

public :
    inline scale_transformer(CalculationType const& scale_x,
                CalculationType const& scale_y,
                CalculationType const& = 0)
        : matrix_transformer<CalculationType, 2, 2>(
                scale_x, 0,       0,
                0,       scale_y, 0,
                0,       0,       1)
    {}

    inline scale_transformer(CalculationType const& scale)
        : matrix_transformer<CalculationType, 2, 2>(
                scale, 0,     0,
                0,     scale, 0,
                0,     0,     1)
    {}
};

template <typename CalculationType>
class scale_transformer<CalculationType, 3, 3> : public matrix_transformer<CalculationType, 3, 3>
{
public :
    inline scale_transformer(CalculationType const& scale_x,
                CalculationType const& scale_y,
                CalculationType const& scale_z)
        : matrix_transformer<CalculationType, 3, 3>(
                scale_x, 0,       0,       0,
                0,       scale_y, 0,       0,
                0,       0,       scale_z, 0,
                0,       0,       0,       1)
    {}

    inline scale_transformer(CalculationType const& scale)
        : matrix_transformer<CalculationType, 3, 3>(
                scale, 0,     0,     0,
                0,     scale, 0,     0,
                0,     0,     scale, 0,
                0,     0,     0,     1)
    {}
};

#ifndef DOXYGEN_NO_DETAIL
namespace detail
{

template <typename DegreeOrRadian>
struct as_radian
{};

template <>
struct as_radian<radian>
{
    template <typename T>
    static inline T get(T const& value)
    {
        return value;
    }
};

template <>
struct as_radian<degree>
{
    template <typename T>
    static inline T get(T const& value)
    {
        typedef typename promote_floating_point<T>::type promoted_type;
        return value * math::d2r<promoted_type>();
    }

};

template
<
    typename CalculationType,
    std::size_t Dimension1,
    std::size_t Dimension2
>
class rad_rotate_transformer
    : public transform::matrix_transformer<CalculationType, Dimension1, Dimension2>
{
public :
    inline rad_rotate_transformer(CalculationType const& angle)
        : transform::matrix_transformer<CalculationType, Dimension1, Dimension2>(
                 cos(angle), sin(angle), 0,
                -sin(angle), cos(angle), 0,
                 0,          0,          1)
    {}
};

} // namespace detail
#endif // DOXYGEN_NO_DETAIL

/*!
\brief Strategy for rotate transformation in Cartesian coordinate system.
\details Rotate rotates a geometry by a specified angle about a fixed point (e.g. origin).
\see http://en.wikipedia.org/wiki/Rotation_%28mathematics%29
\ingroup strategies
\tparam DegreeOrRadian degree/or/radian, type of rotation angle specification
\note A single angle is needed to specify a rotation in 2D.
      Not yet in 3D, the 3D version requires special things to allow
      for rotation around X, Y, Z or arbitrary axis.
\todo The 3D version will not compile.
 */
template
<
    typename DegreeOrRadian,
    typename CalculationType,
    std::size_t Dimension1,
    std::size_t Dimension2
>
class rotate_transformer : public detail::rad_rotate_transformer<CalculationType, Dimension1, Dimension2>
{

public :
    inline rotate_transformer(CalculationType const& angle)
        : detail::rad_rotate_transformer
            <
                CalculationType, Dimension1, Dimension2
            >(detail::as_radian<DegreeOrRadian>::get(angle))
    {}
};

}} // namespace strategy::transform

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_STRATEGIES_TRANSFORM_MATRIX_TRANSFORMERS_HPP


/*
 *  strassen_rectWise.cpp
 *  July 17
 *
 *  Edited by Sarah Loos
 *  Project: Arcee
 *
 *  Copyright (c) 2007-2008 The Trustees of Indiana University. All rights reserved.
 *
 */

#include <iostream>
#include <string>
#include <boost/test/minimal.hpp>
#include <boost/tuple/tuple.hpp>
//#include <papi.h>
#include <vector>

#include <boost/numeric/mtl/matrix/dense2D.hpp>
#include <boost/numeric/mtl/matrix/morton_dense.hpp>
#include <boost/numeric/mtl/matrix/transposed_view.hpp>
#include <boost/numeric/mtl/matrix/parameter.hpp>
#include <boost/numeric/mtl/operation/print_matrix.hpp>
#include <boost/numeric/mtl/operation/sub_matrix.hpp>
#include <boost/numeric/mtl/recursion/matrix_recursator.hpp>
// Replaced preceding file: recurator -> recursator.
#include <boost/numeric/mtl/recursion/base_case_test.hpp>
#include <boost/numeric/mtl/recursion/for_each.hpp>
#include <boost/numeric/mtl/recursion/base_case_cast.hpp>
#include <boost/numeric/mtl/recursion/base_case_matrix.hpp>
#include <boost/numeric/mtl/recursion/bit_masking.hpp>

using namespace mtl;
using namespace std;
using namespace mtl::recursion;

#define NUM_EVENTS 4

const unsigned long mask = generate_mask<true,  2 ,row_major, 0>::value;
typedef morton_dense<double,  mask> matrix_type;

// 'order' Can be passed as command line parameter, see main -- pg

const int basecasesize =4;
typedef recursion::bound_test_static<basecasesize> BaseTest;
BaseTest    is_base;

int  callnum = 0, basehit = 0;
int docholcall=0;
int schurcall=0;
int trischurcall=0;
int trisolvecall=0;

#define lgMtx 4

#define allocTempMtx(lev)                               \
matrix_type M0_##lev(1<<(lgMtx-lev-1),1<<(lgMtx-lev-1));\
matrix_type M1_##lev(1<<(lgMtx-lev-1),1<<(lgMtx-lev-1));\
matrix_type M2_##lev(1<<(lgMtx-lev-1),1<<(lgMtx-lev-1));\
matrix_type M3_##lev(1<<(lgMtx-lev-1),1<<(lgMtx-lev-1));\
matrix_type M4_##lev(1<<(lgMtx-lev-1),1<<(lgMtx-lev-1));\
matrix_type M5_##lev(1<<(lgMtx-lev-1),1<<(lgMtx-lev-1));

typedef mat::recursator<matrix_type> recursator_t;

std::vector<recursator_t> M0;
std::vector<recursator_t> M1;
std::vector<recursator_t> M2;
std::vector<recursator_t> M3;
std::vector<recursator_t> M4;
std::vector<recursator_t> M5;

allocTempMtx(0);
allocTempMtx(1);
/*allocTempMtx(2);
allocTempMtx(3);
allocTempMtx(4);
allocTempMtx(5);*/

template <typename Matrix>
void print_matrix(Matrix& matrix){
  for (int i=0 ; i<matrix.num_rows(); i++ ){
    for(int j=0; j<matrix.num_cols();  j++ ){
      cout.fill (' '); cout.width (8); cout.precision (5); cout.flags (ios_base::left);
      cout << showpoint << /*hex << *reinterpret_cast<long long*>(&*/matrix[i][j] <<"  ";
    }
    cout << endl;
  }
  return;
}

template <typename Matrix>
void fill_matrix_zero(Matrix& matrix){
  for(int i = 0; i < matrix.num_rows(); i++)
    for(int j = 0; j < matrix.num_cols(); j++)
      matrix[i][j] = 0;
}

template <typename Matrix>
void fill_matrix_n(Matrix& matrix){
  for(int i = 0; i < matrix.num_rows(); i++)
    for(int j = 0; j < matrix.num_cols(); j++)
      matrix[i][j] = 2;
}

template < typename Matrix>
void add_base(Matrix & C, Matrix const& A, Matrix const& B)
{
    assert(num_cols(A) == num_cols(B)); assert(num_cols(A) == num_cols(C));
    assert(num_rows(A) == num_rows(B)); assert(num_rows(A) == num_rows(C));

  for (int i = 0; i < basecasesize && i < num_rows(A); i++)
    for (int j = 0; j < basecasesize && j < num_cols(A); j++)
      C[i][j] = A[i][j] + B[i][j];
}

template <typename Recursator>
void add_m(Recursator C, Recursator A, Recursator B)
{
  if (is_base(A))
  {
    using recursion::base_case_cast;
    typename recursion::base_case_matrix<typename Recursator::matrix_type, BaseTest>::type
      prod= base_case_cast<BaseTest>(C.get_value());
      add_base(prod,
	       base_case_cast<BaseTest>(A.get_value()),
	       base_case_cast<BaseTest>(B.get_value()));
  }
  else
  {
    add_m(C.north_west(), A.north_west(), B.north_west());
    add_m(C.north_east(), A.north_east(), B.north_east());
    add_m(C.south_east(), A.south_east(), B.south_east());
    add_m(C.south_west(), A.south_west(), B.south_west());
  }
  return;
}

template < typename Matrix>
void sub_base(Matrix & C, Matrix const& A, Matrix const& B)
{
  for (int i = 0; i < basecasesize; i++)
    for (int j = 0; j < basecasesize; j++)
      C[i][j] = A[i][j] - B[i][j];
}

template <typename Recursator>
void sub_m(Recursator C, Recursator A, Recursator B)
{
  if (is_base(A))
  {
    using recursion::base_case_cast;
    typename recursion::base_case_matrix<typename Recursator::matrix_type, BaseTest>::type
      prod= base_case_cast<BaseTest>(C.get_value());
      sub_base(prod,
	       base_case_cast<BaseTest>(A.get_value()),
	       base_case_cast<BaseTest>(B.get_value()));
  }
  else
  {
    sub_m(C.north_west(), A.north_west(), B.north_west());
    sub_m(C.north_east(), A.north_east(), B.north_east());
    sub_m(C.south_east(), A.south_east(), B.south_east());
    sub_m(C.south_west(), A.south_west(), B.south_west());
  }
  return;
}

template < typename Matrix>
void strassen_base(Matrix & P, Matrix const& A, Matrix const& B)
{
  fill_matrix_zero(P);
  for (int i = 0; i < basecasesize; i++)
    for (int k = 0; k < basecasesize; k++)     //switching j and k lines cuts
      for (int j = 0; j < basecasesize; j++)   //  run time in half.
	P[i][j] += A[i][k] * B[k][j];
}

template <typename Recursator>
void strassen(Recursator P, Recursator A, Recursator B, int lev)
{
  if (A.is_empty() || B.is_empty())
    return;
  if (is_base (A))
  {
    using recursion::base_case_cast;
    typename recursion::base_case_matrix<typename Recursator::matrix_type, BaseTest>::type
      prod= base_case_cast<BaseTest>(P.get_value());
    strassen_base(prod, base_case_cast<BaseTest>(A.get_value()),
		  base_case_cast<BaseTest>(B.get_value()));
    return;
  }
  else
  {

    // M1 = (Anw + Ase)*(Bnw + Bse)
    add_m   (M5[lev],        A.north_west(), A.south_east());
    add_m   (M0[lev],        B.north_west(), B.south_east());
    strassen(M1[lev],        M5[lev],        M0[lev],       lev+1);

    // M2 = (Asw + Ase)*Bnw
    add_m   (M5[lev],        A.south_west(), A.south_east());
    strassen(M2[lev],        M5[lev],        B.north_west(), lev+1);

    // M3 = (Asw - Anw)*(Bnw + Bne)
    add_m   (M5[lev],        B.north_west(), B.north_east());
    sub_m   (M0[lev],        A.south_west(), A.north_west());
    strassen(M3[lev],        M0[lev],        M5[lev],       lev+1);

    // M4 = Anw*(Bne - Bse)
    sub_m   (M5[lev],        B.north_east(), B.south_east());
    strassen(M4[lev],        A.north_west(), M5[lev],       lev+1);

    // Pse = M1 - M2 + M4 + M3
    add_m   (M5[lev],        M1[lev],        M4[lev]      );
    add_m   (M0[lev],        M5[lev],        M3[lev]      );
    sub_m   (P.south_east(), M0[lev],        M2[lev]      );

    // M5 = (Anw + Ane)*Bse
    add_m   (M5[lev],        A.north_west(), A.north_east());
    strassen(M3[lev],        M5[lev],        B.south_east(), lev+1);

    // Pne = M4 + M5
    add_m   (P.north_east(), M4[lev],        M3[lev]      );

    // M7 = Ase*(Bsw - Bnw)
    sub_m   (M5[lev],        B.south_west(), B.north_west());
    strassen(M4[lev],        A.south_east(), M5[lev],       lev+1);

    // Psw = M2 + M7
    add_m   (P.south_west(), M2[lev],        M4[lev]      );

    // M6 = (Ane - Ase)*(Bsw + Bse)
    add_m   (M5[lev],        B.south_west(), B.south_east());
    sub_m   (M0[lev],        A.north_east(), A.south_east());
    strassen(M2[lev],        M0[lev],        M5[lev],       lev+1);

    // Pnw = M1 + M7 - M5 + M6
    add_m   (M5[lev],        M1[lev],        M4[lev]      );
    add_m   (M0[lev],        M5[lev],        M2[lev]      );
    sub_m   (P.north_west(), M0[lev],        M3[lev]      );

    return;
  }

}

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

#define makeRecs(lev)             \
 recursator_t rM0_##lev(M0_##lev);\
 recursator_t rM1_##lev(M1_##lev);\
 recursator_t rM2_##lev(M2_##lev);\
 recursator_t rM3_##lev(M3_##lev);\
 recursator_t rM4_##lev(M4_##lev);\
 recursator_t rM5_##lev(M5_##lev);\
                                  \
 M0.push_back(rM0_##lev);         \
 M1.push_back(rM1_##lev);         \
 M2.push_back(rM2_##lev);         \
 M3.push_back(rM3_##lev);         \
 M4.push_back(rM4_##lev);         \
 M5.push_back(rM5_##lev);

makeRecs(0);
makeRecs(1);
/*makeRecs(2);
makeRecs(3);
makeRecs(4);
makeRecs(5);*/

  int order=14;
  if (argc > 1) order= atoi(argv[1]);

  matrix_type matrix_A(order,order);
  matrix_type matrix_B(order,order);
  matrix_type matrix_P(order,order);

  fill_matrix_n(matrix_A);
  print_matrix(matrix_A);  printf("\n");
  fill_matrix_n(matrix_B);
  print_matrix(matrix_B);
  fill_matrix_zero(matrix_P);  printf("\n");

  recursion::mat::recursator<matrix_type> recursator_P(matrix_P),
                                            recursator_A(matrix_A),
                                            recursator_B(matrix_B);

  strassen(recursator_P, recursator_A, recursator_B, 0);

  print_matrix(matrix_P);

  return 0;
}


/*
	Copyright (C) 2013-2014 by Kristina Simpson <sweet.kristas@gmail.com>

	This software is provided 'as-is', without any express or implied
	warranty. In no event will the authors be held liable for any damages
	arising from the use of this software.

	Permission is granted to anyone to use this software for any purpose,
	including commercial applications, and to alter it and redistribute it
	freely, subject to the following restrictions:

	   1. The origin of this software must not be misrepresented; you must not
	   claim that you wrote the original software. If you use this software
	   in a product, an acknowledgment in the product documentation would be
	   appreciated but is not required.

	   2. Altered source versions must be plainly marked as such, and must not be
	   misrepresented as being the original software.

	   3. This notice may not be removed or altered from any source
	   distribution.
*/

#include <boost/tokenizer.hpp>
#include <sstream>

#include <cfloat>
#include <cmath>
#include "asserts.hpp"
#include "svg_transform.hpp"

namespace KRE
{
	namespace SVG
	{
		class matrix_transform : public transform
		{
		public:
			matrix_transform(const std::vector<double>& params) : transform(TransformType::MATRIX) {
				// Ordering of matrix parameters is as follows.
				// [ 0  2  4 ]
				// [ 1  3  5 ]
				// which neatly is the same order as cairo expects.
				ASSERT_LOG(params.size() == 6,
					"Parsing transform:matrix found "
					<< params.size()
					<< " parameter(s), expected 6");
				cairo_matrix_init(&mat_, params[0], params[1], params[2], params[3], params[4], params[5]);
			}
			virtual ~matrix_transform() {}
			std::string as_string() const override {
				std::stringstream str;
				str << "matrix("
					<< mat_.xx << " " << mat_.yx << " "
					<< mat_.xy << " " << mat_.yy << " "
					<< mat_.x0 << " " << mat_.y0
					<< ")";
				return str.str();
			}
		private:
			void handle_apply(render_context& ctx) override {
				cairo_transform(ctx.cairo(), &mat_);
			}
			void handle_apply_matrix(cairo_matrix_t* mtx) const override {
				cairo_matrix_multiply(mtx, mtx, &mat_);
			}
			cairo_matrix_t mat_;
		};

		class translate_transform : public transform
		{
		public:
			translate_transform(double x, double y) : transform(TransformType::TRANSLATE), x_(x), y_(y) {
			}
			virtual ~translate_transform() {}
			std::string as_string() const override {
				std::stringstream str;
				str << "translate(" << x_ << " " << y_ << ")";
				return str.str();
			}
		private:
			void handle_apply(render_context& ctx) override {
				cairo_translate(ctx.cairo(), x_, y_);
			}
			void handle_apply_matrix(cairo_matrix_t* mtx) const override {
				cairo_matrix_translate(mtx, x_, y_);
			}
			double x_;
			double y_;
		};

		class rotation_transform : public transform
		{
		public:
			rotation_transform(double angle, double cx=0, double cy=0)
				: transform(TransformType::ROTATE),
				angle_(angle),
				cx_(cx),
				cy_(cy) {
			}
			virtual ~rotation_transform() {}
			std::string as_string() const override {
				std::stringstream str;
				if(std::abs(cx_) < DBL_EPSILON && std::abs(cy_) < DBL_EPSILON) {
					str << "rotate(" << angle_ << ")";
				} else {
					str << "rotate(" << angle_ << " " << cx_ << " " << cy_ << ")";
				}
				return str.str();
			}
		private:
			void handle_apply(render_context& ctx) override {
				if(std::abs(cx_) < DBL_EPSILON && std::abs(cy_) < DBL_EPSILON) {
					cairo_rotate(ctx.cairo(), angle_);
				} else {
					cairo_translate(ctx.cairo(), cx_, cy_);
					cairo_rotate(ctx.cairo(), angle_);
					cairo_translate(ctx.cairo(), -cx_, -cy_);
				}
			}
			void handle_apply_matrix(cairo_matrix_t* mtx) const override {
				if(std::abs(cx_) < DBL_EPSILON && std::abs(cy_) < DBL_EPSILON) {
					cairo_matrix_rotate(mtx, angle_);
				} else {
					cairo_matrix_translate(mtx, cx_, cy_);
					cairo_matrix_rotate(mtx, angle_);
					cairo_matrix_translate(mtx, cx_, cy_);
				}
			}
			double angle_;
			double cx_;
			double cy_;
		};

		class scale_transform : public transform
		{
		public:
			scale_transform(double sx, double sy) : transform(TransformType::SCALE), sx_(sx), sy_(sy) {
			}
			virtual ~scale_transform() {}
			std::string as_string() const override {
				std::stringstream str;
				str << "scale(" << sx_ << " " << sy_ << ")";
				return str.str();
			}
		private:
			void handle_apply(render_context& ctx) override {
				cairo_scale(ctx.cairo(), sx_, sy_);
			}
			void handle_apply_matrix(cairo_matrix_t* mtx) const override {
				cairo_matrix_scale(mtx, sx_, sy_);
			}
			double sx_;
			double sy_;
		};

		class skew_x_transform : public transform
		{
		public:
			skew_x_transform(double sx) : transform(TransformType::SKEW_X), sx_(sx) {
				cairo_matrix_init(&mat_, 1, 0, sx_, 1, 0, 0);
			}
			virtual ~skew_x_transform() {}
			std::string as_string() const override {
				std::stringstream str;
				str << "skewX(" << sx_ << ")";
				return str.str();
			}
		private:
			void handle_apply(render_context& ctx) override {
				cairo_transform(ctx.cairo(), &mat_);
			}
			void handle_apply_matrix(cairo_matrix_t* mtx) const override {
				cairo_matrix_multiply(mtx, mtx, &mat_);
			}
			double sx_;
			cairo_matrix_t mat_;
		};

		class skew_y_transform : public transform
		{
		public:
			skew_y_transform(double sy) : transform(TransformType::SKEW_Y), sy_(sy) {
				cairo_matrix_init(&mat_, 1, sy_, 0, 1, 0, 0);
			}
			virtual ~skew_y_transform() {}
			std::string as_string() const override {
				std::stringstream str;
				str << "skewY(" << sy_ << ")";
				return str.str();
			}
		private:
			void handle_apply(render_context& ctx) override {
				cairo_transform(ctx.cairo(), &mat_);
			}
			void handle_apply_matrix(cairo_matrix_t* mtx) const override {
				cairo_matrix_multiply(mtx, mtx, &mat_);
			}
			double sy_;
			cairo_matrix_t mat_;
		};

		transform::transform(TransformType tt)
			: type_(tt)
		{
		}

		void transform::apply(render_context& ctx)
		{
			handle_apply(ctx);
		}

		transform_ptr transform::factory(TransformType tt, const std::vector<double>& params)
		{
			switch(tt) {
			case TransformType::MATRIX: {
				ASSERT_LOG(params.size() == 6, "matrix requires six(6) parameters. Found: " << params.size());
				auto mat = new matrix_transform(params);
				return transform_ptr(mat);
			}
			case TransformType::TRANSLATE: {
				ASSERT_LOG(params.size() == 2, "translate requires two(2) parameters. Found: " << params.size());
				auto trans = new translate_transform(params[0], params[1]);
				return transform_ptr(trans);
			}
			case TransformType::SCALE: {
				ASSERT_LOG(params.size() == 2, "scale requires two parameters. Found: " << params.size());
				auto scale = new scale_transform(params[0], params[1]);
				return transform_ptr(scale);
			}
			case TransformType::ROTATE: {
				ASSERT_LOG(params.size() == 3, "rotate requires three(3) parameters. Found: " << params.size());
				auto rotate = new rotation_transform(params[0],params[1],params[2]);
				return transform_ptr(rotate);
			}
			case TransformType::SKEW_X: {
				ASSERT_LOG(params.size() == 1, "skew_x requires one(1) parameters. Found: " << params.size());
				auto skew_x = new skew_x_transform(params[0]);
				return transform_ptr(skew_x);
			}
			case TransformType::SKEW_Y: {
				ASSERT_LOG(params.size() == 1, "skew_y requires one(1) parameters. Found: " << params.size());
				auto skew_y = new skew_y_transform(params[0]);
				return transform_ptr(skew_y);
			}
			default:
				ASSERT_LOG(false, "Unknown transform type used.");
			}
			return transform_ptr();
		}

		std::vector<transform_ptr> transform::factory(const std::string& s)
		{
			std::vector<transform_ptr> results;
			enum {
				STATE_TYPE,
				STATE_NUMBER,
			} state = STATE_TYPE;

			std::vector<double> parameters;

			TransformType type;

			boost::char_separator<char> seperators(" \n\t\r,", "()");
			boost::tokenizer<boost::char_separator<char>> tok(s, seperators);
			for(auto it = tok.begin(); it != tok.end(); ++it) {
				if(state == STATE_TYPE) {
					if(*it == "matrix") {
						type = TransformType::MATRIX;
					} else if(*it == "translate") {
						type = TransformType::TRANSLATE;
					} else if(*it == "scale") {
						type = TransformType::SCALE;
					} else if(*it == "rotate") {
						type = TransformType::ROTATE;
					} else if(*it == "skewX") {
						type = TransformType::SKEW_X;
					} else if(*it == "skewY") {
						type = TransformType::SKEW_Y;
					} else if(*it == "(") {
						parameters.clear();
						state = STATE_NUMBER;
					} else {
						ASSERT_LOG(false, "Unexpected token while looking for a type: " << *it << " : " << s);
					}
				} else if(state == STATE_NUMBER) {
					if(*it == ")") {
						switch(type) {
							case TransformType::MATRIX: {
								matrix_transform* mtrf = new matrix_transform(parameters);
								results.emplace_back(mtrf);
								break;
							}
							case TransformType::TRANSLATE: {
								ASSERT_LOG(parameters.size() == 1 || parameters.size() == 2, "Parsing transform:translate found " << parameters.size() << " parameter(s), expected 1 or 2");
								double tx = parameters[0];
								double ty = parameters.size() == 2 ? parameters[1] : 0.0f;
								translate_transform * ttrf = new translate_transform(tx, ty);
								results.emplace_back(ttrf);
								break;
							}
							case TransformType::SCALE: {
								ASSERT_LOG(parameters.size() == 1 || parameters.size() == 2, "Parsing transform:scale found " << parameters.size() << " parameter(s), expected 1 or 2");
								double sx = parameters[0];
								double sy = parameters.size() == 2 ? parameters[1] : sx;
								scale_transform * strf = new scale_transform(sx, sy);
								results.emplace_back(strf);
								break;
							}
							case TransformType::ROTATE: {
								ASSERT_LOG(parameters.size() == 1 || parameters.size() == 3, "Parsing transform:rotate found " << parameters.size() << " parameter(s), expected 1 or 3");
								double angle = parameters[0] / 180.0 * M_PI;
								double cx = parameters.size() == 3 ? parameters[1] : 0;
								double cy = parameters.size() == 3 ? parameters[2] : 0;
								rotation_transform* rtrf = new rotation_transform(angle, cx, cy);
								results.emplace_back(rtrf);
								break;
							}
							case TransformType::SKEW_X: {
								ASSERT_LOG(parameters.size() == 1, "Parsing transform:skewX found " << parameters.size() << " parameter(s), expected 1");
								double sa = tan(parameters[0]);
								skew_x_transform* sxtrf = new skew_x_transform(sa);
								results.emplace_back(sxtrf);
								break;
							}
							case TransformType::SKEW_Y: {
								ASSERT_LOG(parameters.size() == 1, "Parsing transform:skewY found " << parameters.size() << " parameter(s), expected 1");
								double sa = tan(parameters[0]);
								skew_y_transform* sxtrf = new skew_y_transform(sa);
								results.emplace_back(sxtrf);
								break;
							}
						}
						state = STATE_TYPE;
					} else {
						char* end = nullptr;
						double value = strtod(it->c_str(), &end);
						if(value == 0 && it->c_str() == end) {
							ASSERT_LOG(false, "Invalid number value: " << *it);
						}
						ASSERT_LOG(errno != ERANGE, "parsed numeric value out-of-range: " << *it);
						parameters.push_back(value);
					}
				}
			}

			return results;
		}

		transform::~transform()
		{
		}

		void transform::apply_matrix(cairo_matrix_t* mtx) const
		{
			handle_apply_matrix(mtx);
		}
	}
}


// Copyright (C) 2021 Christian Brommer, Control of Networked Systems, University of Klagenfurt, Austria.
//
// All rights reserved.
//
// This software is licensed under the terms of the BSD-2-Clause-License with
// no commercial use allowed, the full terms of which are made available
// in the LICENSE file. No license in patents is granted.
//
// You can contact the author at <christian.brommer@ieee.org>

#include <gmock/gmock.h>
#include <mars/ekf.h>
#include <Eigen/Dense>

class mars_Ekf_test : public testing::Test
{
public:
};

TEST_F(mars_Ekf_test, CTOR_EKF)
{
  Eigen::Matrix4d H = Eigen::Matrix4d::Random();
  Eigen::Matrix4d R = Eigen::Matrix4d::Random();
  Eigen::MatrixXd res = Eigen::MatrixXd::Random(4, 1);
  Eigen::Matrix4d P = Eigen::Matrix4d::Random();

  mars::Ekf test(H, R, res, P);
  test.CalculateCorrection();
  test.CalculateCovUpdate();
}


#pragma once

#include <boost/random/random_device.hpp>
#include <boost/random/uniform_int_distribution.hpp>

#include <algorithm>

#include "calotypes/DataSelector.hpp"

namespace calotypes
{

template< class Data >
class SubsampleDataSelector
: public DataSelector<Data>
{
public:

	typedef std::shared_ptr<SubsampleDataSelector> Ptr;
	typedef std::vector<Data> Dataset;

	SubsampleDataSelector( double sr )
	: spanRatio( sr ) {}

	virtual void SelectData( const Dataset& data, unsigned int subsetSize, Dataset& subset)
	{
		unsigned int subsample = std::floor( (data.size()*spanRatio)/(subsetSize-1) );

		// Catch case where there is not enough data
		unsigned int subSize = std::min<double>( subsetSize, std::floor( data.size()/subsample ) );
		subset.clear();
		subset.reserve( subSize );

		unsigned int subSpan = subSize*subsample;
		unsigned int remSpan = data.size() - subSpan;

		boost::random::random_device rng;
		boost::random::uniform_int_distribution<> dist( 0, remSpan );
		unsigned int shift = dist( rng );

		unsigned int ind = shift;
		for( unsigned int i = 0; i < subSize; i++ )
		{
			subset.push_back( data[ind] );
			ind += subsample;
		}

	}

protected:

	double spanRatio;

};

} // end namespace calotypes


#include "hyperopt.h"

#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics/max.hpp>
#include <boost/accumulators/statistics/mean.hpp>
#include <boost/accumulators/statistics/min.hpp>
#include <boost/accumulators/statistics/variance.hpp>
#include <boost/math/distributions/students_t.hpp>
#include <cmath>
#include <tbb/mutex.h>
#include <tbb/parallel_for_each.h>
#include <tbb/parallel_for.h>

#include "export.h"
#include "ndarray_ops.h"
#include "predict.h"
#include "train.h"
#include "utils.h"

namespace curfil {

bool continueSearching(const std::vector<double>& currentBestLosses,
        const std::vector<double>& currentRunLosses) {

    static const size_t MIN_SAMPLES = 3;

    if (currentBestLosses.empty()) {
        CURFIL_INFO("current best is empty. continue searching");
        return true;
    }

    // we want a minimum of three samples
    if (currentRunLosses.size() < MIN_SAMPLES) {
        CURFIL_INFO("too few elements in current run: " << currentRunLosses.size()
                << ". continue searching");
        return true;
    }

    boost::accumulators::accumulator_set<double,
            boost::accumulators::features<boost::accumulators::tag::max,
                    boost::accumulators::tag::mean,
                    boost::accumulators::tag::variance> > acc1;

    boost::accumulators::accumulator_set<double,
            boost::accumulators::features<boost::accumulators::tag::min,
                    boost::accumulators::tag::mean,
                    boost::accumulators::tag::variance> > acc2;

    acc1 = std::for_each(currentBestLosses.begin(), currentBestLosses.end(), acc1);
    acc2 = std::for_each(currentRunLosses.begin(), currentRunLosses.end(), acc2);

    double worstCurrentBest = boost::accumulators::max(acc1);
    double bestOfCurrentLosses = boost::accumulators::min(acc2);

    // continue if any loss is at least as good as the worst loss of the currently best
    if (bestOfCurrentLosses <= worstCurrentBest) {
        CURFIL_INFO("best run of current parameters is at least as good as the worst run with the best parameters: "
                << bestOfCurrentLosses << " <= " << worstCurrentBest
                << ". continue searching");
        return true;
    }

    // http://www.boost.org/doc/libs/1_53_0/libs/math/doc/sf_and_dist/html/math_toolkit/dist/stat_tut/weg/st_eg/two_sample_students_t.html

    double Sm1 = boost::accumulators::mean(acc1);                 // Sm1 = Sample 1 Mean.
    double variance1 = boost::accumulators::variance(acc1);
    double Sd1 = std::sqrt(variance1);                            // Sd1 = Sample 1 Standard Deviation.
    unsigned Sn1 = currentBestLosses.size();                      // Sn1 = Sample 1 Size.

    double Sm2 = boost::accumulators::mean(acc2);                 // Sm2 = Sample 2 Mean.
    double variance2 = boost::accumulators::variance(acc2);
    double Sd2 = std::sqrt(variance2);                            // Sd2 = Sample 2 Standard Deviation.
    unsigned Sn2 = currentRunLosses.size();                       // Sn2 = Sample 2 Size.
    double alpha = 0.005;                                         // alpha = Significance Level.

    // Degrees of freedom:
    double v = Sn1 + Sn2 - 2;
    // Pooled variance:
    double sp = sqrt(((Sn1 - 1) * Sd1 * Sd1 + (Sn2 - 1) * Sd2 * Sd2) / v);
    // t-statistic:
    double t_stat = (Sm1 - Sm2) / (sp * sqrt(1.0 / Sn1 + 1.0 / Sn2));

    boost::math::students_t dist(v);
    double q = boost::math::cdf(boost::math::complement(dist, fabs(t_stat)));

    CURFIL_INFO("Sm1: " << Sm1 << ", Sd1: " << Sd1 << ", Sn1: " << Sn1);
    CURFIL_INFO("Sm2: " << Sm2 << ", Sd2: " << Sd2 << ", Sn2: " << Sn2);
    CURFIL_INFO("q: " << q << ", alpha: " << alpha);

    // continue if we reject hypothesis that sample 1 mean loss is less than sample 2 mean loss. then we do not know and need to continue searching
    return (q > alpha);
}

double Result::getLoss() const {
    double accuracy;
    switch (lossFunctionType) {
        case LossFunctionType::CLASS_ACCURACY:
            accuracy = getClassAccuracy();
            break;
        case LossFunctionType::CLASS_ACCURACY_WITHOUT_VOID:
            accuracy = getClassAccuracyWithoutVoid();
            break;
        case LossFunctionType::PIXEL_ACCURACY:
            accuracy = getPixelAccuracy();
            break;
        case LossFunctionType::PIXEL_ACCURACY_WITHOUT_VOID:
            accuracy = getPixelAccuracyWithoutVoid();
            break;
        default:
            throw std::runtime_error("unknown type of loss function");
    }
    assertProbability(accuracy);
    return 1.0 - accuracy;
}

mongo::BSONObj Result::toBSON() const {

    mongo::BSONObjBuilder o(256);
    o << "randomSeed" << getRandomSeed();
    o << "loss" << getLoss();
    o << "classAccuracy" << getClassAccuracy();
    o << "classAccuracyWithoutVoid" << getClassAccuracyWithoutVoid();
    o << "pixelAccuracy" << getPixelAccuracy();
    o << "pixelAccuracyWithoutVoid" << getPixelAccuracyWithoutVoid();

    mongo::BSONArrayBuilder confusionMatrixArray;
    for (size_t label = 0; label < confusionMatrix.getNumClasses(); label++) {
        mongo::BSONArrayBuilder confusionMatrixRow;
        for (size_t prediction = 0; prediction < confusionMatrix.getNumClasses(); prediction++) {
            double probability = confusionMatrix(label, prediction);
            confusionMatrixRow.append(probability);
        }
        confusionMatrixArray.append(confusionMatrixRow.arr());
    }
    o << "confusionMatrix" << confusionMatrixArray.arr();

    return o.obj();
}

LossFunctionType HyperoptClient::parseLossFunction(const std::string& lossFunctionString) {
    if (lossFunctionString == "classAccuracy") {
        return LossFunctionType::CLASS_ACCURACY;
    } else if (lossFunctionString == "classAccuracyWithoutVoid") {
        return LossFunctionType::CLASS_ACCURACY_WITHOUT_VOID;
    }
    else if (lossFunctionString == "pixelAccuracy") {
        return LossFunctionType::PIXEL_ACCURACY;
    }
    else if (lossFunctionString == "pixelAccuracyWithoutVoid") {
        return LossFunctionType::PIXEL_ACCURACY_WITHOUT_VOID;
    } else {
        throw std::runtime_error(std::string("unknown type of loss function: ") + lossFunctionString);
    }
}

const Result HyperoptClient::test(const RandomForestImage& randomForest,
        const std::vector<LabeledRGBDImage>& testImages) {

    tbb::mutex totalMutex;
    utils::Average averageAccuracy;
    utils::Average averageAccuracyWithoutVoid;

    std::vector<int> indices(testImages.size(), 0);
    for (size_t i = 0; i < testImages.size(); i++) {
        indices[i] = i;
    }

    const LabelType numClasses = randomForest.getNumClasses();

    CURFIL_INFO("testing " << testImages.size() << " images with " << static_cast<int>(numClasses) << " classes");

    ConfusionMatrix totalConfusionMatrix(numClasses);

    tbb::parallel_for_each(indices.begin(), indices.end(), [&](const int& i) {
        const RGBDImage& image = testImages[i].getRGBDImage();
        const LabelImage& groundTruth = testImages[i].getLabelImage();

        LabelImage prediction = randomForest.predict(image);

        tbb::mutex::scoped_lock lock(totalMutex);

        ConfusionMatrix confusionMatrix;
        double accuracy = calculatePixelAccuracy(prediction, groundTruth, true, &confusionMatrix);
        double accuracyWithoutVoid = calculatePixelAccuracy(prediction, groundTruth, false);

        totalConfusionMatrix += confusionMatrix;

        averageAccuracy.addValue(accuracy);
        averageAccuracyWithoutVoid.addValue(accuracyWithoutVoid);
    });

    tbb::mutex::scoped_lock lock(totalMutex);
    double accuracy = averageAccuracy.getAverage();
    double accuracyWithoutVoid = averageAccuracyWithoutVoid.getAverage();

    CURFIL_INFO("accuracy (no void): " << accuracy << " (" << accuracyWithoutVoid << ")");

    return Result(totalConfusionMatrix, accuracy, accuracyWithoutVoid, lossFunction);
}

RandomForestImage HyperoptClient::train(size_t trees,
        const TrainingConfiguration& configuration,
        const std::vector<LabeledRGBDImage>& trainImages) {

    CURFIL_INFO("trees: " << trees);
    CURFIL_INFO(configuration);

    // Train

    RandomForestImage randomForest(trees, configuration);

    // parallel training is not thoroughly tested yet
    static const bool trainTreesSequentially = true;

    utils::Timer trainTimer;
    randomForest.train(trainImages, trainTreesSequentially);
    trainTimer.stop();

    CURFIL_INFO("training took " << trainTimer.format(2) <<
            " (" << std::setprecision(3) << trainTimer.getSeconds() / 60.0 << " min)");

    mongo::BSONObjBuilder featureBuilder(64);
    for (const auto& featureCount : randomForest.countFeatures()) {
        featureBuilder.append(featureCount.first, static_cast<int>(featureCount.second));
    }

    mongo::BSONObjBuilder builder(64);
    builder.append("training_time_millis", trainTimer.getMilliseconds());
    builder.append("featureCounts", featureBuilder.obj());

    log(1, builder.obj());

    return randomForest;
}

double HyperoptClient::measureTrueLoss(unsigned int numTrees, TrainingConfiguration configuration,
        const double histogramBias, double& variance) {

    boost::accumulators::accumulator_set<double,
            boost::accumulators::features<boost::accumulators::tag::mean,
                    boost::accumulators::tag::variance> > acc;

    static const size_t TRUE_LOSS_RUNS = 2;

    CURFIL_INFO("measuring true loss");

    Sampler sampler(randomSeed, 1, 100000);

    for (size_t run = 0; run < TRUE_LOSS_RUNS; run++) {

        CURFIL_INFO("true loss run " << (run + 1) << "/" << TRUE_LOSS_RUNS);

        const int seedOfRun = sampler.getNext();

        configuration.setRandomSeed(seedOfRun);

        RandomForestImage forest = train(numTrees, configuration, allRGBDImages);
        forest.normalizeHistograms(histogramBias);
        Result result = test(forest, allTestImages);

        result.setRandomSeed(seedOfRun);

        CURFIL_INFO(result.getConfusionMatrix());

        log(2, BSON("trueLossRun" << static_cast<int>(run)
                << "result" << result.toBSON()));

        acc(result.getLoss());
    }

    variance = boost::accumulators::variance(acc);

    double trueLoss = boost::accumulators::mean(acc);

    CURFIL_INFO("true loss: " << trueLoss);

    return trueLoss;
}

double HyperoptClient::getAverageLossAndVariance(const std::vector<Result>& results, double& variance) {

    boost::accumulators::accumulator_set<double,
            boost::accumulators::features<boost::accumulators::tag::mean,
                    boost::accumulators::tag::variance> > acc;

    for (const Result& result : results) {
        acc(result.getLoss());
    }

    variance = boost::accumulators::variance(acc);

    return boost::accumulators::mean(acc);
}

void HyperoptClient::randomSplit(const int randomSeed, const double testRatio,
        std::vector<LabeledRGBDImage>& trainImages,
        std::vector<LabeledRGBDImage>& testImages) {

    boost::mt19937 rng(randomSeed);
    boost::uniform_real<> dist(0.0, 1.0);

    trainImages.clear();
    testImages.clear();

    while (testImages.empty() || trainImages.empty()) {
        for (const auto& image : allRGBDImages) {
            double random = dist(rng);
            if (random < testRatio) {
                testImages.push_back(image);
            } else {
                trainImages.push_back(image);
            }
        }
    }

    CURFIL_INFO("random split of " << testRatio <<
            ". train images: " << trainImages.size() <<
            ", test images: " << testImages.size());
}

void HyperoptClient::handle_task(const mongo::BSONObj& task) {
    try {
        CURFIL_INFO("got task object: " << task.toString());

        const unsigned int numTrees = getParameterDouble(task, "numTrees");
        const unsigned int samplesPerImage = getParameterDouble(task, "samplesPerImage");
        const unsigned int featureCount = getParameterDouble(task, "featureCount");
        const unsigned int minSampleCount = getParameterDouble(task, "minSampleCount");
        const int maxDepth = getParameterDouble(task, "maxDepth");
        const uint16_t boxRadius = getParameterDouble(task, "boxRadius");
        const uint16_t regionSize = getParameterDouble(task, "regionSize");
        const uint16_t thresholds = getParameterDouble(task, "thresholds");
        const double histogramBias = getParameterDouble(task, "histogramBias");
        const AccelerationMode accelerationMode = AccelerationMode::GPU_ONLY;

        std::vector<Result> results;

        Sampler sampler(randomSeed, 1, 100000);

        static const int RUNS = 5;
        const double testRatio = 1.0 / RUNS;

        std::vector<double> currentRunLosses;

        for (int run = 0; run < RUNS; run++) {

            CURFIL_INFO("starting run " << (run + 1) << "/" << RUNS);

            const int seedOfRun = sampler.getNext();

            std::vector<LabeledRGBDImage> trainImages;
            std::vector<LabeledRGBDImage> testImages;

            randomSplit(seedOfRun, testRatio, trainImages, testImages);

            unsigned int imageCacheSize = 0;
            unsigned int maxSamplesPerBatch = 0;

            determineImageCacheSizeAndSamplesPerBatch(trainImages, deviceIds, featureCount, thresholds,
                    imageCacheSizeMB, imageCacheSize, maxSamplesPerBatch);

            TrainingConfiguration configuration(seedOfRun, samplesPerImage, featureCount, minSampleCount, maxDepth,
                    boxRadius, regionSize, thresholds, numThreads, maxImages, imageCacheSize, maxSamplesPerBatch,
                    accelerationMode, useCIELab, useDepthFilling, deviceIds, subsamplingType, ignoredColors);

            mongo::BSONObj msg = BSON("run" << run
                    << "randomSeed" << seedOfRun
                    << "numTrainImages" << static_cast<int>(trainImages.size())
                    << "numTestImages" << static_cast<int>(testImages.size()));

            log(3, msg);
            checkpoint();

            RandomForestImage forest = train(numTrees, configuration, trainImages);
            forest.normalizeHistograms(histogramBias);
            Result result = test(forest, testImages);
            result.setRandomSeed(seedOfRun);

            results.push_back(result);

            currentRunLosses.push_back(result.getLoss());

            log(3, result.toBSON());
            checkpoint();

            mongo::BSONObj bestTask;
            if (get_best_task(bestTask)) {

                CURFIL_INFO("best task so far: " << bestTask.getObjectField("result").toString());

                std::vector<double> currentBestLosses;

                mongo::BSONElementSet values;
                bestTask.getFieldsDotted("result.results.loss", values);

                for (const mongo::BSONElement& loss : values) {
                    currentBestLosses.push_back(loss.Double());
                }

                if (!continueSearching(currentBestLosses, currentRunLosses)) {
                    log(1, BSON("stopSearching" << true
                            << "run" << run
                            << "currentBestLosses" << currentBestLosses
                            << "currentRunLosses" << currentRunLosses ));
                    CURFIL_INFO("stop searching");
                    break;
                } else {
                    CURFIL_INFO("continue searching");
                }
            } else {
                CURFIL_INFO("no finished task so far");
            }

        }

        double lossVariance;
        double loss = getAverageLossAndVariance(results, lossVariance);

        unsigned int imageCacheSize = 0;
        unsigned int maxSamplesPerBatch = 0;

        determineImageCacheSizeAndSamplesPerBatch(allRGBDImages, deviceIds, featureCount, thresholds,
                imageCacheSizeMB, imageCacheSize, maxSamplesPerBatch);

        TrainingConfiguration configuration(randomSeed, samplesPerImage, featureCount, minSampleCount, maxDepth,
                boxRadius, regionSize, thresholds, numThreads, maxImages, imageCacheSize, maxSamplesPerBatch,
                accelerationMode, useCIELab, useDepthFilling, deviceIds, subsamplingType, ignoredColors);

        double trueLossVariance;
        double trueLoss = measureTrueLoss(numTrees, configuration, histogramBias, trueLossVariance);

        mongo::BSONObjBuilder builder(64);

        builder << "status" << "ok";
        builder << "loss" << loss << "loss_variance" << lossVariance;
        builder << "true_loss" << trueLoss << "true_loss_variance" << trueLossVariance;

        std::vector<mongo::BSONObj> resultsDetails;
        for (size_t i = 0; i < results.size(); i++) {
            resultsDetails.push_back(results[i].toBSON());
        }

        builder.append("results", resultsDetails);

        finish(builder.obj(), true);

    } catch (const std::runtime_error& e) {
        CURFIL_ERROR(e.what());
        finish(BSON("status" << "fail" << "why" << e.what()), false);
        throw e;
    }

}

double HyperoptClient::getParameterDouble(const mongo::BSONObj& task, const std::string& field) {
    const mongo::BSONObj vals = task.getObjectField("vals");
    if (!vals.hasField(field.c_str())) {
        throw std::runtime_error(task.toString() + " has no value '" + field + "'");
    }
    const mongo::BSONElement value = vals.getField(field);
    const std::vector<mongo::BSONElement> a = value.Array();
    return a.at(0).Double();
}

void HyperoptClient::run() {

    mongo::BSONObj result;
    if (get_best_task(result)) {
        CURFIL_INFO("best result so far: " << result["result"].toString());
    } else {
        CURFIL_INFO("no best result so far");
    }

    CURFIL_INFO("running client");
    bool log_when_waiting = true;
    while (true) {
        mongo::BSONObj task;
        if (!get_next_task(task)) {
            if (log_when_waiting)
                CURFIL_INFO("no task in queue. waiting...");
            log_when_waiting = false;
            sleep(1);
        } else {
            handle_task(task);
            log_when_waiting = true;
        }
    }
}

}


#ifndef MATHTOOLBOX_ACQUISITION_FUNCTIONS_HPP
#define MATHTOOLBOX_ACQUISITION_FUNCTIONS_HPP

#include <Eigen/Core>
#include <functional>

namespace mathtoolbox
{
    double GetExpectedImprovement(const Eigen::VectorXd&                               x,
                                  const std::function<double(const Eigen::VectorXd&)>& mu,
                                  const std::function<double(const Eigen::VectorXd&)>& sigma,
                                  const Eigen::VectorXd&                               x_best);

    Eigen::VectorXd
    GetExpectedImprovementDerivative(const Eigen::VectorXd&                                        x,
                                     const std::function<double(const Eigen::VectorXd&)>&          mu,
                                     const std::function<double(const Eigen::VectorXd&)>&          sigma,
                                     const Eigen::VectorXd&                                        x_best,
                                     const std::function<Eigen::VectorXd(const Eigen::VectorXd&)>& mu_derivative,
                                     const std::function<Eigen::VectorXd(const Eigen::VectorXd&)>& sigma_derivative);

    /// \param hyperparam The hyperparameter that controls the trade-off of exploration and exploitation. Specifically,
    /// this hyperparameter corresponds to the square root of the beta in [Srinivas et al. ICML '10]. Setting this to
    /// zero means pure exploitation, and setting to a very large value means (almost) pure exploration. This value
    /// needs to be non-negative.
    double GetGaussianProcessUpperConfidenceBound(const Eigen::VectorXd&                               x,
                                                  const std::function<double(const Eigen::VectorXd&)>& mu,
                                                  const std::function<double(const Eigen::VectorXd&)>& sigma,
                                                  const double                                         hyperparam);

    /// \param hyperparam The hyperparameter that controls the trade-off of exploration and exploitation. Specifically,
    /// this hyperparameter corresponds to the square root of the beta in [Srinivas et al. ICML '10]. Setting this to
    /// zero means pure exploitation, and setting to a very large value means (almost) pure exploration. This value
    /// needs to be non-negative.
    Eigen::VectorXd GetGaussianProcessUpperConfidenceBoundDerivative(
        const Eigen::VectorXd&                                        x,
        const std::function<double(const Eigen::VectorXd&)>&          mu,
        const std::function<double(const Eigen::VectorXd&)>&          sigma,
        const double                                                  hyperparam,
        const std::function<Eigen::VectorXd(const Eigen::VectorXd&)>& mu_derivative,
        const std::function<Eigen::VectorXd(const Eigen::VectorXd&)>& sigma_derivative);
} // namespace mathtoolbox

#endif // MATHTOOLBOX_ACQUISITION_FUNCTIONS_HPP