#ifndef STAN_MATH_PRIM_FUNCTOR_ODE_RK45_HPP
#define STAN_MATH_PRIM_FUNCTOR_ODE_RK45_HPP

#include <stan/math/prim/meta.hpp>
#include <stan/math/prim/functor/apply.hpp>
#include <stan/math/prim/functor/coupled_ode_system.hpp>
#include <stan/math/prim/functor/ode_store_sensitivities.hpp>
#include <stan/math/prim/fun/Eigen.hpp>
#include <boost/numeric/odeint.hpp>
#include <ostream>
#include <tuple>
#include <vector>

namespace stan {
namespace math {

/**
 * Solve the ODE initial value problem y' = f(t, y), y(t0) = y0 at a set of
 * times, { t1, t2, t3, ... } using the non-stiff Runge-Kutta 45 solver in
 * Boost.
 *
 * If the system of equations is stiff, ode_bdf will likely be
 * faster.
 *
 * \p f must define an operator() with the signature as:
 *   template<typename T_t, typename T_y, typename... T_Args>
 *   Eigen::Matrix<stan::return_type_t<T_t, T_y, T_Args...>, Eigen::Dynamic, 1>
 *     operator()(const T_t& t, const Eigen::Matrix<T_y, Eigen::Dynamic, 1>& y,
 *     std::ostream* msgs, const T_Args&... args);
 *
 * t is the time, y is the vector-valued state, msgs is a stream for error
 * messages, and args are optional arguments passed to the ODE solve function
 * (which are passed through to \p f without modification).
 *
 * @tparam F Type of ODE right hand side
 * @tparam T_y0 Type of initial condition
 * @tparam T_t0 Type of initial time
 * @tparam T_ts Type of output times
 * @tparam T_Args Types of pass-through parameters
 *
 * @param function_name Calling function name (for printing debugging messages)
 * @param f Right hand side of the ODE
 * @param y0_arg Initial state
 * @param t0 Initial time
 * @param ts Times at which to solve the ODE at. All values must be sorted and
 *   greater than t0.
 * @param relative_tolerance Relative tolerance passed to Boost
 * @param absolute_tolerance Absolute tolerance passed to Boost
 * @param max_num_steps Upper limit on the number of integration steps to
 *   take between each output (error if exceeded)
 * @param[in, out] msgs the print stream for warning messages
 * @param args Extra arguments passed unmodified through to ODE right hand side
 * @return Solution to ODE at times \p ts
 */
template <typename F, typename T_y0, typename T_t0, typename T_ts,
          typename... Args, require_eigen_vector_t<T_y0>* = nullptr>
std::vector<Eigen::Matrix<stan::return_type_t<T_y0, T_t0, T_ts, Args...>,
                          Eigen::Dynamic, 1>>
ode_rk45_tol_impl(const char* function_name, const F& f, const T_y0& y0_arg,
                  T_t0 t0, const std::vector<T_ts>& ts,
                  double relative_tolerance, double absolute_tolerance,
                  long int max_num_steps,  // NOLINT(runtime/int)
                  std::ostream* msgs, const Args&... args) {
  using boost::numeric::odeint::integrate_times;
  using boost::numeric::odeint::make_dense_output;
  using boost::numeric::odeint::max_step_checker;
  using boost::numeric::odeint::no_progress_error;
  using boost::numeric::odeint::runge_kutta_dopri5;
  using boost::numeric::odeint::vector_space_algebra;

  using T_y0_t0 = return_type_t<T_y0, T_t0>;

  Eigen::Matrix<T_y0_t0, Eigen::Dynamic, 1> y0
      = y0_arg.template cast<T_y0_t0>();

  check_finite(function_name, "initial state", y0);
  check_finite(function_name, "initial time", t0);
  check_finite(function_name, "times", ts);

  std::tuple<ref_type_t<Args>...> args_ref_tuple(args...);

  apply(
      [&](const auto&... args_ref) {
        // Code from https://stackoverflow.com/a/17340003
        std::vector<int> unused_temp{
            0,
            (check_finite(function_name, "ode parameters and data", args_ref),
             0)...};
      },
      args_ref_tuple);

  check_nonzero_size(function_name, "initial state", y0);
  check_nonzero_size(function_name, "times", ts);
  check_sorted(function_name, "times", ts);
  check_less(function_name, "initial time", t0, ts[0]);

  check_positive_finite(function_name, "relative_tolerance",
                        relative_tolerance);
  check_positive_finite(function_name, "absolute_tolerance",
                        absolute_tolerance);
  check_positive(function_name, "max_num_steps", max_num_steps);

  using return_t = return_type_t<T_y0, T_t0, T_ts, Args...>;
  // creates basic or coupled system by template specializations
  auto&& coupled_system = apply(
      [&](const auto&... args_ref) {
        return coupled_ode_system<F, T_y0_t0, ref_type_t<Args>...>(f, y0, msgs,
                                                                   args_ref...);
      },
      args_ref_tuple);

  // first time in the vector must be time of initial state
  std::vector<double> ts_vec(ts.size() + 1);
  ts_vec[0] = value_of(t0);
  for (size_t i = 0; i < ts.size(); ++i)
    ts_vec[i + 1] = value_of(ts[i]);

  std::vector<Eigen::Matrix<return_t, Eigen::Dynamic, 1>> y;
  y.reserve(ts.size());
  bool observer_initial_recorded = false;
  size_t time_index = 0;

  // avoid recording of the initial state which is included by the
  // conventions of odeint in the output
  auto filtered_observer
      = [&](const std::vector<double>& coupled_state, double t) -> void {
    if (!observer_initial_recorded) {
      observer_initial_recorded = true;
      return;
    }
    apply(
        [&](const auto&... args_ref) {
          y.emplace_back(ode_store_sensitivities(
              f, coupled_state, y0, t0, ts[time_index], msgs, args_ref...));
        },
        args_ref_tuple);
    time_index++;
  };

  // the coupled system creates the coupled initial state
  std::vector<double> initial_coupled_state = coupled_system.initial_state();

  const double step_size = 0.1;
  try {
    integrate_times(
        make_dense_output(absolute_tolerance, relative_tolerance,
                          runge_kutta_dopri5<std::vector<double>, double,
                                             std::vector<double>, double>()),
        std::ref(coupled_system), initial_coupled_state, std::begin(ts_vec),
        std::end(ts_vec), step_size, filtered_observer,
        max_step_checker(max_num_steps));
  } catch (const no_progress_error& e) {
    throw_domain_error(function_name, "", ts_vec[time_index + 1],
                       "Failed to integrate to next output time (",
                       ") in less than max_num_steps steps");
  }

  return y;
}

/**
 * Solve the ODE initial value problem y' = f(t, y), y(t0) = y0 at a set of
 * times, { t1, t2, t3, ... } using the non-stiff Runge-Kutta 45 solver in
 * Boost.
 *
 * If the system of equations is stiff, ode_bdf will likely be
 * faster.
 *
 * \p f must define an operator() with the signature as:
 *   template<typename T_t, typename T_y, typename... T_Args>
 *   Eigen::Matrix<stan::return_type_t<T_t, T_y, T_Args...>, Eigen::Dynamic, 1>
 *     operator()(const T_t& t, const Eigen::Matrix<T_y, Eigen::Dynamic, 1>& y,
 *     std::ostream* msgs, const T_Args&... args);
 *
 * t is the time, y is the vector-valued state, msgs is a stream for error
 * messages, and args are optional arguments passed to the ODE solve function
 * (which are passed through to \p f without modification).
 *
 * @tparam F Type of ODE right hand side
 * @tparam T_0 Type of initial time
 * @tparam T_ts Type of output times
 * @tparam T_Args Types of pass-through parameters
 *
 * @param f Right hand side of the ODE
 * @param y0_arg Initial state
 * @param t0 Initial time
 * @param ts Times at which to solve the ODE at. All values must be sorted and
 *   greater than t0.
 * @param relative_tolerance Relative tolerance passed to Boost
 * @param absolute_tolerance Absolute tolerance passed to Boost
 * @param max_num_steps Upper limit on the number of integration steps to
 *   take between each output (error if exceeded)
 * @param[in, out] msgs the print stream for warning messages
 * @param args Extra arguments passed unmodified through to ODE right hand side
 * @return Solution to ODE at times \p ts
 */
template <typename F, typename T_y0, typename T_t0, typename T_ts,
          typename... Args, require_eigen_vector_t<T_y0>* = nullptr>
std::vector<Eigen::Matrix<stan::return_type_t<T_y0, T_t0, T_ts, Args...>,
                          Eigen::Dynamic, 1>>
ode_rk45_tol(const F& f, const T_y0& y0_arg, T_t0 t0,
             const std::vector<T_ts>& ts, double relative_tolerance,
             double absolute_tolerance,
             long int max_num_steps,  // NOLINT(runtime/int)
             std::ostream* msgs, const Args&... args) {
  return ode_rk45_tol_impl("ode_rk45_tol", f, y0_arg, t0, ts,
                           relative_tolerance, absolute_tolerance,
                           max_num_steps, msgs, args...);
}

/**
 * Solve the ODE initial value problem y' = f(t, y), y(t0) = y0 at a set of
 * times, { t1, t2, t3, ... } using the non-stiff Runge-Kutta 45 solver in Boost
 * with defaults for relative_tolerance, absolute_tolerance, and max_num_steps.
 *
 * If the system of equations is stiff, ode_bdf will likely be
 * faster.
 *
 * \p f must define an operator() with the signature as:
 *   template<typename T_t, typename T_y, typename... T_Args>
 *   Eigen::Matrix<stan::return_type_t<T_t, T_y, T_Args...>, Eigen::Dynamic, 1>
 *     operator()(const T_t& t, const Eigen::Matrix<T_y, Eigen::Dynamic, 1>& y,
 *     std::ostream* msgs, const T_Args&... args);
 *
 * t is the time, y is the vector-valued state, msgs is a stream for error
 * messages, and args are optional arguments passed to the ODE solve function
 * (which are passed through to \p f without modification).
 *
 * @tparam F Type of ODE right hand side
 * @tparam T_y0 Type of initial time
 * @tparam T_ts Type of output times
 * @tparam T_Args Types of pass-through parameters
 *
 * @param f Right hand side of the ODE
 * @param y0 Initial state
 * @param t0 Initial time
 * @param ts Times at which to solve the ODE at. All values must be sorted and
 *   greather than t0.
 * @param[in, out] msgs the print stream for warning messages
 * @param args Extra arguments passed unmodified through to ODE right hand side
 * @return Solution to ODE at times \p ts
 */
template <typename F, typename T_y0, typename T_t0, typename T_ts,
          typename... Args, require_eigen_vector_t<T_y0>* = nullptr>
std::vector<Eigen::Matrix<stan::return_type_t<T_y0, T_t0, T_ts, Args...>,
                          Eigen::Dynamic, 1>>
ode_rk45(const F& f, const T_y0& y0, T_t0 t0, const std::vector<T_ts>& ts,
         std::ostream* msgs, const Args&... args) {
  double relative_tolerance = 1e-6;
  double absolute_tolerance = 1e-6;
  long int max_num_steps = 1e6;  // NOLINT(runtime/int)

  return ode_rk45_tol_impl("ode_rk45", f, y0, t0, ts, relative_tolerance,
                           absolute_tolerance, max_num_steps, msgs, args...);
}

}  // namespace math
}  // namespace stan
#endif


//
//  kfilter.hpp
//  carma_pack
//
//  Created by Brandon Kelly on 6/27/13.
//  Copyright (c) 2013 Brandon Kelly. All rights reserved.
//

#ifndef __carma_pack__kfilter__
#define __carma_pack__kfilter__

#include <armadillo>
#include <utility>
#include <boost/assert.hpp>

// Global random number generator object, instantiated in random.cpp
extern boost::random::mt19937 rng;

// Object containing some common random number generators.
extern RandomGenerator RandGen;

/*
 Abstract base class for the Kalman Filter of a CARMA(p,q) process.
 */

template <class OmegaType>
class KalmanFilter {
public:
    // Kalman mean and variance
    arma::vec mean;
    arma::vec var;

    // Constructor
    KalmanFilter() {};
    KalmanFilter(arma::vec& time, arma::vec& y, arma::vec& yerr) :
    time_(time), y_(y), yerr_(yerr)
    {
        init();
    }

    // Initialize arrays
    void init() {
        int ndata = time_.n_elem;
        dt_ = time_(arma::span(1,ndata-1)) - time_(arma::span(0,ndata-2));

        // Make sure the time vector is strictly increasing
        if (dt_.min() < 0) {
            std::cout << "Time vector is not sorted in increasing order. Sorting the data vectors..."
            << std::endl;
            // Sort the time values such that dt > 0
            arma::uvec sorted_indices = arma::sort_index(time_);
            time_ = time_.elem(sorted_indices);
            y_ = y_.elem(sorted_indices);
            yerr_ = yerr_.elem(sorted_indices);
            dt_ = time_.rows(1,ndata-1) - time_.rows(0,ndata-2);
        }
        // Make sure there are no duplicate values of time
        if (dt_.min() == 0) {
            std::cout << "Found duplicate values of time, removing them..." << std::endl;
            // Find the unique values of time
            arma::uvec unique_values = 1 + arma::find(dt_ != 0);
            // Add extra row in to keep time(0), y(0), yerr(0)
            unique_values.insert_rows(0, arma::zeros<arma::uvec>(1));
            time_ = time_.elem(unique_values);
            y_ = y_.elem(unique_values);
            yerr_ = yerr_.elem(unique_values);
            ndata = time_.n_elem;
            dt_.set_size(ndata-1);
            dt_ = time_.rows(1,time_.n_elem-1) - time_.rows(0,time_.n_elem-2);
        }

        // Set the size of the Kalman Filter mean and variance vectors
        mean.zeros(time_.n_elem);
        var.zeros(time_.n_elem);
    }

    // Methods to set and get the data
    void SetTime(arma::vec& time) {
        time_ = time;
    }
    arma::vec GetTime() {
        return time_;
    }
    void SetTimeSeries(arma::vec& y) {
        y_ = y;
    }
    arma::vec GetTimeSeries() {
        return y_;
    }
    void SetTimeSeriesErr(arma::vec& yerr) {
        yerr_ = yerr;
    }
    arma::vec GetTimeSeriesErr() {
        return yerr_;
    }

    // Methods to set and get the parameter values
    void SetSigsqr(double sigsqr) {
        sigsqr_ = sigsqr;
    }
    double GetSigsqr() {
        return sigsqr_;
    }
    virtual void SetOmega(OmegaType omega) {
        omega_ = omega;
    }
    OmegaType GetOmega() {
        return omega_;
    }
    virtual void SetMA(arma::vec ma_coefs) {}

    double GetConst() { return yconst_; }
    double GetSlope() { return yslope_; }

    std::vector<double> GetMeanSvec() { return arma::conv_to<std::vector<double> >::from(mean); }
    std::vector<double> GetVarSvec() { return arma::conv_to<std::vector<double> >::from(var); }

    /*
     Methods to perform the Kalman Filter operations
     */
    virtual void Reset() = 0;
    virtual void Update() = 0;
    virtual std::pair<double, double> Predict(double time) = 0;

    void Filter() {
        // Run the Kalman Filter
        Reset();
        for (int i=1; i<time_.n_elem; i++) {
            Update();
        }
    }

    // simulate a CARMA process, conditional on the measured time series
    std::vector<double> Simulate(arma::vec time) {
        // first save old values since we will overwrite them later
        arma::vec time0 = time_;
        arma::vec y0 = y_;
        arma::vec yerr0 = yerr_;

        arma::vec ysimulated(time.n_elem);

        time = arma::sort(time);
        unsigned int insert_idx = 0;
        arma::vec tinsert(1);
        arma::vec dt_insert(1);
        arma::vec yinsert(1);
        arma::vec yerr_insert(1);
        yerr_insert(0) = 0.0;
        for (int i=0; i<time.n_elem; i++) {
            insert_idx = 0;
            // first simulate the value at time(i)
            std::pair<double, double> ypredict = this->Predict(time(i));
            ysimulated(i) = RandGen.normal(ypredict.first, sqrt(ypredict.second));
            // find the index where time_[insert_idx-1] < time(i) < time_[insert_idx]
            while (time_(insert_idx) < time(i)) {
                insert_idx++;
                if (insert_idx == (time_.n_elem)) {
                    break;
                }
            }
            // insert the simulated value into the measured time series array. these values are used in subsequent
            // calls to Predict(time(i)).
            tinsert(0) = time(i);
            time_.insert_rows(insert_idx, tinsert);
            dt_ = time_(arma::span(1,time_.n_elem-1)) - time_(arma::span(0,time_.n_elem-2));
            yinsert(0) = ysimulated(i);
            y_.insert_rows(insert_idx, yinsert);
            yerr_.insert_rows(insert_idx, yerr_insert);
            mean.zeros(time_.n_elem);
            var.zeros(time_.n_elem);
        }

        // restore values of measured time series
        time_ = time0;
        dt_ = time_(arma::span(1,time_.n_elem-1)) - time_(arma::span(0,time_.n_elem-2));
        y_ = y0;
        yerr_ = yerr0;
        // restore the original sizes of the kalman mean and variance arrays
        mean.zeros(time_.n_elem);
        var.zeros(time_.n_elem);

        return arma::conv_to<std::vector<double> >::from(ysimulated);
    }

    // Methods needed for interpolation and backcasting
    virtual void InitializeCoefs(double time, unsigned int itime, double ymean, double yvar) = 0;
    virtual void UpdateCoefs() = 0;

protected:
    // Data
    arma::vec time_;
    arma::vec dt_;
    arma::vec y_;
    arma::vec yerr_;
    // The Kalman Filter parameters
    double sigsqr_;
    OmegaType omega_;
    unsigned int current_index_;
    // linear coefficients needed for doing interpolation or backcasting
    double yconst_, yslope_;
};

/*
 Class to perform the Kalman Filter and related operations for a zero-mean CAR(1) process (see below).
 This class will calculate the Kalman Filter, needed to calculate the log-likelihood in the CAR1
 parameter class. It will also provide interpolated, extrapolated, and simulated values given a
 CAR(1) model and a measured time series.
 */

class KalmanFilter1 : public KalmanFilter<double> {
public:
    // Constructors
    KalmanFilter1() : KalmanFilter<double>() {}
    KalmanFilter1(arma::vec& time, arma::vec& y, arma::vec& yerr) : KalmanFilter<double>(time, y, yerr) {}
    KalmanFilter1(arma::vec& time, arma::vec& y, arma::vec& yerr, double sigsqr, double omega) :
        KalmanFilter<double>(time, y, yerr)
    {
        sigsqr_ = sigsqr;
        omega_ = omega;
    }
    KalmanFilter1(std::vector<double> time, std::vector<double> y, std::vector<double> yerr) :
        KalmanFilter<double>()
    {
        arma::vec armatime = arma::conv_to<arma::vec>::from(time);
        arma::vec armay    = arma::conv_to<arma::vec>::from(y);
        arma::vec armady   = arma::conv_to<arma::vec>::from(yerr);
        time_ = armatime;
        y_ = armay;
        yerr_ = armady;
        init();
    }
    KalmanFilter1(std::vector<double> time, std::vector<double> y, std::vector<double> yerr, double sigsqr, double omega) :
        KalmanFilter<double>()
    {
        arma::vec armatime = arma::conv_to<arma::vec>::from(time);
        arma::vec armay    = arma::conv_to<arma::vec>::from(y);
        arma::vec armady   = arma::conv_to<arma::vec>::from(yerr);
        time_ = armatime;
        y_ = armay;
        yerr_ = armady;
        sigsqr_ = sigsqr;
        omega_ = omega;
        init();
    }

    // Methods to perform the Kalman Filter operations
    void Reset();
    void Update();
    std::pair<double, double> Predict(double time);
    void InitializeCoefs(double time, unsigned int itime, double ymean, double yvar);
    void UpdateCoefs();

    std::vector<double> Simulate(std::vector<double> time) {
        arma::vec armatime = arma::conv_to<arma::vec>::from(time);
        arma::vec armasimulate = KalmanFilter<double>::Simulate(armatime);
        std::vector<double> vecsimulate = arma::conv_to<std::vector<double> >::from(armasimulate);
        return vecsimulate;
    }
};

/*
 Same as KalmanFilter1 but for a CARMA(p,q) process
 */

class KalmanFilterp : public KalmanFilter<arma::cx_vec> {
public:
    // Constructors
    KalmanFilterp() : KalmanFilter<arma::cx_vec>() {}
    KalmanFilterp(arma::vec& time, arma::vec& y, arma::vec& yerr) : KalmanFilter<arma::cx_vec>(time, y, yerr) {init();}
    KalmanFilterp(arma::vec& time, arma::vec& y, arma::vec& yerr, double sigsqr, arma::cx_vec& omega, arma::vec& ma_coefs) :
        KalmanFilter<arma::cx_vec>(time, y, yerr)
    {
        sigsqr_ = sigsqr;
        omega_ = omega; // omega are the roots of the AR characteristic polynomial
        p_ = omega_.n_elem;
        if (p_ > ma_coefs.n_elem) {
            ma_coefs.resize(p_);
        }
        SetMA(ma_coefs);

        // set sizes of arrays
        state_vector_.zeros(p_);
        StateVar_.zeros(p_,p_);
        PredictionVar_.zeros(p_,p_);
        kalman_gain_.zeros(p_);
        rho_.zeros(p_);
        state_const_.zeros(p_);
        state_slope_.zeros(p_);
        rotated_ma_coefs_.zeros(p_);
    }
    KalmanFilterp(std::vector<double> time, std::vector<double> y, std::vector<double> yerr) :
        KalmanFilter<arma::cx_vec>()
    {
        arma::vec armatime = arma::conv_to<arma::vec>::from(time);
        arma::vec armay    = arma::conv_to<arma::vec>::from(y);
        arma::vec armady   = arma::conv_to<arma::vec>::from(yerr);
        time_ = armatime;
        y_ = armay;
        yerr_ = armady;
        init();
    }
   KalmanFilterp(std::vector<double> time, std::vector<double> y, std::vector<double> yerr, double sigsqr,
                 std::vector<std::complex<double> > omega, std::vector<double> ma_coefs) :
        KalmanFilter<arma::cx_vec>()
    {
        arma::vec armatime  = arma::conv_to<arma::vec>::from(time);
        arma::vec armay     = arma::conv_to<arma::vec>::from(y);
        arma::vec armady    = arma::conv_to<arma::vec>::from(yerr);
        arma::cx_vec armaomega = arma::conv_to<arma::cx_vec>::from(omega);
        arma::vec armacoefs = arma::conv_to<arma::vec>::from(ma_coefs);
        time_ = armatime;
        y_ = armay;
        yerr_ = armady;
        sigsqr_ = sigsqr;
        omega_ = armaomega;
        p_ = omega_.n_elem;
        if (p_ > armacoefs.n_elem) {
            armacoefs.resize(p_);
        }
        SetMA(armacoefs);

        // set sizes of arrays
        state_vector_.zeros(p_);
        StateVar_.zeros(p_,p_);
        PredictionVar_.zeros(p_,p_);
        kalman_gain_.zeros(p_);
        rho_.zeros(p_);
        state_const_.zeros(p_);
        state_slope_.zeros(p_);
        rotated_ma_coefs_.zeros(p_);

        init();
    }

    // Set and get and moving average terms
    void SetMA(arma::vec ma_coefs) {
        ma_coefs_ = ma_coefs.st();
    }
    arma::vec GetMA() {
        return ma_coefs_.st();
    }

    // set the AR parameters
    void SetOmega(arma::cx_vec omega) {
        omega_ = omega;
        // resize arrays
        p_ = omega_.n_elem;
        state_vector_.zeros(p_);
        StateVar_.zeros(p_,p_);
        PredictionVar_.zeros(p_,p_);
        kalman_gain_.zeros(p_);
        rho_.zeros(p_);
        state_const_.zeros(p_);
        state_slope_.zeros(p_);
        rotated_ma_coefs_.zeros(p_);
    }

    // Methods to perform the Kalman Filter operations
    void Reset();
    void Update();
    std::pair<double, double> Predict(double time);
    void InitializeCoefs(double time, unsigned int itime, double ymean, double yvar);
    void UpdateCoefs();

    std::vector<double> Simulate(std::vector<double> time) {
        arma::vec armatime = arma::conv_to<arma::vec>::from(time);
        arma::vec armasimulate = KalmanFilter<arma::cx_vec>::Simulate(armatime);
        std::vector<double> vecsimulate = arma::conv_to<std::vector<double> >::from(armasimulate);
        return vecsimulate;
    }

private:
    // parameters
    arma::rowvec ma_coefs_; // moving average terms
    unsigned int p_; // the orders of the CARMA process
    // quantities defining the current state of the kalman filter
    arma::cx_vec state_vector_; // current value of the rotated state vector
    arma::cx_mat StateVar_; // stationary covariance matrix of the rotated state vector
    arma::cx_rowvec rotated_ma_coefs_; // rotated moving average coefficients
    arma::cx_mat PredictionVar_; // covariance matrix of the predicted rotated state vector
    arma::cx_vec kalman_gain_;
    arma::cx_vec rho_;
    double innovation_;
    // linear coefficients needed for doing interpolation or backcasting
    arma::cx_vec state_const_;
    arma::cx_vec state_slope_;
};

#endif /* defined(__carma_pack__kfilter__) */


/*********************************************************************
* Software License Agreement (BSD License)
*
*  Copyright (c) 2008, Willow Garage, Inc.
*  All rights reserved.
*
*  Redistribution and use in source and binary forms, with or without
*  modification, are permitted provided that the following conditions
*  are met:
*
*   * Redistributions of source code must retain the above copyright
*     notice, this list of conditions and the following disclaimer.
*   * Redistributions in binary form must reproduce the above
*     copyright notice, this list of conditions and the following
*     disclaimer in the documentation and/or other materials provided
*     with the distribution.
*   * Neither the name of the Willow Garage nor the names of its
*     contributors may be used to endorse or promote products derived
*     from this software without specific prior written permission.
*
*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
*  POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
#include <ros/ros.h>
#include <nodelet/nodelet.h>
#include <image_transport/image_transport.h>
#include <image_transport/subscriber_filter.h>
#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <tf2_ros/buffer.h>
#include <tf2_ros/transform_listener.h>
#include <sensor_msgs/image_encodings.h>
#include <image_geometry/pinhole_camera_model.h>
#include <Eigen/Geometry>
#include <eigen_conversions/eigen_msg.h>
#include <depth_image_proc/depth_traits.h>

namespace depth_image_proc {

using namespace message_filters::sync_policies;
namespace enc = sensor_msgs::image_encodings;

class RegisterNodelet : public nodelet::Nodelet
{
  ros::NodeHandlePtr nh_depth_, nh_rgb_;
  boost::shared_ptr<image_transport::ImageTransport> it_depth_;

  // Subscriptions
  image_transport::SubscriberFilter sub_depth_image_;
  message_filters::Subscriber<sensor_msgs::CameraInfo> sub_depth_info_, sub_rgb_info_;
  boost::shared_ptr<tf2_ros::Buffer> tf_buffer_;
  boost::shared_ptr<tf2_ros::TransformListener> tf_;
  typedef ApproximateTime<sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::CameraInfo> SyncPolicy;
  typedef message_filters::Synchronizer<SyncPolicy> Synchronizer;
  boost::shared_ptr<Synchronizer> sync_;

  // Publications
  boost::mutex connect_mutex_;
  image_transport::CameraPublisher pub_registered_;

  image_geometry::PinholeCameraModel depth_model_, rgb_model_;

  virtual void onInit();

  void connectCb();

  void imageCb(const sensor_msgs::ImageConstPtr& depth_image_msg,
               const sensor_msgs::CameraInfoConstPtr& depth_info_msg,
               const sensor_msgs::CameraInfoConstPtr& rgb_info_msg);

  template<typename T>
  void convert(const sensor_msgs::ImageConstPtr& depth_msg,
               const sensor_msgs::ImagePtr& registered_msg,
               const Eigen::Affine3d& depth_to_rgb);
};

void RegisterNodelet::onInit()
{
  ros::NodeHandle& nh         = getNodeHandle();
  ros::NodeHandle& private_nh = getPrivateNodeHandle();
  nh_depth_.reset( new ros::NodeHandle(nh, "depth") );
  nh_rgb_.reset( new ros::NodeHandle(nh, "rgb") );
  it_depth_.reset( new image_transport::ImageTransport(*nh_depth_) );
  tf_buffer_.reset( new tf2_ros::Buffer );
  tf_.reset( new tf2_ros::TransformListener(*tf_buffer_) );

  // Read parameters
  int queue_size;
  private_nh.param("queue_size", queue_size, 5);

  // Synchronize inputs. Topic subscriptions happen on demand in the connection callback.
  sync_.reset( new Synchronizer(SyncPolicy(queue_size), sub_depth_image_, sub_depth_info_, sub_rgb_info_) );
  sync_->registerCallback(boost::bind(&RegisterNodelet::imageCb, this, _1, _2, _3));

  // Monitor whether anyone is subscribed to the output
  image_transport::ImageTransport it_depth_reg(ros::NodeHandle(nh, "depth_registered"));
  image_transport::SubscriberStatusCallback image_connect_cb = boost::bind(&RegisterNodelet::connectCb, this);
  ros::SubscriberStatusCallback info_connect_cb = boost::bind(&RegisterNodelet::connectCb, this);
  // Make sure we don't enter connectCb() between advertising and assigning to pub_registered_
  boost::lock_guard<boost::mutex> lock(connect_mutex_);
  pub_registered_ = it_depth_reg.advertiseCamera("image_rect", 1,
                                                 image_connect_cb, image_connect_cb,
                                                 info_connect_cb, info_connect_cb);
}

// Handles (un)subscribing when clients (un)subscribe
void RegisterNodelet::connectCb()
{
  boost::lock_guard<boost::mutex> lock(connect_mutex_);
  if (pub_registered_.getNumSubscribers() == 0)
  {
    sub_depth_image_.unsubscribe();
    sub_depth_info_ .unsubscribe();
    sub_rgb_info_   .unsubscribe();
  }
  else if (!sub_depth_image_.getSubscriber())
  {
    image_transport::TransportHints hints("raw", ros::TransportHints(), getPrivateNodeHandle());
    sub_depth_image_.subscribe(*it_depth_, "image_rect",  1, hints);
    sub_depth_info_ .subscribe(*nh_depth_, "camera_info", 1);
    sub_rgb_info_   .subscribe(*nh_rgb_,   "camera_info", 1);
  }
}

void RegisterNodelet::imageCb(const sensor_msgs::ImageConstPtr& depth_image_msg,
                              const sensor_msgs::CameraInfoConstPtr& depth_info_msg,
                              const sensor_msgs::CameraInfoConstPtr& rgb_info_msg)
{
  // Update camera models - these take binning & ROI into account
  depth_model_.fromCameraInfo(depth_info_msg);
  rgb_model_  .fromCameraInfo(rgb_info_msg);

  // Query tf2 for transform from (X,Y,Z) in depth camera frame to RGB camera frame
  Eigen::Affine3d depth_to_rgb;
  try
  {
    geometry_msgs::TransformStamped transform = tf_buffer_->lookupTransform (
                          rgb_info_msg->header.frame_id, depth_info_msg->header.frame_id,
                          depth_info_msg->header.stamp);

    tf::transformMsgToEigen(transform.transform, depth_to_rgb);
  }
  catch (tf2::TransformException& ex)
  {
    NODELET_WARN_THROTTLE(2, "TF2 exception:\n%s", ex.what());
    return;
    /// @todo Can take on order of a minute to register a disconnect callback when we
    /// don't call publish() in this cb. What's going on roscpp?
  }

  // Allocate registered depth image
  sensor_msgs::ImagePtr registered_msg( new sensor_msgs::Image );
  registered_msg->header.stamp    = depth_image_msg->header.stamp;
  registered_msg->header.frame_id = rgb_info_msg->header.frame_id;
  registered_msg->encoding        = depth_image_msg->encoding;

  cv::Size resolution = rgb_model_.reducedResolution();
  registered_msg->height = resolution.height;
  registered_msg->width  = resolution.width;
  // step and data set in convert(), depend on depth data type

  if (depth_image_msg->encoding == enc::TYPE_16UC1)
  {
    convert<uint16_t>(depth_image_msg, registered_msg, depth_to_rgb);
  }
  else if (depth_image_msg->encoding == enc::TYPE_32FC1)
  {
    convert<float>(depth_image_msg, registered_msg, depth_to_rgb);
  }
  else
  {
    NODELET_ERROR_THROTTLE(5, "Depth image has unsupported encoding [%s]", depth_image_msg->encoding.c_str());
    return;
  }

  // Registered camera info is the same as the RGB info, but uses the depth timestamp
  sensor_msgs::CameraInfoPtr registered_info_msg( new sensor_msgs::CameraInfo(*rgb_info_msg) );
  registered_info_msg->header.stamp = registered_msg->header.stamp;

  pub_registered_.publish(registered_msg, registered_info_msg);
}

template<typename T>
void RegisterNodelet::convert(const sensor_msgs::ImageConstPtr& depth_msg,
                              const sensor_msgs::ImagePtr& registered_msg,
                              const Eigen::Affine3d& depth_to_rgb)
{
  // Allocate memory for registered depth image
  registered_msg->step = registered_msg->width * sizeof(T);
  registered_msg->data.resize( registered_msg->height * registered_msg->step );
  // data is already zero-filled in the uint16 case, but for floats we want to initialize everything to NaN.
  DepthTraits<T>::initializeBuffer(registered_msg->data);

  // Extract all the parameters we need
  double inv_depth_fx = 1.0 / depth_model_.fx();
  double inv_depth_fy = 1.0 / depth_model_.fy();
  double depth_cx = depth_model_.cx(), depth_cy = depth_model_.cy();
  double depth_Tx = depth_model_.Tx(), depth_Ty = depth_model_.Ty();
  double rgb_fx = rgb_model_.fx(), rgb_fy = rgb_model_.fy();
  double rgb_cx = rgb_model_.cx(), rgb_cy = rgb_model_.cy();
  double rgb_Tx = rgb_model_.Tx(), rgb_Ty = rgb_model_.Ty();

  // Transform the depth values into the RGB frame
  /// @todo When RGB is higher res, interpolate by rasterizing depth triangles onto the registered image
  const T* depth_row = reinterpret_cast<const T*>(&depth_msg->data[0]);
  int row_step = depth_msg->step / sizeof(T);
  T* registered_data = reinterpret_cast<T*>(&registered_msg->data[0]);
  int raw_index = 0;
  for (unsigned v = 0; v < depth_msg->height; ++v, depth_row += row_step)
  {
    for (unsigned u = 0; u < depth_msg->width; ++u, ++raw_index)
    {
      T raw_depth = depth_row[u];
      if (!DepthTraits<T>::valid(raw_depth))
        continue;

      double depth = DepthTraits<T>::toMeters(raw_depth);

      /// @todo Combine all operations into one matrix multiply on (u,v,d)
      // Reproject (u,v,Z) to (X,Y,Z,1) in depth camera frame
      Eigen::Vector4d xyz_depth;
      xyz_depth << ((u - depth_cx)*depth - depth_Tx) * inv_depth_fx,
                   ((v - depth_cy)*depth - depth_Ty) * inv_depth_fy,
                   depth,
                   1;

      // Transform to RGB camera frame
      Eigen::Vector4d xyz_rgb = depth_to_rgb * xyz_depth;

      // Project to (u,v) in RGB image
      double inv_Z = 1.0 / xyz_rgb.z();
      int u_rgb = (rgb_fx*xyz_rgb.x() + rgb_Tx)*inv_Z + rgb_cx + 0.5;
      int v_rgb = (rgb_fy*xyz_rgb.y() + rgb_Ty)*inv_Z + rgb_cy + 0.5;

      if (u_rgb < 0 || u_rgb >= (int)registered_msg->width ||
          v_rgb < 0 || v_rgb >= (int)registered_msg->height)
        continue;

      T& reg_depth = registered_data[v_rgb*registered_msg->width + u_rgb];
      T  new_depth = DepthTraits<T>::fromMeters(xyz_rgb.z());
      // Validity and Z-buffer checks
      if (!DepthTraits<T>::valid(reg_depth) || reg_depth > new_depth)
        reg_depth = new_depth;
    }
  }
}

} // namespace depth_image_proc

// Register as nodelet
#include <pluginlib/class_list_macros.h>
PLUGINLIB_EXPORT_CLASS(depth_image_proc::RegisterNodelet,nodelet::Nodelet);


/* -*- Mode: C++; tab-width: 4; indent-tabs-mode: t; c-basic-offset: 4 -*- vim:set ts=4 sw=4 sts=4 noet: */
#include "uuidgen.h"

#include <cstdio>
#include <iostream>
#include <memory>
#include <string>

#define BOOST_DATE_TIME_NO_LIB
#include <boost/interprocess/file_mapping.hpp>
#include <boost/interprocess/mapped_region.hpp>

namespace flint {
namespace {

class Adler32Loader {
public:
	Adler32Loader(const Adler32Loader &) = delete;
	Adler32Loader &operator=(const Adler32Loader &) = delete;

	explicit Adler32Loader(const char *file)
  	: fm_(file, boost::interprocess::read_only),
	  mr_(fm_, boost::interprocess::read_only)
	{
	}

	bool Load(std::uint32_t *value) {
		const int kMod = 65521;

		void *addr = mr_.get_address();
		size_t size = mr_.get_size();
		if (size == 0) {
			std::cerr << "mapped region is of size 0" << std::endl;
			return false;
		}
		std::uint32_t a = 1;
		std::uint32_t b = 0;
		char *data = static_cast<char *>(addr);
		for (size_t i=0;i<size;i++) {
			a += data[i];
			a %= kMod;
			b += a;
			b %= kMod;
		}
		*value = (b<<16)|a;
		return true;
	}

private:
	boost::interprocess::file_mapping fm_;
	boost::interprocess::mapped_region mr_;
};

} // namespace

UuidGenerator::UuidGenerator(const boost::filesystem::path &path)
	: ran_(),
	  gen_(&ran_)
{
	std::string path_s = path.string();
	std::uint32_t value;
	std::unique_ptr<Adler32Loader> loader(new Adler32Loader(path_s.c_str()));
	if (!loader->Load(&value))
		value = 0; // fallback to seed zero
	ran_.seed(value);
}

boost::uuids::uuid UuidGenerator::operator()()
{
	return gen_();
}

}


/**  \file boundingbox.hpp \brief Module for box constrain management */
/*
-----------------------------------------------------------------------------
   Copyright (C) 2011-2015 Ruben Martinez-Cantin <rmcantin@unizar.es>

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

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

   You should have received a copy of the GNU Affero General Public License
   along with this program.  If not, see <http://www.gnu.org/licenses/>.
-----------------------------------------------------------------------------
*/
#ifndef  _BOUNDING_BOX_HPP_
#define  _BOUNDING_BOX_HPP_

// BOOST Libraries
#include <boost/numeric/ublas/vector.hpp>
#include "ublas_elementwise.hpp"

namespace bayesopt {

  namespace utils {
    /** Defines a bounding box or axis-alligned bound constraints. */
    template <class V>
    class BoundingBox
    {
    public:
      BoundingBox(const V &lbound, const V &ubound):
	mLowerBound(lbound), mRangeBound(ubound - lbound)
      {};

      virtual ~BoundingBox(){};

      inline V unnormalizeVector( const V &vin )
      {
	return ublas_elementwise_prod(vin,mRangeBound) + mLowerBound;
      };  // unnormalizeVector

      inline V normalizeVector( const V &vin )
      {
	return ublas_elementwise_div(vin - mLowerBound, mRangeBound);
      }  // normalizeVector

    protected:
      V mLowerBound; ///< Lower bound of the input space
      V mRangeBound; ///< Range (up-low) of the input space
    };

  } //namespace utils

} //namespace bayesopt

#endif


///////////////////////////////////////////////////////////////////////////////
// count_matching.hpp                                                        //
//                                                                           //
//  Copyright 2010 Erwann Rogard. Distributed under the Boost                //
//  Software License, Version 1.0. (See accompanying file                    //
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)         //
///////////////////////////////////////////////////////////////////////////////
#ifndef BOOST_STATISTICS_DETAIL_NON_PARAMETRIC_CONTINGENCY_TABLE_CELLS_COUNT_MATCHING_HPP_ER_2010
#define BOOST_STATISTICS_DETAIL_NON_PARAMETRIC_CONTINGENCY_TABLE_CELLS_COUNT_MATCHING_HPP_ER_2010
#include <boost/mpl/assert.hpp>
#include <boost/type_traits/is_same.hpp>
#include <boost/statistics/detail/non_parametric/contingency_table/cells/cells.hpp>
#include <boost/statistics/detail/non_parametric/contingency_table/cells/traits.hpp>
#include <boost/mpl/detail/wrapper.hpp>

namespace boost{
namespace statistics{
namespace detail{
namespace contingency_table{
namespace result_of{

    template<typename Keys,typename AccSet>
    struct count_matching
        : contingency_table::cells_traits::mapped_of<Keys, AccSet>{};

}// result_of

  	template<typename Keys,typename AccSet,typename T>
    typename contingency_table::result_of::count_matching<Keys,AccSet>::type
    count_matching(AccSet const& acc,const T& sample){
        namespace ns = contingency_table;
        return ns::extract::cells<Keys>( acc )[ sample ];
    }

    // A: accumulator set
    // T: sample
    // F: unary functor
    template<typename A,typename T,typename F>
    struct fun_count_matcher{

        typedef void result_type;

        fun_count_matcher(const A& a,const T& s, const F& f)
            :acc( a ), sample( s ), fun( f ){}

        template<typename Keys>
        void operator()(const boost::mpl::detail::wrapper<Keys>& u)const{
            namespace ns = contingency_table;
            this->fun(
                ns::count_matching<Keys>(
                    this->acc,
                    this->sample
                )
            );
        }

        private:
        const A& acc;
        const T& sample;
        F fun;
    };

    template<typename A,typename T,typename F>
    fun_count_matcher<A,T,F>
    make_fun_count_matcher(const A& a,const T& s, const F& f){
        namespace ns = contingency_table;
        typedef ns::fun_count_matcher<A,T,F> result_;
        return result_(a, s, f);
    }

}// contingency_table
}// detail
}// statistics
}// boost

#endif


//
// Created by asem on 13/09/18.
//
#include "SVMModel.hpp"
#include <dlib/global_optimization.h>
#include "ConfusionMatrix.hpp"

SVMModel::SVMModel( SVMConfiguration config )
        : _configuration( std::move( config ))
{

}

void SVMModel::fit(
        std::vector<std::string_view> &&labels,
        std::vector<std::vector<double >> &&samples
)
{
    _decisionFunction =
            _fit( _registerLabels( std::move( labels )),
                  _svmSamples( std::move( samples )),
                  _configuration,
                  _label2Index );
}

SVMModel::DecisionFunction SVMModel::_fit(
        std::vector<Label> &&labels,
        std::vector<SampleType> &&samples,
        const SVMConfiguration &configuration,
        const std::map<std::string_view, int> &label2Index
)
{
    if ( configuration.tuning )
    {
        return _fitTuningHyperParameters(
                std::move( labels ), std::move( samples ), configuration, label2Index );
    } else
    {
        assert( configuration.gamma );
        return _fitFixedHyperParameters(
                std::move( labels ), std::move( samples ), configuration, label2Index );
    }
}

ScoredLabels SVMModel::predict( std::vector<double> &&features ) const
{
    auto bestLabel = _index2Label.at( _decisionFunction( _svmFeatures( std::move( features ))));
    std::map<std::string_view, size_t> votes;
    for ( auto &[label, idx] : _label2Index )
        votes[label] += 0;
    votes[bestLabel] += 1;

    ScoredLabels vPQ( _label2Index.size());
    for ( auto&[label, n] : votes )
        vPQ.emplace( label, n );
    return vPQ;
}

SVMModel::SampleType SVMModel::_svmFeatures( std::vector<double> &&features )
{
    using namespace dlib_utilities;
    return vector_to_column_matrix_like( std::move( features ));
}

std::vector<SVMModel::SampleType> SVMModel::_svmSamples( std::vector<std::vector<double >> &&samples )
{
    std::vector<SampleType> svmSamples;
    for ( auto &&f : samples )
        svmSamples.emplace_back( _svmFeatures( std::move( f )));
    return svmSamples;
}

std::vector<std::pair<int, int>> SVMModel::_one2oneCombination( int nLabels )
{
    std::vector<std::pair<int, int>> pairs;
    for ( auto i = 0; i < nLabels; ++i )
    {
        for ( auto j = i + 1; j < nLabels; ++j )
        {
            pairs.emplace_back( i, j );
        }
    }
    return pairs;
}

std::vector<SVMModel::Label> SVMModel::_registerLabels( std::vector<std::string_view> &&labels )
{
    std::vector<Label> svmLabels;

    _labels.clear();
    _label2Index.clear();
    _index2Label.clear();
    for ( auto label : labels )
        _labels.insert( label );

    int i = 0;
    for ( auto &label : _labels )
    {
        _label2Index.emplace( label, i );
        _index2Label.emplace( i, label );
        ++i;
    }

    for ( auto label : labels )
        svmLabels.push_back( _label2Index.at( label ));
    return svmLabels;
}

SVMModel::DecisionFunction SVMModel::_fitFixedHyperParameters(
        std::vector<Label> &&labels,
        std::vector<SampleType> &&samples,
        const SVMConfiguration &configuration,
        const std::map<std::string_view, int> &label2Index
)
{
    using GammaSetting = SVMConfiguration::GammaMultiLabelSettingEnum;
    assert( configuration.gamma );
    if ( !configuration.gamma )
        throw std::runtime_error( "Missing gamma value." );
    auto &&gamma = configuration.gamma.value();

    switch ( configuration.gammaSetting )
    {
        case GammaSetting::SingleGamma_ONE_VS_ALL :
        {
            assert( gamma.size() == 1 );
            if ( gamma.size() != 1 )
                throw std::runtime_error( "Gamma vector size must equal 1." );

            SVMOneVsAllTrainer trainer;
            trainer.set_num_threads( std::thread::hardware_concurrency());
            SVMBinaryTrainer btrainer;
            btrainer.use_classification_loss_for_loo_cv();
            btrainer.set_kernel( SVMRBFKernel( gamma.front()));
            trainer.set_trainer( btrainer );
            return trainer.train( samples, labels );
        }
            break;
        case GammaSetting::GammaVector_ONE_VS_ALL:
        {
            assert( gamma.size() == label2Index.size());
            if ( gamma.size() != label2Index.size())
                throw std::runtime_error( "Gammas count should equal to labels count." );

            SVMOneVsAllTrainer trainer;
            trainer.set_num_threads( std::thread::hardware_concurrency());

            for ( auto i = 0; i < label2Index.size(); ++i )
            {
                SVMBinaryTrainer btrainer;
                btrainer.use_classification_loss_for_loo_cv();
                btrainer.set_kernel( SVMRBFKernel( gamma.at( i )));
                btrainer.be_verbose();
                trainer.set_trainer( btrainer, i );
                trainer.be_verbose();
            }
            return trainer.train( samples, labels );
        }
            break;
        case GammaSetting::GammaVector_ONE_VS_ONE:
        {
            auto nLabels = label2Index.size();
            auto one2oneComb = _one2oneCombination( static_cast<int>(nLabels));
            assert( gamma.size() == nLabels * ( nLabels - 1 ) / 2 &&
                    gamma.size() == one2oneComb.size());
            if ( gamma.size() != nLabels * ( nLabels - 1 ) / 2
                 || gamma.size() != one2oneComb.size())
                throw std::runtime_error( "Gammas count should equal to n(n-1)/2; n: labeles count." );

            SVMOneVsOneTrainer trainer;
            trainer.set_num_threads( std::thread::hardware_concurrency());

            for ( auto i = 0; i < one2oneComb.size(); ++i )
            {
                SVMBinaryTrainer btrainer;
                btrainer.use_classification_loss_for_loo_cv();
                btrainer.set_kernel( SVMRBFKernel( gamma.at( i )));
                trainer.set_trainer( btrainer, one2oneComb.at( i ).first, one2oneComb.at( i ).second );
            }
            return trainer.train( samples, labels );
        }
            break;
        default:throw std::runtime_error( "Unhandled gamma setting" );
    }

    throw std::runtime_error( "Function should not reach here." );
}

auto SVMModel::_crossValidationScoreSingleGamma_ONE_VS_ALL(
        const std::vector<SVMModel::SampleType> &samples,
        const std::vector<SVMModel::Label> &labels
)
{
    return [&]( const double gamma )
    {
      SVMBinaryTrainer btrainer;
      btrainer.use_classification_loss_for_loo_cv();

      SVMOneVsAllTrainer trainer;
      trainer.set_num_threads( 1 );
      btrainer.set_kernel( SVMRBFKernel( gamma ));
      trainer.set_trainer( btrainer );

      // Finally, perform 10-fold cross validation and then print and return the results.
      auto result = dlib::cross_validate_multiclass_trainer( trainer, samples, labels, 10 );
      auto rawCM = std::vector<std::vector<size_t>>( result.nr(), std::vector<size_t>( result.nc()));
      for ( auto r = 0; r < result.nr(); ++r )
          for ( auto c = 0; c < result.nc(); ++c )
              rawCM[r][c] = static_cast<size_t>(result( r, c ));

      auto cm = ConfusionMatrix<std::string, size_t>::fromRawConfusionMatrix( rawCM );
      auto objective = cm.microFScore();

//        fmt::print( "gamma:{:.11f}\n"
//                    "CV F1-Score:{}\n", gamma, objective );
      return objective;
    };
}

auto SVMModel::_crossValidationScoreMultipleGammas_ONE_VS_ALL(
        const std::vector<SVMModel::SampleType> &samples,
        const std::vector<SVMModel::Label> &labels,
        const std::map<std::string_view, int> &label2Index
)
{
    return [&]( dlib::matrix<double, 0, 1> gammas )
    {
      assert( gammas.size() == label2Index.size());
      SVMOneVsAllTrainer trainer;
      trainer.set_num_threads( 1 );
      for ( auto &&[label, index] : label2Index )
      {
          SVMBinaryTrainer btrainer;
          btrainer.use_classification_loss_for_loo_cv();
          btrainer.set_kernel( SVMRBFKernel( gammas( index )));
          trainer.set_trainer( btrainer, index );
      }

      // Finally, perform 10-fold cross validation and then print and return the results.
      auto result = dlib::cross_validate_multiclass_trainer( trainer, samples, labels, 10 );
      auto rawCM = std::vector<std::vector<size_t>>( result.nr(), std::vector<size_t>( result.nc()));
      for ( auto r = 0; r < result.nr(); ++r )
          for ( auto c = 0; c < result.nc(); ++c )
              rawCM[r][c] = static_cast<size_t>(result( r, c ));

      auto cm = ConfusionMatrix<std::string, size_t>::fromRawConfusionMatrix( rawCM );
      auto objective = cm.microFScore();
//        fmt::print( "ovaGammas:{:.11f}\n"
//                    "CV F1-Score:{}\n", fmt::join( ovaGammas, ", " ), objective );
      return objective;
    };
}

auto SVMModel::_crossValidationScoreMultipleGammas_ONE_VS_ONE(
        const std::vector<SVMModel::SampleType> &samples,
        const std::vector<SVMModel::Label> &labels,
        const std::vector<std::pair<int, int>> &one2oneComb
)
{
    return [&]( dlib::matrix<double, 0, 1> gammas )
    {
      assert( gammas.size() == one2oneComb.size());
      SVMOneVsOneTrainer trainer;
      trainer.set_num_threads( 1 );
      for ( auto i = 0; i < one2oneComb.size(); ++i )
      {
          SVMBinaryTrainer btrainer;
          btrainer.use_classification_loss_for_loo_cv();
          btrainer.set_kernel( SVMRBFKernel( gammas( i )));
          trainer.set_trainer( btrainer, one2oneComb.at( i ).first, one2oneComb.at( i ).second );
      }

      // Finally, perform 10-fold cross validation and then print and return the results.
      auto result = dlib::cross_validate_multiclass_trainer( trainer, samples, labels, 10 );
      auto rawCM = std::vector<std::vector<size_t>>( result.nr(), std::vector<size_t>( result.nc()));
      for ( auto r = 0; r < result.nr(); ++r )
          for ( auto c = 0; c < result.nc(); ++c )
              rawCM[r][c] = static_cast<size_t>(result( r, c ));

      auto cm = ConfusionMatrix<std::string, size_t>::fromRawConfusionMatrix( rawCM );
      auto objective = cm.microFScore();
//        fmt::print( "ovoGammas:{:.11f}\n"
//                    "CV F1-Score:{}\n" , fmt::join( gammas , ", " ) , objective );
      return objective;
    };
}

SVMModel::DecisionFunction SVMModel::_fitTuningHyperParameters(
        std::vector<Label> &&labels,
        std::vector<SampleType> &&samples,
        const SVMConfiguration &configuration,
        const std::map<std::string_view, int> &label2Index
)
{
    using GammaSetting = SVMConfiguration::GammaMultiLabelSettingEnum;

    assert( configuration.tuning );
    auto newConfig = configuration;
    newConfig.tuning.reset();
    newConfig.gamma.reset();

    auto tp = dlib::thread_pool(
            static_cast<size_t>(std::max( 1, static_cast<int>(std::thread::hardware_concurrency()))));
    auto[min, max] = configuration.tuning->gammaBounds;
    auto maxCalls = configuration.tuning->maxTrials;

    switch ( configuration.gammaSetting )
    {
        case GammaSetting::SingleGamma_ONE_VS_ALL :
        {
            using DLIBCVector = dlib::matrix<double, 0, 1>;

            DLIBCVector minGammas = dlib::uniform_matrix<double>( 1, 1, min );
            DLIBCVector maxGammas = dlib::uniform_matrix<double>( 1, 1, max );

            auto result = dlib::find_max_global(
                    tp, _crossValidationScoreSingleGamma_ONE_VS_ALL( samples, labels ),
                    minGammas, maxGammas, dlib::max_function_calls( maxCalls ));

            newConfig.gamma.emplace( {result.x( 0 )} );
        }
            break;
        case GammaSetting::GammaVector_ONE_VS_ALL:
        {
            using DLIBCVector = dlib::matrix<double, 0, 1>;

            DLIBCVector minGammas = dlib::uniform_matrix<double>( label2Index.size(), 1, min );
            DLIBCVector maxGammas = dlib::uniform_matrix<double>( label2Index.size(), 1, max );

            auto result = dlib::find_max_global(
                    tp, _crossValidationScoreMultipleGammas_ONE_VS_ALL(
                            samples, labels, label2Index ), minGammas, maxGammas,
                    dlib::max_function_calls( static_cast<size_t>( maxCalls )));

            auto &&gammas = result.x;
            std::vector<double> newGammas;
            for ( double gamma : gammas )
                newGammas.push_back( gamma );

            newConfig.gamma.emplace( std::move( newGammas ));
        }
            break;
        case GammaSetting::GammaVector_ONE_VS_ONE:
        {
            int nLabels = static_cast<int>( label2Index.size());
            auto one2oneComb = _one2oneCombination( nLabels );
            using DLIBCVector = dlib::matrix<double, 0, 1>;

            DLIBCVector minGammas = dlib::uniform_matrix<double>( one2oneComb.size(), 1, min );
            DLIBCVector maxGammas = dlib::uniform_matrix<double>( one2oneComb.size(), 1, max );

            auto result = dlib::find_max_global(
                    tp, _crossValidationScoreMultipleGammas_ONE_VS_ONE(
                            samples, labels, one2oneComb ), minGammas, maxGammas,
                    dlib::max_function_calls( static_cast<size_t>( maxCalls )));

            auto &&gammas = result.x;
            std::vector<double> newGammas;
            for ( double gamma : gammas )
                newGammas.push_back( gamma );

            newConfig.gamma.emplace( std::move( newGammas ));
        }
            break;
        default:throw std::runtime_error( "Unhandled gamma setting" );
    }

    return _fit( std::move( labels ), std::move( samples ), newConfig, label2Index );
}


/** -*- Mode: C++; indent-tabs-mode: nil; c-basic-offset: 4; fill-column: 78 -*-
 *
 * @file uri_grammar
 *
 * From:
 *
 * Copyright 2010  Braden McDaniel
 *
 * Distributed under the Boost Software License, Version 1.0.
 *
 * See accompanying file COPYING or copy at
 * http://www.boost.org/LICENSE_1_0.txt
 */

# ifndef URI_GRAMMAR_HPP
#  define URI_GRAMMAR_HPP

#   include <boost/fusion/include/boost_array.hpp>
#   include <boost/fusion/include/std_pair.hpp>
#   include <boost/spirit/include/qi.hpp>
#   include <boost/spirit/include/phoenix.hpp>
#include <boost/cstdint.hpp>
#define BOOST_SPIRIT_DEBUG
namespace uri {

template <typename Iterator>
struct ipv4_grammar :
	boost::spirit::qi::grammar<Iterator, boost::uint32_t()> {

    ipv4_grammar(): ipv4_grammar::base_type(ipv4address)
    {
	using boost::spirit::qi::eps;
	using boost::spirit::qi::_1;
	using boost::spirit::qi::_val;
	using boost::uint32_t;

	ipv4address
		=   eps[_val = 0]
		>> dec_octet[_val  = (_1 << 24)] >> '.'
		>> dec_octet[_val |= (_1 << 16)] >> '.'
		>> dec_octet[_val |= (_1 << 8) ] >> '.'
		>> dec_octet[_val |= _1]
		;

		dec_octet
		=   boost::spirit::qi::uint_parser<boost::uint8_t, 10, 1, 3>()
		;
    }

		//
		// dec_octet needs a uint32_t attribute so that bit shifting will work
		// properly.
		//
		boost::spirit::qi::rule<Iterator, boost::uint32_t()> ipv4address,
		dec_octet;
    };

		template <typename Iterator>
		struct ipv6_grammar :
		boost::spirit::qi::grammar<Iterator, boost::spirit::qi::locals<unsigned>, boost::array<boost::uint16_t, 8>()> {

	    ipv6_grammar(): ipv6_grammar::base_type(ipv6address)
	    {
		using boost::uint16_t;
		using boost::uint64_t;
		using boost::array;
		using boost::spirit::qi::eps;
		using boost::spirit::qi::lit;
		using boost::spirit::qi::repeat;
		using boost::spirit::qi::_1;
		using boost::spirit::qi::_a;
		using boost::spirit::qi::_val;
		using boost::phoenix::at_c;
		using boost::phoenix::static_cast_;

		ipv6address
			=                                                                                repeat(6)[h16[_val[_a++]    = _1] >> ':'] >> ls32[at_c<6>(_val) = at_c<0>(_1), at_c<7>(_val) = at_c<1>(_1)]
			|   eps[_val = array<uint16_t, 8>()        ]                                                                            >> lit("::")[_a = 1] >> repeat(5)[h16[_val[_a++]    = _1] >> ':'] >> ls32[at_c<6>(_val) = at_c<0>(_1), at_c<7>(_val) = at_c<1>(_1)]
			|   eps[_val = array<uint16_t, 8>()        ] >> -(                                             h16[at_c<0>(_val) = _1]) >> lit("::")[_a = 2] >> repeat(4)[h16[_val[_a++]    = _1] >> ':'] >> ls32[at_c<6>(_val) = at_c<0>(_1), at_c<7>(_val) = at_c<1>(_1)]
			|   eps[_val = array<uint16_t, 8>(), _a = 0] >> -(repeat(0, 1)[h16[_val[_a++] = _1] >> ':'] >> h16[_val[_a]      = _1]) >> lit("::")[_a = 3] >> repeat(3)[h16[_val[_a++]    = _1] >> ':'] >> ls32[at_c<6>(_val) = at_c<0>(_1), at_c<7>(_val) = at_c<1>(_1)]
			|   eps[_val = array<uint16_t, 8>(), _a = 0] >> -(repeat(0, 2)[h16[_val[_a++] = _1] >> ':'] >> h16[_val[_a]      = _1]) >> lit("::")[_a = 4] >> repeat(2)[h16[_val[_a++]    = _1] >> ':'] >> ls32[at_c<6>(_val) = at_c<0>(_1), at_c<7>(_val) = at_c<1>(_1)]
			|   eps[_val = array<uint16_t, 8>(), _a = 0] >> -(repeat(0, 3)[h16[_val[_a++] = _1] >> ':'] >> h16[_val[_a]      = _1]) >>     "::"          >>           h16[at_c<5>(_val) = _1] >> ':'  >> ls32[at_c<6>(_val) = at_c<0>(_1), at_c<7>(_val) = at_c<1>(_1)]
			|   eps[_val = array<uint16_t, 8>(), _a = 0] >> -(repeat(0, 4)[h16[_val[_a++] = _1] >> ':'] >> h16[_val[_a]      = _1]) >>     "::"                                                       >> ls32[at_c<6>(_val) = at_c<0>(_1), at_c<7>(_val) = at_c<1>(_1)]
			|   eps[_val = array<uint16_t, 8>(), _a = 0] >> -(repeat(0, 5)[h16[_val[_a++] = _1] >> ':'] >> h16[_val[_a]      = _1]) >>     "::"          >>           h16[at_c<7>(_val) = _1]
			|   eps[_val = array<uint16_t, 8>(), _a = 0] >> -(repeat(0, 6)[h16[_val[_a++] = _1] >> ':'] >> h16[_val[_a]      = _1]) >>     "::"
			;

			ls32
			=   h16 >> ':' >> h16
			|   ipv4address[at_c<0>(_val) = (_1 >> 16),
			at_c<1>(_val) = (0x0000ffff & _1)]
			;

			BOOST_SPIRIT_DEBUG_NODES((ipv6address))
	    }

			boost::spirit::qi::rule<Iterator,
			std::pair<boost::uint64_t, boost::uint64_t>()>
			start;
			boost::spirit::qi::rule<Iterator,
			boost::spirit::qi::locals<unsigned>,
			boost::array<boost::uint16_t, 8>()>
			ipv6address;
			boost::spirit::qi::rule<Iterator, std::pair<boost::uint16_t, boost::uint16_t>()> ls32;
			boost::spirit::qi::uint_parser<boost::uint16_t, 16, 1, 4> h16;
			ipv4_grammar<Iterator> ipv4address;
	    };

			template <typename Iterator>
			struct components {
		    boost::iterator_range<Iterator> scheme, userinfo, host, port, path, query, fragment;
		};

		template <typename Iterator>
		struct sub_delims_grammar :
			boost::spirit::qi::grammar<Iterator, char()> {

		    sub_delims_grammar(): sub_delims_grammar::base_type(sub_delims)
		    {
			using namespace boost::spirit::qi;
			sub_delims
				%=   char_("!$&\\()*+,;=")
				;
		    }

		    boost::spirit::qi::rule<Iterator, char()> sub_delims;
		};

		template <typename Iterator>
		struct pct_encoded_grammar :
			boost::spirit::qi::grammar<Iterator, std::string()> {

		    pct_encoded_grammar(): pct_encoded_grammar::base_type(pct_encoded)
		    {
			using namespace boost::spirit::qi;
			pct_encoded
				%=   '%' >> xdigit >> xdigit
					    ;
		    }

		    boost::spirit::qi::rule<Iterator, std::string()> pct_encoded;
		};

		template <typename Iterator>
		struct unreserved_grammar :
			boost::spirit::qi::grammar<Iterator, char()> {

		    unreserved_grammar(): unreserved_grammar::base_type(unreserved)
		    {
			using namespace boost::spirit::qi;

			unreserved
				%=   alnum
				|   char_("-._~")
				;
		    }

		    boost::spirit::qi::rule<Iterator, char()> unreserved;
		};

		template <typename Iterator>
		struct pchar_grammar : boost::spirit::qi::grammar<Iterator> {
		    pchar_grammar(): pchar_grammar::base_type(pchar)
		    {
			using namespace boost::spirit::qi;

			pchar
				=   unreserved
				|   pct_encoded
				|   sub_delims
				|   char_(":@")
				;
		    }

		    boost::spirit::qi::rule<Iterator> pchar;
		    sub_delims_grammar<Iterator> sub_delims;
		    pct_encoded_grammar<Iterator> pct_encoded;
		    unreserved_grammar<Iterator> unreserved;
		};

		template <typename Iterator>
		struct scheme_grammar : boost::spirit::qi::grammar<Iterator> {
		    scheme_grammar(): scheme_grammar::base_type(scheme)
		    {
			using namespace boost::spirit::qi;
			scheme
				=   alpha >> *(alnum | char_("+-."))
					     ;
		    }

		    boost::spirit::qi::rule<Iterator> scheme;
		};

		/*
    template <typename Iterator>
    struct authority_grammar : boost::spirit::qi::grammar<Iterator> {
	explicit authority_grammar(components<Iterator> & c):
	    authority_grammar::base_type(authority),
	    components_(c)
	{
	    using namespace boost::spirit::qi;

	    userinfo
		=  *(   unreserved
		    |   pct_encoded
		    |   sub_delims
		    |   ':'
		    )
		;

	    ip_literal
		=   '[' >> (ipv6address | ipvfuture) >> ']'
		;

	    ipvfuture
		=   'v' >> +xdigit >> '.' >> +(unreserved | sub_delims | ':')
		;

	    ipv6address
		=                                                 repeat(6)[h16 >> ':'] >> ls32
		|                                         "::" >> repeat(5)[h16 >> ':'] >> ls32
		|   -(                            h16) >> "::" >> repeat(4)[h16 >> ':'] >> ls32
		|   -(repeat(0, 1)[h16 >> ':'] >> h16) >> "::" >> repeat(3)[h16 >> ':'] >> ls32
		|   -(repeat(0, 2)[h16 >> ':'] >> h16) >> "::" >> repeat(2)[h16 >> ':'] >> ls32
		|   -(repeat(0, 3)[h16 >> ':'] >> h16) >> "::" >>           h16 >> ':'  >> ls32
		|   -(repeat(0, 4)[h16 >> ':'] >> h16) >> "::"                          >> ls32
		|   -(repeat(0, 5)[h16 >> ':'] >> h16) >> "::" >>           h16
		|   -(repeat(0, 6)[h16 >> ':'] >> h16) >> "::"
		;

	    h16
		=   repeat(1, 4)[xdigit]
		;

	    ls32
		=   h16 >> ':' >> h16
		|   ipv4address
		;

	    ipv4address
		=   dec_octet >> '.' >> dec_octet >> '.' >> dec_octet >> '.'
		    >> dec_octet
		;

	    dec_octet
		=   "25" >> char_("0-5")
		|   '2' >> char_("0-4") >> digit
		|   '1' >> repeat(2)[digit]
		|   char_("1-9") >> digit
		|   digit
		;

	    reg_name
		=  *(   unreserved
		    |   pct_encoded
		    |   sub_delims
		    )
		;

	    host
		=   ip_literal
		|   ipv4address
		|   reg_name
		;

	    port
		=   *digit
		;

	    authority
		=  -(   raw[userinfo][boost::phoenix::ref(userinfo_temp_) = _1]
			>> '@'
		    )[
			boost::phoenix::ref(components_.userinfo) =
			    boost::phoenix::ref(userinfo_temp_)
		    ]
		    >> raw[host][boost::phoenix::ref(components_.host) = _1]
		    >> -(':' >> raw[port][boost::phoenix::ref(components_.port) = _1])
		;

	    BOOST_SPIRIT_DEBUG_NODE(authority);
	    BOOST_SPIRIT_DEBUG_NODE(host);
	    BOOST_SPIRIT_DEBUG_NODE(ipv4address);
	    BOOST_SPIRIT_DEBUG_NODE(ipv6address);
	    BOOST_SPIRIT_DEBUG_NODE(dec_octet);
	    BOOST_SPIRIT_DEBUG_NODE(reg_name);
	    BOOST_SPIRIT_DEBUG_NODE(ipv6address);
	    BOOST_SPIRIT_DEBUG_NODE(ls32);
	    BOOST_SPIRIT_DEBUG_NODE(h16);
	}

	components<Iterator> & components_;
	boost::iterator_range<Iterator> userinfo_temp_;

	boost::spirit::qi::rule<Iterator> authority, ip_literal, ipv6address,
	    userinfo, host, port, reg_name, ipv4address, dec_octet, h16, ls32,
	    ipvfuture;
	sub_delims_grammar<Iterator> sub_delims;
	pct_encoded_grammar<Iterator> pct_encoded;
	unreserved_grammar<Iterator> unreserved;
    };
*/

		template <typename Iterator>
		struct path_abempty_grammar : boost::spirit::qi::grammar<Iterator> {

		    path_abempty_grammar(): path_abempty_grammar::base_type(path_abempty)
		    {
			using namespace boost::spirit::qi;
			path_abempty
				=   *('/' >> *pchar)
				;
		    }

		    boost::spirit::qi::rule<Iterator> path_abempty;
		    pchar_grammar<Iterator> pchar;
		};

		template <typename Iterator>
		struct path_absolute_grammar : boost::spirit::qi::grammar<Iterator> {

		    path_absolute_grammar(): path_absolute_grammar::base_type(path_absolute)
		    {
			using namespace boost::spirit::qi;
			path_absolute
				=   '/' >> -(+pchar >> *('/' >> *pchar))
					   ;
		    }

		    boost::spirit::qi::rule<Iterator> path_absolute;
		    pchar_grammar<Iterator> pchar;
		};

		template <typename Iterator>
		struct hier_part_grammar : boost::spirit::qi::grammar<Iterator> {
		    explicit hier_part_grammar(components<Iterator> & c):
			hier_part_grammar::base_type(hier_part),
			components_(c)/*,
	      authority(c)*/
		    {
			using namespace boost::spirit::qi;

			path_rootless
				=   +pchar >> *('/' >> *pchar)
					      ;

			path_empty
				=   eps
				;

			hier_part
				=   /*"//" >> authority >> raw[path_abempty][
				boost::phoenix::ref(components_.path) = _1
			    ]
			|*/   raw[(path_absolute | path_rootless | path_empty)][
				boost::phoenix::ref(components_.path) = _1
				]
				;
		    }

		    components<Iterator> & components_;

		    boost::spirit::qi::rule<Iterator> hier_part, path_rootless, path_empty;
		    //authority_grammar<Iterator> authority;
		    path_abempty_grammar<Iterator> path_abempty;
		    path_absolute_grammar<Iterator> path_absolute;
		    pchar_grammar<Iterator> pchar;
		};

		template <typename Iterator>
		struct query_grammar : boost::spirit::qi::grammar<Iterator> {
		    query_grammar(): query_grammar::base_type(query)
		    {
			using namespace boost::spirit::qi;
			query
				=  *(pchar | char_("/?"))
				;
		    }

		    boost::spirit::qi::rule<Iterator> query;
		    pchar_grammar<Iterator> pchar;
		};

		template <typename Iterator>
		struct fragment_grammar : boost::spirit::qi::grammar<Iterator> {

		    fragment_grammar(): fragment_grammar::base_type(fragment)
		    {
			using namespace boost::spirit::qi;
			fragment
				=  *(pchar | char_("/?"))
				;
		    }

		    boost::spirit::qi::rule<Iterator> fragment;
		    pchar_grammar<Iterator> pchar;
		};

		template <typename Iterator>
		struct relative_grammar : boost::spirit::qi::grammar<Iterator> {
		    explicit relative_grammar(components<Iterator> & c):
			relative_grammar::base_type(relative_ref),
			components_(c)/*,
	      authority(c)*/
		    {
			using namespace boost::spirit::qi;

			segment_nz_nc
				=  +(   unreserved
					|   pct_encoded
					|   sub_delims
					|   '@'
					)
				;

			path_noscheme
				=   segment_nz_nc >> *('/' >> *pchar)
						     ;

			path_empty
				=   eps
				;

			relative_part
				=   /*"//" >> authority >> raw[path_abempty][
				boost::phoenix::ref(components_.path) = _1
			    ]
			|*/   raw[(path_absolute | path_noscheme | path_empty)][
				boost::phoenix::ref(components_.path) = _1
				]
				;

			relative_ref
				=   relative_part
				>> -('?'
				     >> raw[query][boost::phoenix::ref(components_.query) = _1])
				>> -('#'
				     >> raw[fragment][boost::phoenix::ref(components_.fragment) = _1])
				;

			BOOST_SPIRIT_DEBUG_NODE(relative_ref);
			BOOST_SPIRIT_DEBUG_NODE(relative_part);
		    }

		    components<Iterator> & components_;

		    boost::spirit::qi::rule<Iterator> relative_ref, relative_part,
			    path_noscheme, path_empty, segment_nz_nc;
		    sub_delims_grammar<Iterator> sub_delims;
		    pct_encoded_grammar<Iterator> pct_encoded;
		    unreserved_grammar<Iterator> unreserved;
		    pchar_grammar<Iterator> pchar;
		    //authority_grammar<Iterator> authority;
		    query_grammar<Iterator> query;
		    fragment_grammar<Iterator> fragment;
		    path_abempty_grammar<Iterator> path_abempty;
		    path_absolute_grammar<Iterator> path_absolute;
		};

		template <typename Iterator>
		struct grammar : boost::spirit::qi::grammar<Iterator> {

		    explicit grammar(components<Iterator> & c):
			grammar::base_type(uri_reference),
			components_(c),
			hier_part(c),
			relative_ref(c)
		    {
			using namespace boost::spirit::qi;

			uri
				=   raw[scheme][boost::phoenix::ref(components_.scheme) = _1]
				>> ':' >> hier_part
				>> -('?' >> raw[query][boost::phoenix::ref(components_.query) = _1])
				>> -('#' >> raw[fragment][boost::phoenix::ref(components_.fragment) = _1])
				;

			uri_reference
				=   uri
				|   raw[eps][boost::phoenix::ref(components_.scheme) = _1] >> relative_ref
											      ;

			BOOST_SPIRIT_DEBUG_NODE(uri_reference);
			BOOST_SPIRIT_DEBUG_NODE(uri);
			BOOST_SPIRIT_DEBUG_NODE(hier_part);
		    }

		    typedef boost::spirit::qi::rule<Iterator> rule_t;

		    components<Iterator> & components_;

		    rule_t uri_reference, uri;
		    scheme_grammar<Iterator> scheme;
		    hier_part_grammar<Iterator> hier_part;
		    relative_grammar<Iterator> relative_ref;
		    query_grammar<Iterator> query;
		    fragment_grammar<Iterator> fragment;
		};

		template <typename Iterator>
		struct absolute_grammar : boost::spirit::qi::grammar<Iterator> {

		    explicit absolute_grammar(components<Iterator> & c):
			absolute_grammar::base_type(absolute_uri),
			components_(c),
			hier_part(c)
		    {
			using namespace boost::spirit::qi;
			absolute_uri
				=   raw[scheme][boost::phoenix::ref(components_.fragment) = _1]
				>> ':' >> hier_part
				>> -('?' >> raw[query][boost::phoenix::ref(components_.fragment) = _1])
				;
		    }

		    components<Iterator> & components_;

		    boost::spirit::qi::rule<Iterator> absolute_uri;
		    scheme_grammar<Iterator> scheme;
		    hier_part_grammar<Iterator> hier_part;
		    query_grammar<Iterator> query;
		};
	    } // namespace uri

# endif // ifndef URI_GRAMMAR_HPP


#include <boost/numeric/odeint/iterator/n_step_time_iterator.hpp>


#include "map.h"
#include "node.h"
#include "path_finding.h"
#include <iostream>
#include <boost/assert.hpp>
#include <boost/assign/list_inserter.hpp>

pf::Map generateMap()
{
	std::vector<int> q0, q1, q2, q3, q4, q5, q6, q7, q8, q9;

	boost::assign::push_back(q0) (1)(1)(1)(1)(1)(9)(1)(1)(1)(1);
	boost::assign::push_back(q1) (1)(1)(1)(1)(1)(9)(1)(1)(1)(1);
	boost::assign::push_back(q2) (1)(1)(9)(9)(9)(9)(9)(9)(9)(1);
	boost::assign::push_back(q3) (1)(1)(1)(1)(1)(1)(1)(1)(9)(1);
	boost::assign::push_back(q4) (9)(9)(9)(9)(9)(9)(9)(1)(9)(1);
	boost::assign::push_back(q5) (1)(1)(1)(1)(1)(1)(9)(1)(9)(1);
	boost::assign::push_back(q6) (1)(1)(1)(1)(9)(9)(9)(1)(9)(1);
	boost::assign::push_back(q7) (1)(1)(1)(1)(1)(1)(1)(1)(9)(1);
	boost::assign::push_back(q8) (1)(1)(1)(1)(9)(9)(9)(9)(9)(1);
	boost::assign::push_back(q9) (1)(1)(1)(1)(1)(1)(1)(1)(1)(1);

	std::vector< std::vector<int> > fieldMap;
	boost::assign::push_back(fieldMap)
		(q0)(q1)(q2)(q3)(q4)(q5)(q6)(q7)(q8)(q9)
	;

	pf::Map map(fieldMap);
	return map;
}

void main()
{
	pf::Map map = generateMap();

	pf::LPNode nodeStart(new pf::Node(map.getField(0, 3)));
	pf::LPNode nodeStop (new pf::Node(map.getField(9, 3)));

	std::cout << "before" << std::endl;
	pf::PathFinding::print(map, nodeStart, nodeStop, pf::NodeList());

	pf::NodeList path = pf::PathFinding::search(map, nodeStart, nodeStop);

	std::cout << std::endl << "after" << std::endl;
	pf::PathFinding::print(map, nodeStart, nodeStop, path);
	std::cout << std::endl;
}


//   -----------------------------------------------------------------------------------------------
//    Copyright 2015 André Bergner. Distributed under the Boost Software License, Version 1.0.
//     (See accompanying file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
//      --------------------------------------------------------------------------------------------

#include <iostream>
#include <array>

#include <boost/mpl/int.hpp>
#include <boost/mpl/min_max.hpp>
#include <boost/typeof/typeof.hpp>
#include <boost/proto/core.hpp>
#include <boost/proto/context.hpp>
#include <boost/proto/transform.hpp>
#include <boost/proto/debug.hpp>

#include "callable_decltype.hpp"
#include "tuple_tools.hpp"
#include "demangle.h"

namespace mpl = boost::mpl;
namespace proto = boost::proto;

namespace building_blocks
{
   using namespace proto;

   //  ---------------------------------------------------------------------------------------------
   // definition of the building blocks of the language
   //  ---------------------------------------------------------------------------------------------

   template < typename I >  struct placeholder       { using arity = I; };
   template < typename T >  struct placeholder_arity { using type = typename T::arity; };

   template < typename idx = _ >
   using delayed_placeholder = subscript< terminal<placeholder<idx>> , terminal<placeholder<_>> >;

   //  ---------------------------------------------------------------------------------------------
   // block composition operators -- TODO needs correct grammar as arguments

   using channel_operator  = comma              < _ , _ >;
   using parallel_operator = bitwise_or         < _ , _ >;
   using sequence_operator = bitwise_or_assign  < _ , _ >;
}

using building_blocks :: placeholder;
using building_blocks :: placeholder_arity;
using building_blocks :: delayed_placeholder;

using building_blocks :: channel_operator;
using building_blocks :: sequence_operator;
using building_blocks :: parallel_operator;

BOOST_PROTO_DEFINE_ENV_VAR( current_input_t, current_input );
BOOST_PROTO_DEFINE_ENV_VAR( delayed_input_t, delayed_input );

//  ------------------------------------------------------------------------------------------------
// very simple delay_line operation on std::array --> TODO should be own type static_delay_line !

struct
{
    template< typename T , size_t N , typename Y >
    void operator()( std::array<T,N>& xs , Y y ) const
    {
        for ( size_t n = 1 ; n < xs.size() ; ++n )  xs[n-1] = xs[n];
        xs.back() = y;
    }

    template< typename T , typename Y >
    void operator()( std::array<T,0>& xs , Y y ) const { }

    template< typename T , typename Y >   // if input is not an array, e.g. for not fully
    void operator()( T& , Y ) const { }   // unpacked state while traversing the tree (tuple o'tuples)
}
rotate_push_back;

namespace transforms
{
   using namespace proto;

   //  ---------------------------------------------------------------------------------------------
   // static analysis transforms  --  helpers that inspect structure of expression
   //  ---------------------------------------------------------------------------------------------

   struct output_arity;

   struct input_arity : or_
   <  when
      <  delayed_placeholder<>
      ,  placeholder_arity<_value(_left)>()
      >
   ,  when
      <  terminal< placeholder<_> >
      ,  placeholder_arity<_value>()
      >
   ,  when
      <  terminal<_>
      ,  mpl::int_<0>()
      >
   ,  when
      <  parallel_operator
      ,  mpl::plus< input_arity(_left) , input_arity(_right) >()
      >
   ,  when
      <  sequence_operator
      ,  mpl::plus
         <  input_arity(_left)
         ,  mpl::max
            <  mpl::int_<0>
            ,  mpl::minus< input_arity(_right) , output_arity(_left) >
            >
         >()
      >
   ,  when
      <  nary_expr<_, vararg<_>>
      ,  fold<_, mpl::int_<0>(), mpl::max<input_arity, _state>()>
      >
   >
   {};

   struct output_arity : or_
   <  when
      <  channel_operator
      ,  mpl::plus< output_arity(_left) , output_arity(_right) >()
      >
   ,  when
      <  parallel_operator
      ,  mpl::plus< output_arity(_left) , output_arity(_right) >()
      >
   ,  when
      <  sequence_operator
      ,  mpl::plus
         <  output_arity(_right)
         ,  mpl::max
            <  mpl::int_<0>
            ,  mpl::minus< output_arity(_left) , input_arity(_right) >
            >
         >()
      >
   ,  otherwise< mpl::int_<1>() >
   >
   {};

   template < typename arity , typename delay = mpl::int_<0> >
   struct make_arity_impl
   {
      using type = tuple_cat_t< repeat_t< arity::value-1ul, mpl::int_<0> >
                              , std::tuple<delay>
                              >;
   };

   template < typename delay >
   struct make_arity_impl<mpl::int_<0>, delay>
   {
      using type = std::tuple<>;
   };

   template < typename arity , typename delay = mpl::int_<0> >
   using make_arity = typename make_arity_impl<arity,delay>::type;

   struct delay_per_wire : callable_decltype
   {
      using default_t = placeholder<mpl::int_<0>>;

      template < typename Placeholder = default_t , typename Delay = default_t>
      auto operator()( Placeholder const & = default_t{}, Delay const & = default_t{} ) const
      {
         return make_arity<typename Placeholder::arity, typename Delay::arity>{};
      }
   };

   struct tuple_cat_fn : callable_decltype
   {
      template < typename... Tuples >
      auto operator()( Tuples&&... ts ) const
      {
         return std::tuple_cat( std::forward<Tuples>(ts)... );
      }
   };

   struct make_tuple_fn : callable_decltype
   {
      template < typename... Ts >
      auto operator()( Ts&&... ts ) const
      {
         return std::make_tuple( std::forward<Ts>(ts)... );
      }
   };

   // -------------------------------------------------------------------------------------------
   // max-zip tuple
   // -------------------------------------------------------------------------------------------

   template < typename Tuple1, typename Tuple2, std::size_t... Ns >
   auto zip_with_max( Tuple1&& t1, Tuple2&& t2, std::index_sequence<Ns...> )
   {
      using namespace std;
      return tuple< typename mpl::max< decay_t<decltype(get<Ns>(t1))> , decay_t<decltype(get<Ns>(t2))> >::type... >{};
   }

   template < typename Tuple1, typename Tuple2 >
   auto zip_with_max( Tuple1&&, Tuple2&&, std::index_sequence<> )
   {
      return std::tuple<>{};
   }

   struct max_delay_of_wires : callable_decltype
   {
      template < typename Tuple_l , typename Tuple_r >
      auto operator()( Tuple_l&& tl, Tuple_r&& tr ) const
      {
         using tuple_tl = std::decay_t<Tuple_l>;
         using tuple_tr = std::decay_t<Tuple_r>;
         using min_size = typename mpl::min< mpl::int_<std::tuple_size<tuple_tl>::value>
                                           , mpl::int_<std::tuple_size<tuple_tr>::value>
                                           >::type;
         return  std::tuple_cat(  zip_with_max(tl,tr,std::make_index_sequence<min_size::value>{})
                               ,  tuple_drop<min_size::value>( std::forward<Tuple_l>(tl) )
                               ,  tuple_drop<min_size::value>( std::forward<Tuple_r>(tr) )
                               );
      }
   };

   struct tuple_drop_ : callable_decltype
   {
      template< typename Drop , typename Tuple >
      decltype(auto) operator()( Drop , Tuple const & t ) const
      {
         return tuple_drop<Drop::value>(t);
      }
   };

   struct input_delays : or_
   <  when
      <  delayed_placeholder<>
      ,  delay_per_wire( _value(_left),_value(_right))
      >
   ,  when
      <  terminal< placeholder<_> >
      ,  delay_per_wire( _value() )
      >
   ,  when
      <  terminal<_>
      ,  std::tuple<>()//delay_per_wire()
      >
   ,  when
      <  parallel_operator
      ,  tuple_cat_fn( input_delays(_left) , input_delays(_right) )
      >
   ,  when
      <  sequence_operator
      ,  tuple_cat_fn
         (  input_delays(_left)
         ,  tuple_drop_ // <-- problem here????
            (  output_arity(_left)
            ,  input_delays(_right)
            )
         )
      >
   ,  when
      <  nary_expr<_, vararg<_>>
      ,  fold<_, std::tuple<>(), max_delay_of_wires(input_delays, _state) >
      >
   >
   {};

   template < typename Expr >
   using input_arity_t = typename boost::result_of<input_arity(Expr)>::type;

   struct identity : callable_decltype
   {
      template < typename X >
      auto operator()( X&& x ) { return std::forward<X>(x); }
   };

   template < typename StateCtor >
   struct build_state_impl
   {
      struct apply : or_
      <  when
         <  sequence_operator
         ,  make_tuple_fn
            (  apply( _left  , StateCtor(input_delays( _left  )) )
            ,  apply( _right , StateCtor(input_delays( _right )) )
            )
         >
      ,  otherwise< _state >
      >
      {};
   };

   template < typename StateCtor = identity >
   using build_state = typename build_state_impl<StateCtor>::apply;

   //  ---------------------------------------------------------------------------------------------
   // Evaluators -- transforms that work together for the evaluation of flowz-expressions
   //  ---------------------------------------------------------------------------------------------

   struct place_the_holder;
   struct place_delay;
   struct sequence;
   struct parallel;

   struct eval_it : or_
   <  when
      <  delayed_placeholder<>
      ,  place_delay( _value(_left) , _value(_right) , _env_var<delayed_input_t> )
      >
   ,  when
      <  terminal< placeholder<_> >
      ,  place_the_holder( _value , _env_var<current_input_t> )
      >
   ,  when
      <  sequence_operator
      ,  sequence( _left , _right , _env_var<current_input_t> , _env_var<delayed_input_t> )
      >
   ,  when
      <  parallel_operator
      ,  parallel( _left , _right , _env_var<current_input_t> , _env_var<delayed_input_t> )
      >
   ,  when
      <  channel_operator    //  grammar rule: only allowed after sequence or recursion at top of sub-tree
      ,  make_tuple_fn( eval_it(_left), eval_it(_right) )
      >
   ,  when
      <  _
      ,  _default< eval_it >
      >
   >
   {};

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

   template < typename T >
   using in_t = T const &;
   //using in_t = T;

   struct place_the_holder : callable_decltype
   {
      template < typename I , typename Tuple >
      auto operator()( placeholder<I> const & , Tuple const & args ) const
      {
         return std::get<I::value-1>( args );
      }
   };

   struct place_delay : callable_decltype
   {
      template < typename I , typename J , typename Delayed_input >
      auto operator()( placeholder<I> , placeholder<J> , in_t<Delayed_input> del_in ) const
      {
         auto const & s = std::get<I::value-1>(del_in);
         return s[s.size()-J::value];
      }
   };

   struct sequence : callable_decltype
   {
      template < typename LeftExpr , typename RightExpr , typename Input , typename Delay >
      auto operator()( in_t<LeftExpr> l , in_t<RightExpr> r , in_t<Input> input , Delay& delay ) const
      {
         auto e = eval_it{};

         auto& ls = std::get<0>(delay);
         auto& rs = std::get<1>(delay);

         auto left_result =
            flatten_tuple( std::make_tuple(
               e( l, 0, ( current_input = input , delayed_input = boost::ref(ls) ))
            ));

         tuple_for_each( rotate_push_back, ls, input );
         using left_size = std::tuple_size<decltype(left_result)>;
         auto right_input = tuple_cat( left_result, tuple_drop<left_size::value>(input) );

         auto right_result =
            flatten_tuple( std::make_tuple(
               e( r, 0, ( current_input = right_input , delayed_input = boost::ref(rs)) )
            ));

         tuple_for_each( rotate_push_back, rs, left_result );
         using right_size = std::tuple_size<decltype(right_result)>;
         return tuple_cat( right_result, tuple_drop<right_size::value>(std::move(left_result)) );
      }
   };

   struct parallel : callable_decltype
   {
      template < typename LeftExpr , typename RightExpr , typename Input , typename Delay >
      auto operator()( in_t<LeftExpr> l , in_t<RightExpr> r , in_t<Input> input , Delay& del ) const
      {
         auto e = eval_it{};
         auto left_state  = ( current_input = input , delayed_input = del );
         auto right_state = ( current_input = tuple_drop<input_arity_t<LeftExpr>::value>(input)
                            , delayed_input = tuple_drop<input_arity_t<LeftExpr>::value>(del)
                            );
         return std::make_tuple
                (  e( l, 0, left_state )
                ,  e( r, 0, right_state )
                );
      }
   };

}

using  transforms :: input_arity;
using  transforms :: input_delays;
using  transforms :: build_state;
using  transforms :: input_arity_t;
using  transforms :: output_arity;
using  transforms :: eval_it;

//  ------------------------------------------------------------------------------------------------
// supporting state tools
//  ------------------------------------------------------------------------------------------------

//  ------------------------------------------------------------------------------------------------
// lift_into_tuple  --  lifts a meta-function into a tuple,
//                      i.e. applies it on each type in tuple, returns tuple of new types

template< typename F , typename Tuple >
struct lift_into_tuple;

template< typename F , typename... Ts >
struct lift_into_tuple< F , std::tuple<Ts...> >
{
    using type = std::tuple< typename F::template apply_t<Ts>... >;
};

template< typename F , typename Tuple >
using lift_into_tuple_t = typename lift_into_tuple<F,Tuple>::type;

//  ------------------------------------------------------------------------------------------------
// to_array  --  meta-function that maps mpl::int_<N> to array<T,N>

template< typename T >
struct to_array
{
   template< typename Int >
   struct apply
   {
      using type = std::array< T , std::decay_t<Int>::value >;
   };

   template< typename Int >
   using apply_t = typename apply<Int>::type;

};

template < typename T >
struct to_array_tuple
{
   struct apply : proto::callable_decltype
   {
       template< typename Tuple >
       auto operator()( Tuple ) { return lift_into_tuple_t< to_array<T>, Tuple > {}; }
   };
};

//  ------------------------------------------------------------------------------------------------
// compile  --  main function of framework
//              • takes an expression
//              • returns clojure
//  ------------------------------------------------------------------------------------------------

template
<  typename Expression
,  typename State
,  size_t   arity
>
struct stateful_lambda
{
private:

   Expression  expr_;
   State       state_;

   template < typename... Args >
   auto call_impl( mpl::int_<0> , Args const &... args ) -> decltype(auto)
   {
      auto result = eval_it{}( expr_, 0, ( current_input = std::make_tuple(args...) , delayed_input = boost::ref(state_) ) );
      return flatten_tuple( std::make_tuple( result ));
   }

   template < int arg_diff , typename... Args >
   auto call_impl( mpl::int_<arg_diff> , Args const &... args )
   {
      return [ args...
             , expr = *this ]           // TODO should not be a lambda, but a type that is
      ( auto const &... missing_args ) mutable  //      aware of the # of args (enable_if them)
      {
         return expr( args..., missing_args... );
      };
   }

public:

   stateful_lambda( Expression expr ) : expr_( std::move(expr)) {}

   template < typename... Args , typename = std::enable_if_t< sizeof...(Args) <= arity > >
   auto operator()( Args const &... args ) -> decltype(auto)
   {
      return call_impl( mpl::int_< arity - sizeof...(Args) >{} , args... );
   }
};

// TODO template input type determinse state type (float)
auto compile = []( auto expr )        // TODO need to define value_type for state
{
   using expr_t = decltype(expr);
   using arity_t = input_arity_t<expr_t>;

   auto builder = build_state< to_array_tuple<float>::apply >{};
   using state_t = decltype( builder(expr) );

   return stateful_lambda< expr_t, state_t, arity_t::value>{ expr };
};

const proto::terminal< placeholder< mpl::int_<1> >>::type   _1  = {{}};
const proto::terminal< placeholder< mpl::int_<2> >>::type   _2  = {{}};
const proto::terminal< placeholder< mpl::int_<3> >>::type   _3  = {{}};
const proto::terminal< placeholder< mpl::int_<4> >>::type   _4  = {{}};
const proto::terminal< placeholder< mpl::int_<5> >>::type   _5  = {{}};
const proto::terminal< placeholder< mpl::int_<6> >>::type   _6  = {{}};

int main()
{
   auto print_ins_and_outs = []( auto const & expr )
   {
       std::cout << "-------------------------" << std::endl;
       //proto::display_expr( expr );
       std::cout << "#ins:  " << input_arity{} (expr) << std::endl;
       std::cout << "#outs: " << output_arity{}(expr) << std::endl;
       std::cout << std::endl;
   };

   auto e1 = ( _2 + _1 , _2 );
   auto e2 = ( _1*_2 - 100 );
   auto seq_expr =(      e1 | (_1+_2)
                      |= e2 | (_1*_1)
                  );

   print( input_delays{}(seq_expr));

   print_ins_and_outs( seq_expr );
   auto seq = compile( seq_expr );

   std::cout << std::get<1>(seq(1337,3,4)(3)) << std::endl;

   std::cout << "-------------------------" << std::endl;
   build_state<> b;
   build_state< to_array_tuple<float>::apply > d;
   std::cout << b( (_2[_1] |= _1[_2]) |= (_1[_3] |= _3[_4]) ) << std::endl;
   std::cout << b(  _2[_1] |= (_1[_2] |=  _1[_3] |= _3[_4]) ) << std::endl;
   std::cout << b( (_2[_1] |=  _1[_2] |=  _1[_3]) |= _3[_4] ) << std::endl;
   std::cout << b( _1, std::tuple<>{} ) << std::endl;
   std::cout << type_name( d( (_2[_1] |= _1[_2]) |= (_1[_3] |= _3[_4]) )) << std::endl;
   std::cout << "-------------------------" << std::endl;

   auto z = compile( ((_1|_1) |= (_1[_2] - _2[_3])) |= _1[_1] );

   std::cout << std::get<0>(z(1,1)) << std::endl;
   std::cout << std::get<0>(z(1,2)) << std::endl;
   std::cout << std::get<0>(z(1,3)) << std::endl;
   std::cout << std::get<0>(z(1,4)) << std::endl;
   std::cout << std::get<0>(z(1,5)) << std::endl;
   std::cout << std::get<0>(z(1,6)) << std::endl;
   std::cout << std::get<0>(z(3,10)) << std::endl;

   auto wire_around_prev_box = (_1 |= _2);
   print_ins_and_outs( wire_around_prev_box );
   auto wp = compile( wire_around_prev_box );
   std::cout << wp(2,1337) << std::endl;

   auto wire_around_succ_box = ( (_1,_1) |= _1);
   print_ins_and_outs( wire_around_succ_box );
   auto ws = compile( wire_around_succ_box );
   std::cout << ws(1337) << std::endl;
}


#ifndef STAN_MATH_REV_SCAL_FUN_BOOST_ISNORMAL_HPP
#define STAN_MATH_REV_SCAL_FUN_BOOST_ISNORMAL_HPP

#include <stan/math/rev/meta.hpp>
#include <boost/math/special_functions/fpclassify.hpp>
#include <stan/math/rev/core.hpp>

namespace boost {
namespace math {

/**
 * Checks if the given number is normal.
 *
 * Return true if the specified variable
 * has a value that is normal.
 *
 * @param v Variable to test.
 * @return true if variable is normal.
 */
template <>
inline bool isnormal(const stan::math::var& v) {
  return (boost::math::isnormal)(v.val());
}

}  // namespace math
}  // namespace boost
#endif


#include "gtest/gtest.h"
#include <Eigen/Dense>

#include "conex/cone_program.h"
#include "conex/debug_macros.h"
#include "conex/dense_lmi_constraint.h"
#include "conex/linear_constraint.h"
#include "conex/test/test_util.h"

namespace conex {

using Eigen::MatrixXd;

GTEST_TEST(Warmstart, AgreesWithFullSolveIfNoDataIsChanged) {
  SolverConfiguration config;
  config.inv_sqrt_mu_max = 10000000;
  config.final_centering_steps = 0;
  int num_iters = 10;
  int n = 15;
  int m = 13;
  auto constraints2 = GetRandomDenseMatrices(n, m);

  DenseMatrix affine2 = Eigen::MatrixXd::Identity(n, n);
  DenseLMIConstraint LMI{n, constraints2, affine2};

  Program prog(m);
  DenseMatrix y(m, 1);
  prog.AddConstraint(LMI);

  auto b = GetFeasibleObjective(&prog);
  config.max_iterations = num_iters;
  Solve(b, prog, config, y.data());

  DenseMatrix ywarm(m, 1);
  for (int i = 0; i < num_iters; i++) {
    config.initialization_mode = CONEX_INITIALIZATION_MODE_COLDSTART;
    if (i != 0) {
      config.initialization_mode = CONEX_INITIALIZATION_MODE_WARMSTART;
    }
    config.max_iterations = 1;
    config.final_centering_steps = 0;
    Solve(b, prog, config, ywarm.data());
  }
  EXPECT_NEAR((y - ywarm).norm(), 0, 1e-12);
}

GTEST_TEST(Warmstart, TestWorkspaceInitialization) {
  int n = 15;
  int m = 13;
  auto constraints2 = GetRandomDenseMatrices(n, m);
  DenseMatrix affine2 = Eigen::MatrixXd::Identity(n, n);
  DenseLMIConstraint LMI{n, constraints2, affine2};

  DenseMatrix Alinear = DenseMatrix::Random(n, m);
  DenseMatrix Clinear(n, 1);
  Clinear.setConstant(1);
  LinearConstraint linear_constraint{n, &Alinear, &Clinear};

  DenseMatrix y(m, 1);

  Program prog(m);
  prog.AddConstraint(LMI);
  prog.AddConstraint(linear_constraint);

  auto b = GetFeasibleObjective(&prog);
  auto config = SolverConfiguration();
  config.final_centering_steps = 3;
  config.final_centering_tolerance = .01;
  Solve(b, prog, config, y.data());

  Program prog2(m, &prog.memory_);
  prog2.AddConstraint(LMI);
  prog2.AddConstraint(linear_constraint);
  config.initialization_mode = CONEX_INITIALIZATION_MODE_WARMSTART;
  config.max_iterations = 2;
  DenseMatrix ywarm(m, 1);
  Solve(b, prog2, config, ywarm.data());
  EXPECT_NEAR((y - ywarm).norm(), 0, 1e-9);
}

/*
GTEST_TEST(Warmstart, ObjectivePertubation) {
  SolverConfiguration config;
  config.inv_sqrt_mu_max = 1000;
  config.final_centering_steps = 0;
  config.divergence_upper_bound = 10000;
  int num_iters = 20;
  int n = 15;
  int m = 13;
  auto constraints2 = GetRandomDenseMatrices(n, m);

  DenseMatrix affine2 = Eigen::MatrixXd::Identity(n, n);
  DenseLMIConstraint LMI{n, constraints2, affine2};

  Program prog;
  DenseMatrix y(m, 1);
  prog.AddConstraint(LMI);

  auto b = GetFeasibleObjective(m, prog.constraints);
  config.max_iterations = num_iters;
  Solve(b, prog, config, y.data());

  b = b + MatrixXd::Random(m, 1) * .05;
  DenseMatrix ywarm(m, 1);
  config.initialization_mode = 1;
  Solve(b, prog, config, ywarm.data());
}*/

}  // namespace conex